Feat: visualise pose in rviz

CMakeLists.txt

@@ -82,6 +82,9 @@ target_link_libraries(emc_test_subscriber emc_system)
 add_executable(emc_test_publisher examples/test_publisher.cpp)
 target_link_libraries(emc_test_publisher emc_system)
 
+add_executable(pose_tf2_broadcaster src/pose_tf2_broadcaster.cpp)
+target_link_libraries(pose_tf2_broadcaster emc_system)
+
 #add_executable(emc_example2 examples/example02.cpp)
 #target_link_libraries(emc_example2 emc_system)
 
@@ -99,7 +102,7 @@ target_link_libraries(emc_test_publisher emc_system)
 #############
 ## Install ##
 #############
-install(TARGETS emc_example1 emc_example3 emc_test_io emc_test_speech emc_test_subscriber emc_test_publisher
+install(TARGETS emc_example1 emc_example3 emc_test_io emc_test_speech emc_test_subscriber emc_test_publisher pose_tf2_broadcaster
   DESTINATION lib/${PROJECT_NAME}
 )
 

launch/client.launch.py

@@ -2,14 +2,15 @@
 
 import os
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
+from launch.actions import DeclareLaunchArgument, GroupAction
 from launch_ros.actions import Node
 from launch.substitutions import LaunchConfiguration
 from ament_index_python.packages import get_package_share_directory
 
 # Define the launch description
 def generate_launch_description():
     ld = LaunchDescription()
+    package_name = 'emc_system'
     vrpn_mocap = get_package_share_directory('vrpn_mocap')
     server = LaunchConfiguration('server')
     port = LaunchConfiguration('port')
@@ -25,18 +26,30 @@ def generate_launch_description():
         default_value='3883',
     )
 
-    mocap = Node(
-        package='vrpn_mocap',
-        namespace='vrpn_mocap',
-        executable='client_node',
-        name='vrpn_mocap_client_node',
-        output='screen',
-        parameters=[{
-            'server': server,
-            'port': port, 
-        }, config]
-    )
+    mocap = GroupAction([
+        Node(
+            package='vrpn_mocap',
+            namespace='vrpn_mocap',
+            executable='client_node',
+            name='vrpn_mocap_client_node',
+            output='screen',
+            parameters=[{
+                'server': server,
+                'port': port, 
+            }, config]
+        ),
 
+        Node(
+            package=package_name,
+            executable='pose_tf2_broadcaster',
+            name='pose_tf2_broadcaster_node',
+            output='screen',
+            parameters=[{
+                'robotname' : 'ROSbot_Coco' 
+            }]
+        )
+    ])
+
     ld.add_action(declare_server)
     ld.add_action(declare_port)
     ld.add_action(mocap)

src/communication.cpp

@@ -91,12 +91,12 @@ bool Communication::readPoseData(PoseData& pose)
         return false;
 
     pose.x = msg.pose.position.x;
-    pose.y = msg.pose.position.y;
-    pose.z = msg.pose.position.z;
+    pose.y = msg.pose.position.z;
+    pose.z = msg.pose.position.y;
 
     // Calculate roll, pitch and yaw from quaternion
     const geometry_msgs::msg::Quaternion& q = msg.pose.orientation;
-    tf2::Quaternion tfq(q.x, q.y, q.z, q.w);
+    tf2::Quaternion tfq(q.x, q.z, q.y, -q.w);
     tf2::Matrix3x3 m(tfq);
     m.getRPY(pose.roll, pose.pitch, pose.yaw);
 

src/pose_tf2_broadcaster.cpp

@@ -0,0 +1,73 @@
+#include <functional>
+#include <memory>
+#include <sstream>
+#include <string>
+
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "tf2/LinearMath/Quaternion.h"
+#include "tf2_ros/transform_broadcaster.h"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+class FramePublisher : public rclcpp::Node
+{
+public:
+  FramePublisher()
+  : Node("pose_tf2_frame_publisher")
+  {
+    // Declare and acquire `robotname` parameter
+    robotname_ = this->declare_parameter<std::string>("robotname", "Wall-E");
+
+    // Initialize the transform broadcaster
+    tf_broadcaster_ =
+        std::make_unique<tf2_ros::TransformBroadcaster>(*this);
+
+    // Subscribe to a /vrpn_mocap/robotname/pose topic and call handle_robot_pose
+    // callback function on each message
+    std::ostringstream stream;
+    stream << "/vrpn_mocap/" << robotname_.c_str() << "/pose"; 
+    std::string topic_name = stream.str();
+
+    rclcpp::QoS qos = rclcpp::QoS(rclcpp::KeepLast(10)).best_effort();
+    subscription_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
+        topic_name, qos,
+        std::bind(&FramePublisher::handle_robot_pose, this, std::placeholders::_1));
+  }
+
+private:
+  void handle_robot_pose(const std::shared_ptr<geometry_msgs::msg::PoseStamped> msg)
+  {
+    geometry_msgs::msg::TransformStamped t;
+
+    // Read message content and assign it to
+    // corresponding tf variables
+    t.header.stamp = this->get_clock()->now();
+    t.header.frame_id = "map";
+    t.child_frame_id = robotname_.c_str();
+
+    t.transform.translation.x = msg->pose.position.x;
+    t.transform.translation.y = msg->pose.position.z;
+    t.transform.translation.z = msg->pose.position.y;
+
+    tf2::Quaternion q;
+    t.transform.rotation.x = msg->pose.orientation.x;
+    t.transform.rotation.y = msg->pose.orientation.z;
+    t.transform.rotation.z = msg->pose.orientation.y;
+    t.transform.rotation.w = -msg->pose.orientation.w;
+
+    // Send the transformation
+    tf_broadcaster_->sendTransform(t);
+  }
+
+  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr subscription_;
+  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+  std::string robotname_;
+};
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<FramePublisher>());
+  rclcpp::shutdown();
+  return 0;
+}