From 48ad65298baed63cfd282f9d1145e6c60a3a4569 Mon Sep 17 00:00:00 2001 From: Tamaki Nishino Date: Wed, 3 Apr 2024 00:14:23 +0900 Subject: [PATCH 1/3] Add publish_stamped_twist option Signed-off-by: Tamaki Nishino --- CMakeLists.txt | 1 + src/teleop_twist_joy.cpp | 54 ++++++++++++++++--- test/publish_stamped_twist_joy_launch_test.py | 40 ++++++++++++++ test/test_joy_twist.py | 9 +++- 4 files changed, 94 insertions(+), 10 deletions(-) create mode 100644 test/publish_stamped_twist_joy_launch_test.py diff --git a/CMakeLists.txt b/CMakeLists.txt index e523ae2..d0e8f1a 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/src/teleop_twist_joy.cpp b/src/teleop_twist_joy.cpp index 49d2c20..a414356 100644 --- a/src/teleop_twist_joy.cpp +++ b/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 +#include #include #include #include @@ -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::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; @@ -80,7 +88,17 @@ TeleopTwistJoy::TeleopTwistJoy(const rclcpp::NodeOptions & options) { pimpl_ = new Impl; - pimpl_->cmd_vel_pub = this->create_publisher("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( + "cmd_vel", 10); + } else { + pimpl_->cmd_vel_pub = this->create_publisher("cmd_vel", 10); + } pimpl_->joy_sub = this->create_subscription( "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(); + 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; +} +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(); - 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 From 2438ac142872618253fa9469c4f82baad2cee09b Mon Sep 17 00:00:00 2001 From: Tamaki Nishino Date: Wed, 3 Apr 2024 19:38:36 +0900 Subject: [PATCH 2/3] Add launch argument Signed-off-by: Tamaki Nishino --- launch/teleop-launch.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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'))}, ), ]) From 7690e8605ae0b68e4e5bfcbc59ba88e28cf077d2 Mon Sep 17 00:00:00 2001 From: Tamaki Nishino Date: Wed, 3 Apr 2024 19:38:44 +0900 Subject: [PATCH 3/3] Update README Signed-off-by: Tamaki Nishino --- README.md | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 1540a27..7147fed 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 @@ -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//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.