From 324ceb731d194aae9d94cf00d8a4eed944c08a68 Mon Sep 17 00:00:00 2001 From: Tamaki Nishino Date: Wed, 3 Apr 2024 20:11:25 +0900 Subject: [PATCH] Add an option to publish TwistStamped (#42) Signed-off-by: Tamaki Nishino (cherry picked from commit 76cd6508a8c4e35d9fe3a6a8968abbe7159ffc08) # Conflicts: # README.md # src/teleop_twist_joy.cpp --- CMakeLists.txt | 1 + README.md | 16 ++++- launch/teleop-launch.py | 5 +- src/teleop_twist_joy.cpp | 69 +++++++++++++++++-- test/publish_stamped_twist_joy_launch_test.py | 40 +++++++++++ test/test_joy_twist.py | 9 ++- 6 files changed, 129 insertions(+), 11 deletions(-) create mode 100644 test/publish_stamped_twist_joy_launch_test.py diff --git a/CMakeLists.txt b/CMakeLists.txt index fd30e68..81dc700 100644 --- 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 index 8218482..da9afa6 100644 --- 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 @@ -65,7 +66,18 @@ The package comes with the `teleop_node` that republishes `sensor_msgs/msg/Joy` - `scale_angular_turbo.roll (double, default: 0.0)` +<<<<<<< HEAD +======= +- `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. +>>>>>>> 76cd650 (Add an option to publish TwistStamped (#42)) # Usage @@ -93,3 +105,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//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 index 4ab5a4a..293d42d 100755 --- 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 index 6b731fd..f72989b 100644 --- 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 #include +#include #include #include #include @@ -51,11 +52,22 @@ namespace teleop_twist_joy struct TeleopTwistJoy::Impl { void joyCallback(const sensor_msgs::msg::Joy::SharedPtr joy); +<<<<<<< HEAD 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); +>>>>>>> 76cd650 (Add an option to publish TwistStamped (#42)) rclcpp::Subscription::SharedPtr joy_sub; rclcpp::Publisher::SharedPtr cmd_vel_pub; + rclcpp::Publisher::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 +88,24 @@ TeleopTwistJoy::TeleopTwistJoy(const rclcpp::NodeOptions& options) : Node("teleo { pimpl_ = new Impl; +<<<<<<< HEAD pimpl_->cmd_vel_pub = this->create_publisher("cmd_vel", 10); pimpl_->joy_sub = this->create_subscription("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( + "cmd_vel", 10); + } else { + pimpl_->cmd_vel_pub = this->create_publisher("cmd_vel", 10); + } + pimpl_->joy_sub = this->create_subscription( + "joy", rclcpp::QoS(10), +>>>>>>> 76cd650 (Add an option to publish TwistStamped (#42)) std::bind(&TeleopTwistJoy::Impl::joyCallback, this->pimpl_, std::placeholders::_1)); pimpl_->require_enable_button = this->declare_parameter("require_enable_button", true); @@ -324,18 +352,38 @@ double getVal(const sensor_msgs::msg::Joy::SharedPtr joy_msg, const std::map(); + if (publish_stamped_twist) { + auto cmd_vel_stamped_msg = std::make_unique(); + 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(); + fillCmdVelMsg(joy_msg, which_map, cmd_vel_msg.get()); + cmd_vel_pub->publish(std::move(cmd_vel_msg)); + } + sent_disable_msg = false; +} +<<<<<<< HEAD cmd_vel_msg->linear.x = getVal(joy_msg, axis_linear_map, scale_linear_map[which_map], "x"); +======= +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"); + + cmd_vel_msg->linear.x = lin_x; +>>>>>>> 76cd650 (Add an option to publish TwistStamped (#42)) 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 +407,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(); - cmd_vel_pub->publish(std::move(cmd_vel_msg)); + if (publish_stamped_twist) { + auto cmd_vel_stamped_msg = std::make_unique(); + 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(); + 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 new file mode 100644 index 0000000..0a44ba0 --- /dev/null +++ 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 index ca9b48c..f16b8bf 100644 --- 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