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)
);
}