Add an option to publish TwistStamped

CMakeLists.txt

@@ -80,6 +80,7 @@ if(BUILD_TESTING)
     test/turbo_angular_enable_joy_launch_test.py
 
     test/no_require_enable_joy_launch_test.py
+    test/publish_stamped_twist_joy_launch_test.py
   )
 
   find_package(launch_testing_ament_cmake REQUIRED)

src/teleop_twist_joy.cpp

@@ -32,6 +32,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
 #include "rcutils/logging_macros.h"
 
 #include <geometry_msgs/msg/twist.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_components/register_node_macro.hpp>
 #include <sensor_msgs/msg/joy.hpp>
@@ -53,10 +54,17 @@ struct TeleopTwistJoy::Impl
 {
   void joyCallback(const sensor_msgs::msg::Joy::SharedPtr joy);
   void sendCmdVelMsg(const sensor_msgs::msg::Joy::SharedPtr, const std::string & which_map);
+  void fillCmdVelMsg(
+    const sensor_msgs::msg::Joy::SharedPtr, const std::string & which_map,
+    geometry_msgs::msg::Twist * cmd_vel_msg);
 
   rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub;
   rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub;
+  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr cmd_vel_stamped_pub;
+  rclcpp::Clock::SharedPtr clock;
 
+  bool publish_stamped_twist;
+  std::string frame_id;
   bool require_enable_button;
   int64_t enable_button;
   int64_t enable_turbo_button;
@@ -80,7 +88,17 @@ TeleopTwistJoy::TeleopTwistJoy(const rclcpp::NodeOptions & options)
 {
   pimpl_ = new Impl;
 
-  pimpl_->cmd_vel_pub = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 10);
+  pimpl_->clock = this->get_clock();
+
+  pimpl_->publish_stamped_twist = this->declare_parameter("publish_stamped_twist", false);
+  pimpl_->frame_id = this->declare_parameter("frame", "teleop_twist_joy");
+
+  if (pimpl_->publish_stamped_twist) {
+    pimpl_->cmd_vel_stamped_pub = this->create_publisher<geometry_msgs::msg::TwistStamped>(
+      "cmd_vel", 10);
+  } else {
+    pimpl_->cmd_vel_pub = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 10);
+  }
   pimpl_->joy_sub = this->create_subscription<sensor_msgs::msg::Joy>(
     "joy", rclcpp::QoS(10),
     std::bind(&TeleopTwistJoy::Impl::joyCallback, this->pimpl_, std::placeholders::_1));
@@ -273,9 +291,25 @@ void TeleopTwistJoy::Impl::sendCmdVelMsg(
   const sensor_msgs::msg::Joy::SharedPtr joy_msg,
   const std::string & which_map)
 {
-  // Initializes with zeros by default.
-  auto cmd_vel_msg = std::make_unique<geometry_msgs::msg::Twist>();
+  if (publish_stamped_twist) {
+    auto cmd_vel_stamped_msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
+    cmd_vel_stamped_msg->header.stamp = clock->now();
+    cmd_vel_stamped_msg->header.frame_id = frame_id;
+    fillCmdVelMsg(joy_msg, which_map, &cmd_vel_stamped_msg->twist);
+    cmd_vel_stamped_pub->publish(std::move(cmd_vel_stamped_msg));
+  } else {
+    auto cmd_vel_msg = std::make_unique<geometry_msgs::msg::Twist>();
+    fillCmdVelMsg(joy_msg, which_map, cmd_vel_msg.get());
+    cmd_vel_pub->publish(std::move(cmd_vel_msg));
+  }
+  sent_disable_msg = false;
+}
 
+void TeleopTwistJoy::Impl::fillCmdVelMsg(
+  const sensor_msgs::msg::Joy::SharedPtr joy_msg,
+  const std::string & which_map,
+  geometry_msgs::msg::Twist * cmd_vel_msg)
+{
   double lin_x = getVal(joy_msg, axis_linear_map, scale_linear_map[which_map], "x");
   double ang_z = getVal(joy_msg, axis_angular_map, scale_angular_map[which_map], "yaw");
 
@@ -285,9 +319,6 @@ void TeleopTwistJoy::Impl::sendCmdVelMsg(
   cmd_vel_msg->angular.z = (lin_x < 0.0 && inverted_reverse) ? -ang_z : ang_z;
   cmd_vel_msg->angular.y = getVal(joy_msg, axis_angular_map, scale_angular_map[which_map], "pitch");
   cmd_vel_msg->angular.x = getVal(joy_msg, axis_angular_map, scale_angular_map[which_map], "roll");
-
-  cmd_vel_pub->publish(std::move(cmd_vel_msg));
-  sent_disable_msg = false;
 }
 
 void TeleopTwistJoy::Impl::joyCallback(const sensor_msgs::msg::Joy::SharedPtr joy_msg)
@@ -307,8 +338,15 @@ void TeleopTwistJoy::Impl::joyCallback(const sensor_msgs::msg::Joy::SharedPtr jo
     // in order to stop the robot.
     if (!sent_disable_msg) {
       // Initializes with zeros by default.
-      auto cmd_vel_msg = std::make_unique<geometry_msgs::msg::Twist>();
-      cmd_vel_pub->publish(std::move(cmd_vel_msg));
+      if (publish_stamped_twist) {
+        auto cmd_vel_stamped_msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
+        cmd_vel_stamped_msg->header.stamp = clock->now();
+        cmd_vel_stamped_msg->header.frame_id = frame_id;
+        cmd_vel_stamped_pub->publish(std::move(cmd_vel_stamped_msg));
+      } else {
+        auto cmd_vel_msg = std::make_unique<geometry_msgs::msg::Twist>();
+        cmd_vel_pub->publish(std::move(cmd_vel_msg));
+      }
       sent_disable_msg = true;
     }
   }

test/publish_stamped_twist_joy_launch_test.py

@@ -0,0 +1,40 @@
+import geometry_msgs.msg
+import launch
+import launch_ros.actions
+import launch_testing
+
+import pytest
+
+import test_joy_twist
+
+
+@pytest.mark.rostest
+def generate_test_description():
+    teleop_node = launch_ros.actions.Node(
+        package='teleop_twist_joy',
+        executable='teleop_node',
+        parameters=[{
+            'publish_stamped_twist': True,
+            'axis_linear.x': 1,
+            'axis_angular.yaw': 0,
+            'scale_linear.x': 2.0,
+            'scale_angular.yaw': 3.0,
+            'enable_button': 0,
+        }],
+    )
+
+    return launch.LaunchDescription([
+            teleop_node,
+            launch_testing.actions.ReadyToTest(),
+        ]), locals()
+
+
+class PublishTwistStamped(test_joy_twist.TestJoyTwist):
+
+    def setUp(self):
+        self.cmd_vel_msg_type = geometry_msgs.msg.TwistStamped
+        super().setUp()
+        self.joy_msg['axes'] = [0.3, 0.4]
+        self.joy_msg['buttons'] = [1]
+        self.expect_cmd_vel['linear']['x'] = 0.8
+        self.expect_cmd_vel['angular']['z'] = 0.9

test/test_joy_twist.py

@@ -41,7 +41,9 @@ def setUp(self):
                                       automatically_declare_parameters_from_overrides=True)
         self.message_pump = launch_testing_ros.MessagePump(self.node, context=self.context)
         self.pub = self.node.create_publisher(sensor_msgs.msg.Joy, 'joy', 1)
-        self.sub = self.node.create_subscription(geometry_msgs.msg.Twist,
+        if not hasattr(self, "cmd_vel_msg_type"):
+            self.cmd_vel_msg_type = geometry_msgs.msg.Twist
+        self.sub = self.node.create_subscription(self.cmd_vel_msg_type,
                                                  'cmd_vel', self.callback, 1)
         self.message_pump.start()
 
@@ -91,4 +93,7 @@ def test_expected(self):
 
     def callback(self, msg):
         self.received_cmd_vel = geometry_msgs.msg.Twist()
-        self.received_cmd_vel = msg
+        if self.cmd_vel_msg_type is geometry_msgs.msg.Twist:
+            self.received_cmd_vel = msg
+        else:
+            self.received_cmd_vel = msg.twist