Add an option to publish TwistStamped (backport #42) (#45)

* Add an option to publish TwistStamped (#42) Signed-off-by: Tamaki Nishino <[email protected]> (cherry picked from commit 76cd650) # Conflicts: # README.md # src/teleop_twist_joy.cpp * resolved merge conflicts (#47) Co-authored-by: Yannic Bachmann <[email protected]> --------- Co-authored-by: Tamaki Nishino <[email protected]> Co-authored-by: Yannic Bachmann <[email protected]> Co-authored-by: Yannic Bachmann <[email protected]> Co-authored-by: Alejandro Hernández Cordero <[email protected]>
ros2 · Jun 14, 2024 · 90833e9 · 90833e9
1 parent e49881c
commit 90833e9
Show file tree

Hide file tree

Showing 6 changed files with 113 additions and 16 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -82,6 +82,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)

diff --git a/README.md b/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
@@ -63,9 +64,15 @@ The package comes with the `teleop_node` that republishes `sensor_msgs/msg/Joy`
   - `scale_angular_turbo.yaw (double, default: 1.0)`
   - `scale_angular_turbo.pitch (double, default: 0.0)`
   - `scale_angular_turbo.roll (double, default: 0.0)`
-
 
-
+- `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
@@ -93,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.
diff --git a/launch/teleop-launch.py b/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'))},
             ),
     ])
diff --git a/src/teleop_twist_joy.cpp b/src/teleop_twist_joy.cpp
@@ -30,6 +30,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
 #include <string>
 
 #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 <rcutils/logging_macros.h>
@@ -51,11 +52,18 @@ namespace teleop_twist_joy
 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 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;
@@ -76,8 +84,19 @@ TeleopTwistJoy::TeleopTwistJoy(const rclcpp::NodeOptions& options) : Node("teleo
 {
   pimpl_ = new Impl;
 
-  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),
+  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));
 
   pimpl_->require_enable_button = this->declare_parameter("require_enable_button", true);
@@ -324,18 +343,31 @@ double getVal(const sensor_msgs::msg::Joy::SharedPtr joy_msg, const std::map<std
 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)
+{
   cmd_vel_msg->linear.x = getVal(joy_msg, axis_linear_map, scale_linear_map[which_map], "x");
   cmd_vel_msg->linear.y = getVal(joy_msg, axis_linear_map, scale_linear_map[which_map], "y");
   cmd_vel_msg->linear.z = getVal(joy_msg, axis_linear_map, scale_linear_map[which_map], "z");
   cmd_vel_msg->angular.z = getVal(joy_msg, axis_angular_map, scale_angular_map[which_map], "yaw");
   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)
@@ -359,8 +391,15 @@ void TeleopTwistJoy::Impl::joyCallback(const sensor_msgs::msg::Joy::SharedPtr jo
     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;
     }
   }

diff --git a/test/publish_stamped_twist_joy_launch_test.py b/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
diff --git a/test/test_joy_twist.py b/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