Read and Apply Transformation to ROS 2 Message in Simulink

This example shows how to read transformations from ROS 2 network and use them to transform a pose message using Simulink®.

Publish Transformation and Pose Messages on ROS 2 Network

Create a ROS 2 node.

node = ros2node("/node1");

Use the exampleHelperROS2StartTfPublisher function to start publishing transformation data. This function publishes transformations between these frames:

• robot_base

• camera_center

• mounting_point

exampleHelperROS2StartTfPublisher

Create a ROS 2 publisher to publish pose messages.

[posePub,poseMsg] = ros2publisher(node,"/pose","geometry_msgs/PoseStamped",Durability="transientlocal",Depth=2);

Create and publish the pose message.

poseMsg.pose.position.x = 1;
poseMsg.pose.position.y = 1;
poseMsg.pose.position.z = 0.5;
send(posePub,poseMsg)

Read and Apply Transformation

Open the readAndApplyROS2Transform model. The Get Transform block reads the transformation between the target robot_base and the source camera_center frames as a geometry_msgs/TransformStamped bus. The Header Assignment block assigns the frame ID camera_center to the pose message. Then, the Apply Transform block transforms the pose message using the read transformation value. The model then writes the transformed pose message into MATLAB® workspace.

modelName = "readAndApplyROS2Transform";
open_system(modelName)

The readAndApplyROS2Transform model uses Simulation Pacing to run at a pace where the simulation time is same as the wall clock time. This is important to ensure that Simulink synchronizes correctly with the transforms being published to the transformation tree in the ROS 2 network.

Simulate the model.

out = sim(modelName);

Visualize Input and Transformed Poses

Get transformed pose as geometry_msgs/Pose message from the simulation output variable in the workspace.

transformedPoseMsg = helperGetTransformedROS2PoseFromSimout(out);

You can visualize poses as local coordinate frames in the global cartesian coordinates. You can calculate the location and basis axes orientation of the local frames by using the position and orientation information in the poses, respectively. For both input and transformed poses, use the helperGetTransformedAxesVectorsFromROS2Quat function to get the basis vectors of the local coordinate frame.

[inputPoseX,inputPoseY,inputPoseZ] = helperGetTransformedAxesVectorsFromROS2Quat(poseMsg.pose.orientation);
[transformedPoseX,transformedPoseY,transformedPoseZ] = helperGetTransformedAxesVectorsFromROS2Quat(transformedPoseMsg.pose.orientation);

Visualize the poses as local coordinate frames based on the position and orientation values. The quiver3 function visualizes the basis vectors at the specified pose location.

quiver3(poseMsg.pose.position.x,poseMsg.pose.position.y,poseMsg.pose.position.z,inputPoseX(1),inputPoseX(2),inputPoseX(3),'r',LineWidth=2)
hold on
quiver3(poseMsg.pose.position.x,poseMsg.pose.position.y,poseMsg.pose.position.z,inputPoseY(1),inputPoseY(2),inputPoseY(3),'b',LineWidth=2)
quiver3(poseMsg.pose.position.x,poseMsg.pose.position.y,poseMsg.pose.position.z,inputPoseZ(1),inputPoseZ(2),inputPoseZ(3),'g',LineWidth=2)
quiver3(transformedPoseMsg.pose.position.x,transformedPoseMsg.pose.position.y,transformedPoseMsg.pose.position.z,transformedPoseX(1),transformedPoseX(2),transformedPoseX(3),'c',LineWidth=2)
quiver3(transformedPoseMsg.pose.position.x,transformedPoseMsg.pose.position.y,transformedPoseMsg.pose.position.z,transformedPoseY(1),transformedPoseY(2),transformedPoseY(3),'m',LineWidth=2)
quiver3(transformedPoseMsg.pose.position.x,transformedPoseMsg.pose.position.y,transformedPoseMsg.pose.position.z,transformedPoseZ(1),transformedPoseZ(2),transformedPoseZ(3),'k',LineWidth=2)
legend("inputX","inputY","inputZ","transformedX","transformedY","transformedZ")
xlabel('X')
ylabel('Y')
zlabel('Z')
view([-24.39 11.31])