public static RosSharp.RosBridgeClient.MessageTypes.Geometry.Pose ToRosPose(this IPose pose)
{
(var pos, var rot) = (pose.ToPositionVector(), pose.ToRotationQuaternion());
pos = RosSharp.TransformExtensions.Unity2Ros(pos);
rot = RosSharp.TransformExtensions.Unity2Ros(rot);
RosSharp.RosBridgeClient.MessageTypes.Geometry.Pose p = new RosSharp.RosBridgeClient.MessageTypes.Geometry.Pose();
p.position = new RosSharp.RosBridgeClient.MessageTypes.Geometry.Point()
{
x = pos.x,
y = pos.y,
z = pos.z
};
p.orientation = new RosSharp.RosBridgeClient.MessageTypes.Geometry.Quaternion()
{
x = rot.x,
y = rot.y,
z = rot.z,
w = rot.w
};
return(p);
}
IEnumerator CommandToPose(IPose pos)
{
targetPose = pos;
Debug.Log($"{pos.X} | {pos.Y} | {pos.Z}, Sleeping for 2 secs");
yield return(new WaitForSeconds(2f));
if (currentAnchorController == null)
{
Debug.Log("Current anchor controller = null");
}
ExecuteOnUpdate(() =>
{
rosSocket.Publish(anchored_goal_publishing_id, new AsaRelPoseStamped()
{
pose = pos.ToRosPose(),
anchor_id = currentAnchorController.GetAnchor().Id,
header = new RosSharp.RosBridgeClient.MessageTypes.Std.Header()
});
RobotTargetIndicator.transform.localPosition = currentAnchorController.transform.position + pos.ToPositionVector();
});
}
