private void ReceiveMessage(object sender, MessageEventArgs e)
{
message = (GeometryTwist)e.Message;
linearVelocity = getVector3(message.linear).Ros2Unity();
angularVelocity = -getVector3(message.angular).Ros2Unity();
isMessageReceived = true;
}
private void updateVelocity(Message message)
{
GeometryTwist geometryTwist = (GeometryTwist)message;
// In ROS conventions, the coordinate system is right-handed with
// x pointing forward
// y pointing to the left
// z pointing to the top
// In Unity conventions, the coordinate system is left-handed with
// x pointing to the right
// y pointing to the top
// z pointing forward
// Hopefully, the conversion below should handle the transformation between
// the two coordinate systems
// But I'm not completely sure :)
velocityTransformManager.updateTransform(
new Vector3(-geometryTwist.linear.y,
geometryTwist.linear.z,
geometryTwist.linear.x),
new Vector3(geometryTwist.angular.y,
-geometryTwist.angular.z,
-geometryTwist.angular.x));
}
private static Vector3 getAngularVelocity(GeometryTwist _geometryTwist)
{
return(new Vector3(
_geometryTwist.angular.x,
-_geometryTwist.angular.z,
_geometryTwist.angular.y));
}
private static Vector3 getLinearVelocity(GeometryTwist geometryTwist)
{
return(new Vector3(
-geometryTwist.linear.y,
geometryTwist.linear.z,
geometryTwist.linear.x));
}
private void updateOdometry(Message message)
{
GeometryTwist geometryTwist = (GeometryTwist)message;
Debug.Log("Action received");
velocityManager.updateTransform(getLinearVelocity(geometryTwist), getAngularVelocity(geometryTwist));
actionReceived = true;
}
public GeometryTwistWithCovariance()
{
twist = new GeometryTwist();
covariance = new float[36];
}
private void InitializeMessage()
{
message = new GeometryTwist();
message.linear = new GeometryVector3();
message.angular = new GeometryVector3();
}
public GeometryTwistStamped()
{
header = new StandardHeader();
twist = new GeometryTwist();
}