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