private void SetFurnitureToHumanNaviTaskInfo() { // Get furniture List <GameObject> furnitureObjects = GameObject.FindGameObjectsWithTag(TagNameOfFurniture).ToList <GameObject>(); if (furnitureObjects.Count == 0) { throw new Exception("Furniture is not found."); } foreach (GameObject furnitureObject in furnitureObjects) { // transtrate the coordinate system of GameObject (left-handed, Z-axis:front, Y-axis:up) to ROS coodinate system (right-handed, X-axis:front, Z-axis:up) Vector3 positionInROS = this.ConvertCoordinateSystemUnityToROS_Position(furnitureObject.transform.position); Quaternion orientationInROS = this.ConvertCoordinateSystemUnityToROS_Rotation(furnitureObject.transform.rotation); SIGVerse.RosBridge.human_navigation.HumanNaviObjectInfo objInfo = new SIGVerse.RosBridge.human_navigation.HumanNaviObjectInfo { name = furnitureObject.name.Substring(0, furnitureObject.name.Length - 3), position = positionInROS, orientation = orientationInROS }; taskInfoForRobot.furniture.Add(objInfo); // SIGVerseLogger.Info("Furniture : " + objInfo.name + " " + objInfo.position + " " + objInfo.orientation); } }
private void SetObjectListToHumanNaviTaskInfo() { // Get graspable objects this.graspableObjects = GameObject.FindGameObjectsWithTag(TagNameOfGraspables).ToList <GameObject>(); if (this.graspableObjects.Count == 0) { throw new Exception("Graspable object is not found."); } foreach (GameObject graspableObject in this.graspableObjects) { // transtrate the coordinate system of GameObject (left-handed, Z-axis:front, Y-axis:up) to ROS coodinate system (right-handed, X-axis:front, Z-axis:up) Vector3 positionInROS = this.ConvertCoordinateSystemUnityToROS_Position(graspableObject.transform.position); Quaternion orientationInROS = this.ConvertCoordinateSystemUnityToROS_Rotation(graspableObject.transform.rotation); if (graspableObject.name == currentTaskInfo.target) { taskInfoForRobot.target_object.name = graspableObject.name.Substring(0, graspableObject.name.Length - 3); taskInfoForRobot.target_object.position = positionInROS; taskInfoForRobot.target_object.orientation = orientationInROS; // for penalty this.initialTargetObjectPosition = graspableObject.transform.position; } else { SIGVerse.RosBridge.human_navigation.HumanNaviObjectInfo objInfo = new SIGVerse.RosBridge.human_navigation.HumanNaviObjectInfo { name = graspableObject.name.Substring(0, graspableObject.name.Length - 3), position = positionInROS, orientation = orientationInROS }; taskInfoForRobot.non_target_objects.Add(objInfo); // SIGVerseLogger.Info("Non-target object : " + objInfo.name + " " + objInfo.position + " " + objInfo.orientation); } } SIGVerseLogger.Info("Target object : " + taskInfoForRobot.target_object.name + " " + taskInfoForRobot.target_object.position + " " + taskInfoForRobot.target_object.orientation); if (taskInfoForRobot.target_object.name == "") { throw new Exception("Target object is not found."); } }
private void SendRosObjectStatusMessage() { RosBridge.human_navigation.HumanNaviObjectStatus objectStatus = new RosBridge.human_navigation.HumanNaviObjectStatus(); foreach (GameObject graspableObject in this.graspableObjects) { Vector3 positionInROS = this.ConvertCoordinateSystemUnityToROS_Position(graspableObject.transform.position); Quaternion orientationInROS = this.ConvertCoordinateSystemUnityToROS_Rotation(graspableObject.transform.rotation); if (graspableObject.name == currentTaskInfo.target) { objectStatus.target_object.name = graspableObject.name.Substring(0, graspableObject.name.Length - 3); objectStatus.target_object.position = positionInROS; objectStatus.target_object.orientation = orientationInROS; } else { SIGVerse.RosBridge.human_navigation.HumanNaviObjectInfo objInfo = new SIGVerse.RosBridge.human_navigation.HumanNaviObjectInfo { name = graspableObject.name.Substring(0, graspableObject.name.Length - 3), position = positionInROS, orientation = orientationInROS }; objectStatus.non_target_objects.Add(objInfo); } } ExecuteEvents.Execute <IRosObjectStatusSendHandler> ( target: this.gameObject, eventData: null, functor: (reciever, eventData) => reciever.OnSendRosObjectStatusMessage(objectStatus) ); ExecuteEvents.Execute <IRosObjectStatusSendHandler> ( target: this.playbackManager, eventData: null, functor: (reciever, eventData) => reciever.OnSendRosObjectStatusMessage(objectStatus) ); }