public static void SetFromRotation(this IPose pose, Quaternion quat) { pose.i = quat.x; pose.j = quat.y; pose.k = quat.z; pose.w = quat.w; }
public Pose(IPose poseToCopy) { if (poseToCopy != null) { Location = poseToCopy.Location; Rotation = poseToCopy.Rotation; } }
public static RobotUtilities.Poses.PositionOnly2DPose To2DPositionOnly(this IPose pose) { return(new RobotUtilities.Poses.PositionOnly2DPose() { X = pose.X, Y = pose.Y, Z = pose.Z }); }
/// <summary> /// Invoked from the Waypoint controller to command the robot to this position /// </summary> /// <param name="pos"></param> private void CommandRobotToPose(IPose pos) { if (rosSocket == null) { Debug.Log("Did not publish goal since the rosConnector is not connected"); return; } StartCoroutine(CommandToPose(pos)); }
public static void Update(this IPose pose, Odometry odom) { pose.X = odom.pose.pose.position.x; pose.Y = odom.pose.pose.position.y; pose.Z = odom.pose.pose.position.z; pose.i = odom.pose.pose.orientation.x; pose.j = odom.pose.pose.orientation.y; pose.k = odom.pose.pose.orientation.z; pose.w = odom.pose.pose.orientation.w; }
public static RobotUtilities.IPose Translate <T>(this IPose pose, Vector3 offset) where T : IPose, new() { T t = new T(); t.X = pose.X + offset.x; t.Y = pose.Y + offset.y; t.Z = pose.Z + offset.z; t.i = pose.i; t.j = pose.j; t.k = pose.k; t.w = pose.w; return(t); }
public virtual void CommandTrajectory(IPose targetPose) { MultiDOFJointTrajectory msg = new MultiDOFJointTrajectory(); msg.joint_names = new[] { "base_link" }; msg.points = new[] { new MultiDOFJointTrajectoryPoint() { transforms = new[] { targetPose.ToTransform() } } }; CommandTrajectory(msg); }
public static Transform ToTransform(this IPose pose) { Transform t = new Transform(); t.translation = new Vector3() { x = pose.X, y = pose.Y, z = pose.Z, }; t.rotation = new Quaternion() { x = pose.i, y = pose.j, z = pose.k, w = pose.w }; return(t); }
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); }
/// <summary> /// Returns true if the pose is about equal to the specified pose with different tolerances for translations and rotations /// </summary> /// <param name="pose"></param> /// <returns></returns> public virtual bool IsAboutEqual(IPose pose, float translationTolerance, float rotationTolerance) { var myValues = values; var otherValues = pose.values; for (int i = 0; i < 3; i++) { if (Math.Abs(myValues[i] - otherValues[i]) > translationTolerance) { return(false); } } for (int i = 3; i < 7; i++) { if (Math.Abs(myValues[i] - otherValues[i]) > rotationTolerance) { return(false); } } return(true); }
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(); }); }
public static void SetFromPosition(this IPose pose, Vector3 pos) { pose.X = pos.x; pose.Y = pos.y; pose.Z = pos.z; }
public static RobotUtilities.Pose Translate(this IPose pose, Vector3 offset) { return(new RobotUtilities.Pose(pose.X + offset.x, pose.Y + offset.y, pose.Z + offset.z, pose.i, pose.j, pose.k, pose.w)); }
/// <summary> /// Returns true if the pose is about equal to the specified pose with a specified tolerance in each dimension /// </summary> /// <param name="pose"></param> /// <returns></returns> public virtual bool IsAboutEqual(IPose pose, float tolerance) { return(IsAboutEqual(pose, tolerance, tolerance)); }
/// <summary> /// Returns true if the pose is about equal to the specified pose with a tolerance of 0.1 in each dimension /// </summary> /// <param name="pose"></param> /// <returns></returns> public virtual bool IsAboutEqual(IPose pose) { return(IsAboutEqual(pose, 0.1f)); }