Add an option to publish TwistStamped (#42)

Signed-off-by: Tamaki Nishino <[email protected]>

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)

README.md

@@ -9,13 +9,14 @@ This node provides no rate limiting or autorepeat functionality. It is expected
 
 ## Executables
 The package comes with the `teleop_node` that republishes `sensor_msgs/msg/Joy` messages as scaled `geometry_msgs/msg/Twist` messages.
+The message type can be changed to `geometry_msgs/msg/TwistStamped` by the `publish_stamped_twist` parameter.
 
 ## Subscribed Topics
 - `joy (sensor_msgs/msg/Joy)`
   - Joystick messages to be translated to velocity commands.
 
 ## Published Topics
-- `cmd_vel (geometry_msgs/msg/Twist)`
+- `cmd_vel (geometry_msgs/msg/Twist or geometry_msgs/msg/TwistStamped)`
   - Command velocity messages arising from Joystick commands.
 
 ## Parameters
@@ -67,6 +68,11 @@ The package comes with the `teleop_node` that republishes `sensor_msgs/msg/Joy`
 - `inverted_reverse (bool, default: false)`
   - Whether to invert turning left-right while reversing (useful for differential wheeled robots).
 
+- `publish_stamped_twist (bool, default: false)`
+  - Whether to publish `geometry_msgs/msg/TwistStamped` for command velocity messages.
+
+- `frame (string, default: 'teleop_twist_joy')`
+  - Frame name used for the header of TwistStamped messages.
 
 
 # Usage
@@ -94,3 +100,5 @@ __Note:__ this launch file also launches the `joy` node so do not run it separat
   - Joystick device to use
 - `config_filepath (string, default: '/opt/ros/<rosdistro>/share/teleop_twist_joy/config/' + LaunchConfig('joy_config') + '.config.yaml')`
   - Path to config files
+- `publish_stamped_twist (bool, default: false)`
+  - Whether to publish `geometry_msgs/msg/TwistStamped` for command velocity messages.

launch/teleop-launch.py

@@ -9,12 +9,14 @@
 def generate_launch_description():
     joy_config = launch.substitutions.LaunchConfiguration('joy_config')
     joy_dev = launch.substitutions.LaunchConfiguration('joy_dev')
+    publish_stamped_twist = launch.substitutions.LaunchConfiguration('publish_stamped_twist')
     config_filepath = launch.substitutions.LaunchConfiguration('config_filepath')
 
     return launch.LaunchDescription([
         launch.actions.DeclareLaunchArgument('joy_vel', default_value='cmd_vel'),
         launch.actions.DeclareLaunchArgument('joy_config', default_value='ps3'),
         launch.actions.DeclareLaunchArgument('joy_dev', default_value='/dev/input/js0'),
+        launch.actions.DeclareLaunchArgument('publish_stamped_twist', default_value='false'),
         launch.actions.DeclareLaunchArgument('config_filepath', default_value=[
             launch.substitutions.TextSubstitution(text=os.path.join(
                 get_package_share_directory('teleop_twist_joy'), 'config', '')),
@@ -29,7 +31,8 @@ def generate_launch_description():
             }]),
         launch_ros.actions.Node(
             package='teleop_twist_joy', executable='teleop_node',
-            name='teleop_twist_joy_node', parameters=[config_filepath],
+            name='teleop_twist_joy_node',
+            parameters=[config_filepath, {'publish_stamped_twist': publish_stamped_twist}],
             remappings={('/cmd_vel', launch.substitutions.LaunchConfiguration('joy_vel'))},
             ),
     ])

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