Add support for Ackermann /drive topics

CMakeLists.txt

@@ -16,6 +16,7 @@ find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(ackermann_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(stage REQUIRED)
@@ -32,7 +33,7 @@ add_library(stage_node SHARED
             src/stage_node.cpp src/vehicle.cpp src/ranger.cpp src/camera.cpp)
 target_compile_definitions(stage_node PRIVATE "STAGE_ROS2_PACKAGE_DLL")
 ament_target_dependencies(stage_node rclcpp rclcpp_components
-sensor_msgs geometry_msgs nav_msgs std_msgs tf2_ros std_srvs tf2_geometry_msgs)
+sensor_msgs geometry_msgs nav_msgs std_msgs tf2_ros std_srvs tf2_geometry_msgs ackermann_msgs)
 
 
 # This package installs libraries without exporting them.

README.md

@@ -12,6 +12,13 @@ This stage ROS2 bridge can be configured to use stamped velocities on the topic
 ros2 launch stage_ros2 demo.launch.py world:=cave use_stamped_velocity:=false
 ```
 
+## Ackermann Support (AckermannDrive or AckermannDriveStamped)
+For using AckermannDrive /drive Messages instead of Twist /cmd_vel messages, use the following parameter:
+```
+ros2 launch stage_ros2 demo.launch.py world:=cave use_ackermann:=true
+```
+
+
 ## Single Robot Support
 This stage ROS bridge can be configured to use a namespace even for one robot.
 ```

include/stage_ros2/stage_node.hpp

@@ -14,6 +14,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <std_srvs/srv/empty.hpp>
 #include <geometry_msgs/msg/twist_stamped.hpp>
+#include <ackermann_msgs/msg/ackermann_drive_stamped.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/image_encodings.hpp>
@@ -116,6 +117,7 @@ class StageNode : public rclcpp::Node
     std::string topic_name_space_;
     std::string frame_name_space_;
     std::string topic_name_cmd_;
+    std::string topic_name_drive_;
 
     std::string topic_name_tf_;
     std::string topic_name_tf_static_;
@@ -137,6 +139,8 @@ class StageNode : public rclcpp::Node
     void init(bool use_topic_prefixes, bool use_one_tf_tree);
     void callback_cmd(const geometry_msgs::msg::Twist::SharedPtr msg);
     void callback_cmd_stamped(const geometry_msgs::msg::TwistStamped::SharedPtr msg);
+    void callback_drive(const ackermann_msgs::msg::AckermannDrive::SharedPtr msg);
+    void callback_drive_stamped(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr msg);
     void publish_msg();
     void publish_tf();
     void check_watchdog_timeout();
@@ -154,6 +158,8 @@ class StageNode : public rclcpp::Node
     rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_ground_truth_;             // one ground truth
     rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr sub_cmd_;                 // one cmd_vel subscriber
     rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr sub_cmd_stamped_;  // one sub_cmd_stamped_ subscriber
+    rclcpp::Subscription<ackermann_msgs::msg::AckermannDrive>::SharedPtr sub_drive_;     // one drive subscriber
+    rclcpp::Subscription<ackermann_msgs::msg::AckermannDriveStamped>::SharedPtr sub_drive_stamped_;  // one drive_stamped_ subscriber
 
     std::shared_ptr<stage_ros2::StaticTransformBroadcaster> tf_static_broadcaster_;
     std::shared_ptr<stage_ros2::TransformBroadcaster> tf_broadcaster_;
@@ -168,6 +174,7 @@ class StageNode : public rclcpp::Node
   bool one_tf_tree_;                       /// ROS parameter
   bool enable_gui_;                        /// ROS parameter
   bool use_stamped_velocity_;              /// ROS parameter
+  bool use_ackermann_;                     /// ROS parameter
   bool publish_ground_truth_;              /// ROS parameter
   bool use_static_transformations_;        /// ROS parameter
   std::string world_file_;                 /// ROS parameter

launch/demo.launch.py

@@ -28,6 +28,12 @@ def generate_launch_description():
         default_value='false',
         description='on true stage will accept TwistStamped command messages')
 
+    use_ackermann = LaunchConfiguration('use_ackermann')
+    use_ackermann_arg = DeclareLaunchArgument(
+        'use_ackermann',
+        default_value='false',
+        description='on true stage will accept AckermannDrive or AckermannDriveStamped command messages')
+
     one_tf_tree = LaunchConfiguration('one_tf_tree')
     one_tf_tree_cmd = DeclareLaunchArgument(
         'one_tf_tree',
@@ -60,6 +66,7 @@ def generate_launch_description():
     return LaunchDescription([
         declare_namespace_cmd,
         use_stamped_velocity_cmd,
+        use_ackermann_arg,
         declare_rviz_cmd,
         declare_stage_cmd,
         enforce_prefixes_cmd,
@@ -77,5 +84,6 @@ def generate_launch_description():
             launch_arguments={'one_tf_tree':one_tf_tree,
                               'enforce_prefixes':enforce_prefixes,
                               'use_stamped_velocity': use_stamped_velocity,
+                              'use_ackermann': use_ackermann,
                               'world': world}.items()),
     ])

launch/stage.launch.py

@@ -18,13 +18,18 @@ def generate_launch_description():
         'use_stamped_velocity',
         default_value='false',
         description='on true stage will accept TwistStamped command messages')
+
+    use_ackermann = LaunchConfiguration('use_ackermann')
+    use_ackermann_arg = DeclareLaunchArgument(
+        'use_ackermann',
+        default_value='false',
+        description='on true stage will accept AckermannDrive or AckermannDriveStamped command messages')
 
     stage_world_arg = DeclareLaunchArgument(
         'world',
         default_value=TextSubstitution(text='cave'),
         description='World file relative to the project world file, without .world')
 
-
     enforce_prefixes = LaunchConfiguration('enforce_prefixes')
     enforce_prefixes_arg = DeclareLaunchArgument(
         'enforce_prefixes',
@@ -54,6 +59,7 @@ def stage_world_configuration(context):
 
     return LaunchDescription([
         use_stamped_velocity_arg,
+        use_ackermann_arg,
         stage_world_arg,
         one_tf_tree_arg, 
         enforce_prefixes_arg, 
@@ -63,10 +69,11 @@ def stage_world_configuration(context):
             package='stage_ros2',
             executable='stage_ros2',
             name='stage',
-            parameters=[{'one_tf_tree': one_tf_tree,
-                        'enforce_prefixes': enforce_prefixes,
-                        'use_stamped_velocity': use_stamped_velocity,
-                        'use_static_transformations': use_static_transformations,
-                "world_file": [LaunchConfiguration('world_file')]}],
+            parameters=[{'use_stamped_velocity': use_stamped_velocity,
+                         'use_ackermann': use_ackermann,
+                         'enforce_prefixes': enforce_prefixes,
+                         'use_static_transformations': use_static_transformations,
+                         'one_tf_tree': one_tf_tree,
+                         'world_file': [LaunchConfiguration('world_file')]}]
         )
     ])

package.xml

@@ -14,6 +14,7 @@
   <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>ackermann_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>std_msgs</depend>
   <depend>stage</depend>

res/install.md

@@ -6,6 +6,7 @@ mkdir src
 cd src
 git clone [email protected]:tuw-robotics/Stage.git
 git clone [email protected]:tuw-robotics/stage_ros2.git
+sudo apt install ros-jazzy-ackermann-msgs
 rosdep update
 rosdep install --from-paths ./Stage --ignore-src -r -y  # install dependencies for Stage
 rosdep install --from-paths ./stage_ros2 --ignore-src -r -y  # install dependencies for stage_ros2

src/stage_node.cpp

@@ -23,6 +23,10 @@ void StageNode::declare_parameters()
   param_desc_use_stamped_velocity.description = "on true it uses stamped command velocities, TwistStamped on false Twist msgs!";
   this->declare_parameter<bool>("use_stamped_velocity", false, param_desc_use_stamped_velocity);
 
+  auto param_desc_use_ackermann = rcl_interfaces::msg::ParameterDescriptor{};
+  param_desc_use_ackermann.description = "on true it uses Ackermann /drive topic instead of Twist /cmd_vel";
+  this->declare_parameter<bool>("use_ackermann", false, param_desc_use_ackermann);
+
   auto param_desc_enable_gui = rcl_interfaces::msg::ParameterDescriptor{};
   param_desc_enable_gui.description = "Enable GUI!";
   this->declare_parameter<bool>("enable_gui", true, param_desc_enable_gui);
@@ -83,6 +87,7 @@ void StageNode::update_parameters()
 {
   double base_watchdog_timeout_sec{5.0};
   this->get_parameter("use_stamped_velocity", this->use_stamped_velocity_);
+  this->get_parameter("use_ackermann", this->use_ackermann_);
   this->get_parameter("enable_gui", this->enable_gui_);
   this->get_parameter("enforce_prefixes", this->enforce_prefixes_);
   this->get_parameter("one_tf_tree", this->one_tf_tree_);

src/vehicle.cpp

@@ -9,6 +9,7 @@
 #define TOPIC_ODOM "odom"
 #define TOPIC_GROUND_TRUTH "ground_truth"
 #define TOPIC_CMD_VEL "cmd_vel"
+#define TOPIC_DRIVE "drive"
 
 using std::placeholders::_1;
 
@@ -66,26 +67,43 @@ void StageNode::Vehicle::init(bool use_topic_prefixes, bool use_one_tf_tree)
   topic_name_odom_ = topic_name_space_ + TOPIC_ODOM;
   topic_name_ground_truth_ = topic_name_space_ + TOPIC_GROUND_TRUTH;
   topic_name_cmd_ = topic_name_space_ + TOPIC_CMD_VEL;
+  topic_name_drive_ = topic_name_space_ + TOPIC_DRIVE;
 
   tf_static_broadcaster_ = std::make_shared<stage_ros2::StaticTransformBroadcaster>(node_, topic_name_tf_static_.c_str());
   tf_broadcaster_ = std::make_shared<stage_ros2::TransformBroadcaster>(node_, topic_name_tf_.c_str());
 
   pub_odom_ = node_->create_publisher<nav_msgs::msg::Odometry>(topic_name_odom_, 10);
   pub_ground_truth_ =
       node_->create_publisher<nav_msgs::msg::Odometry>(topic_name_ground_truth_, 10);
-
-  if(node_->use_stamped_velocity_){    
-    sub_cmd_stamped_ =
-      node_->create_subscription<geometry_msgs::msg::TwistStamped>(
-          topic_name_cmd_, 10,
-          std::bind(&StageNode::Vehicle::callback_cmd_stamped, this, _1));
-    RCLCPP_INFO(node_->get_logger(), "%s is using stamped velocity commands.", name().c_str());
+
+  if(node_->use_ackermann_){
+      if(node_->use_stamped_velocity_){
+        sub_drive_stamped_ =
+          node_->create_subscription<ackermann_msgs::msg::AckermannDriveStamped>(
+              topic_name_drive_, 10,
+              std::bind(&StageNode::Vehicle::callback_drive_stamped, this, _1));
+        RCLCPP_INFO(node_->get_logger(), "%s is using stamped Ackermann velocity commands.", name().c_str());
+      } else {
+        sub_drive_ =
+            node_->create_subscription<ackermann_msgs::msg::AckermannDrive>(
+                topic_name_drive_, 10,
+                std::bind(&StageNode::Vehicle::callback_drive, this, _1));
+        RCLCPP_INFO(node_->get_logger(), "%s is using unstamped Ackermann velocity commands.", name().c_str());
+      }
   } else {
-    sub_cmd_ =
-        node_->create_subscription<geometry_msgs::msg::Twist>(
-            topic_name_cmd_, 10,
-            std::bind(&StageNode::Vehicle::callback_cmd, this, _1));
-    RCLCPP_INFO(node_->get_logger(), "%s is using unstamped velocity commands.", name().c_str());
+      if(node_->use_stamped_velocity_){
+        sub_cmd_stamped_ =
+          node_->create_subscription<geometry_msgs::msg::TwistStamped>(
+              topic_name_cmd_, 10,
+              std::bind(&StageNode::Vehicle::callback_cmd_stamped, this, _1));
+        RCLCPP_INFO(node_->get_logger(), "%s is using stamped velocity commands.", name().c_str());
+      } else {
+        sub_cmd_ =
+            node_->create_subscription<geometry_msgs::msg::Twist>(
+                topic_name_cmd_, 10,
+                std::bind(&StageNode::Vehicle::callback_cmd, this, _1));
+        RCLCPP_INFO(node_->get_logger(), "%s is using unstamped velocity commands.", name().c_str());
+      }
   }
   positionmodel->Subscribe();
 
@@ -220,3 +238,25 @@ void StageNode::Vehicle::callback_cmd_stamped(const geometry_msgs::msg::TwistSta
   time_last_cmd_received_ = node_->sim_time_;
   timeout_cmd_ = time_last_cmd_received_ + node_->base_watchdog_timeout_;
 }
+
+void StageNode::Vehicle::callback_drive(const ackermann_msgs::msg::AckermannDrive::SharedPtr msg)
+{
+  std::scoped_lock lock(node_->msg_lock);
+  this->positionmodel->SetSpeed(
+      msg->speed,
+      0.0,
+      msg->steering_angle);
+  time_last_cmd_received_ = node_->sim_time_;
+  timeout_cmd_ = time_last_cmd_received_ + node_->base_watchdog_timeout_;
+}
+
+void StageNode::Vehicle::callback_drive_stamped(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr msg)
+{
+  std::scoped_lock lock(node_->msg_lock);
+  this->positionmodel->SetSpeed(
+      msg->drive.speed,
+      0.0,
+      msg->drive.steering_angle);
+  time_last_cmd_received_ = node_->sim_time_;
+  timeout_cmd_ = time_last_cmd_received_ + node_->base_watchdog_timeout_;
+}