diff --git a/CMakeLists.txt b/CMakeLists.txt index e914cbc..ed644b9 100644 --- a/CMakeLists.txt +++ b/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. diff --git a/README.md b/README.md index 08a7053..95eb207 100644 --- a/README.md +++ b/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. ``` diff --git a/include/stage_ros2/stage_node.hpp b/include/stage_ros2/stage_node.hpp index 861ed23..ce3333c 100644 --- a/include/stage_ros2/stage_node.hpp +++ b/include/stage_ros2/stage_node.hpp @@ -14,6 +14,7 @@ #include #include #include +#include #include #include #include @@ -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::SharedPtr pub_ground_truth_; // one ground truth rclcpp::Subscription::SharedPtr sub_cmd_; // one cmd_vel subscriber rclcpp::Subscription::SharedPtr sub_cmd_stamped_; // one sub_cmd_stamped_ subscriber + rclcpp::Subscription::SharedPtr sub_drive_; // one drive subscriber + rclcpp::Subscription::SharedPtr sub_drive_stamped_; // one drive_stamped_ subscriber std::shared_ptr tf_static_broadcaster_; std::shared_ptr 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 diff --git a/launch/demo.launch.py b/launch/demo.launch.py index e2a3637..766f525 100755 --- a/launch/demo.launch.py +++ b/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()), ]) diff --git a/launch/stage.launch.py b/launch/stage.launch.py index a93306b..1304d0b 100755 --- a/launch/stage.launch.py +++ b/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')]}] ) ]) diff --git a/package.xml b/package.xml index f8c6023..9a9ae3b 100644 --- a/package.xml +++ b/package.xml @@ -14,6 +14,7 @@ rclcpp_components sensor_msgs geometry_msgs + ackermann_msgs nav_msgs std_msgs stage diff --git a/res/install.md b/res/install.md index 29b6868..f19e2f5 100644 --- a/res/install.md +++ b/res/install.md @@ -6,6 +6,7 @@ mkdir src cd src git clone git@github.com:tuw-robotics/Stage.git git clone git@github.com: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 diff --git a/src/stage_node.cpp b/src/stage_node.cpp index bccdc1c..7d9c2b3 100644 --- a/src/stage_node.cpp +++ b/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("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("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("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_); diff --git a/src/vehicle.cpp b/src/vehicle.cpp index a2a6acd..72fd4bf 100644 --- a/src/vehicle.cpp +++ b/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,6 +67,7 @@ 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(node_, topic_name_tf_static_.c_str()); tf_broadcaster_ = std::make_shared(node_, topic_name_tf_.c_str()); @@ -73,19 +75,35 @@ void StageNode::Vehicle::init(bool use_topic_prefixes, bool use_one_tf_tree) pub_odom_ = node_->create_publisher(topic_name_odom_, 10); pub_ground_truth_ = node_->create_publisher(topic_name_ground_truth_, 10); - - if(node_->use_stamped_velocity_){ - sub_cmd_stamped_ = - node_->create_subscription( - 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( + 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( + 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( - 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( + 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( + 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_; +} \ No newline at end of file