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