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 4e286c6..95eb207 100644 --- a/README.md +++ b/README.md @@ -9,9 +9,16 @@ It supports multiple robots with one or multiple tf-trees. ## Stamped velocity Support (TwistStamped) This stage ROS2 bridge can be configured to use stamped velocities on the topic __cmd_vel__. ``` -ros2 launch stage_ros2 demo.launch.py world:=cave use_stamped_velocity:=true +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/launch/stage_street.launch.py b/launch/stage_street.launch.py new file mode 100755 index 0000000..a3c4031 --- /dev/null +++ b/launch/stage_street.launch.py @@ -0,0 +1,86 @@ +"""Demo for a street and a vehicle with AckermannStamped control and Remapping for ARRK Chimera project""" +#!/usr/bin/python3 +# -*- coding: utf-8 -*- +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.substitutions import LaunchConfiguration, TextSubstitution +from launch.actions import DeclareLaunchArgument, OpaqueFunction, SetLaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + + this_directory = get_package_share_directory('stage_ros2') + + use_stamped_velocity = LaunchConfiguration('use_stamped_velocity') + use_stamped_velocity_arg = DeclareLaunchArgument( + 'use_stamped_velocity', + default_value='true', + description='on true stage will accept TwistStamped command messages') + + use_ackermann = LaunchConfiguration('use_ackermann') + use_ackermann_arg = DeclareLaunchArgument( + 'use_ackermann', + default_value='true', + description='on true stage will accept AckermannDrive or AckermannDriveStamped command messages') + + stage_world_arg = DeclareLaunchArgument( + 'world', + default_value=TextSubstitution(text='street'), + description='World file relative to the project world file, without .world') + + enforce_prefixes = LaunchConfiguration('enforce_prefixes') + enforce_prefixes_arg = DeclareLaunchArgument( + 'enforce_prefixes', + default_value='false', + description='on true a prefixes are used for a single robot environment') + + use_static_transformations = LaunchConfiguration('use_static_transformations') + use_static_transformations_arg = DeclareLaunchArgument( + 'use_static_transformations', + default_value='true', + description='Use static transformations for sensor frames!') + + one_tf_tree = LaunchConfiguration('one_tf_tree') + one_tf_tree_arg = DeclareLaunchArgument( + 'one_tf_tree', + default_value='false', + description='on true all tfs are published with a namespace on /tf and /tf_static') + + def stage_world_configuration(context): + file = os.path.join( + this_directory, + 'world', + context.launch_configurations['world'] + '.world') + return [SetLaunchConfiguration('world_file', file)] + + stage_world_configuration_arg = OpaqueFunction(function=stage_world_configuration) + + return LaunchDescription([ + use_stamped_velocity_arg, + use_ackermann_arg, + stage_world_arg, + one_tf_tree_arg, + enforce_prefixes_arg, + use_static_transformations_arg, + stage_world_configuration_arg, + Node( + package='stage_ros2', + executable='stage_ros2', + name='stage', + 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')]}], + remappings=[('/base_scan', '/scan'), + ('/image1', '/zed/left/image'), + ('/depth1', '/zed/left/depth'), + ('/camera_info1', '/zed/left/info'), + ('/image2', '/webcam/image'), + ('/depth2', '/webcam/depth'), + ('/camera_info2', '/webcam/info')] + ) + ]) 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 e3d98f8..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 useing 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 diff --git a/world/bitmaps/oval_circuit.png b/world/bitmaps/oval_circuit.png new file mode 100644 index 0000000..2d652c6 Binary files /dev/null and b/world/bitmaps/oval_circuit.png differ diff --git a/world/bitmaps/street.png b/world/bitmaps/street.png new file mode 100644 index 0000000..e3b2079 Binary files /dev/null and b/world/bitmaps/street.png differ diff --git a/world/include/chimera.inc b/world/include/chimera.inc new file mode 100644 index 0000000..1b62a1f --- /dev/null +++ b/world/include/chimera.inc @@ -0,0 +1,49 @@ +define stereo camera +( + range [0.1 20.0 ] # range in meters that the camera can see + resolution [672 376] # lowest resolution of ZED (max resolution in Stage is 940x939) + size [0.00 0.00 0.00] # size of physical camera + fov [90 60] # field of view in degrees + pantilt [0 0 ] # inclination to the right/inclination downwards + alwayson 1 +) + +define logi_camera camera +( + range [0.01 20.0] + resolution [640 320] # lowered resolution (max resolution possible in Stage is 940x939) + size [0.00 0.00 0.00] + fov [60 40] + pantilt [0 75] # 15 downwards in y-axis + alwayson 1 +) + +define lidar ranger # This is the LIDAR +( + sensor + ( + range [0.0 3.0] # meters that can detect + fov 270.25 + samples 1081 + ) +) + +define Chimera position +( drive "car" + color "white" + stereo( pose [0.220 0.060 0.110 0.000] color "red" pantilt [ 0 0]) # Left camera offset z 0.48 fov 54 110 + logi_camera( pose [0.230 0.000 0.300 0.000 0] color "black" pantilt [ 0 75 ]) + # stereo( pose [0.220 -0.060 0.110 0.000 0] color "pink" ) # Right camera is currently disabled + lidar( pose [0.050 0.000 0.000 0.000] ) + obstacle_return 0 + localization "gps" # Change to "gps" to have impossibly perfect, global odometry + velocity_bounds [-1 1 0 0 0 0 -45.0 45.0] + acceleration_bounds [-0.5 0.5 0 0 0 0 -45 45.0] + gui_nose 1 +) + +define obstacle model +( + size [0.450 0.280 0.220] + gui_nose 0 +) diff --git a/world/oval_circuit.world b/world/oval_circuit.world new file mode 100644 index 0000000..8519623 --- /dev/null +++ b/world/oval_circuit.world @@ -0,0 +1,47 @@ +# oval circuit with obstacles +# Copyright 2025 ARRK Engineering GmbH +include "include/chimera.inc" + +resolution 0.05 +interval_sim 50 + +define floorplan model +( + color "gray30" + boundary 0 + gui_move 0 + gui_nose 1 + gui_grid 1 + gui_outline 0 + gripper_return 1 + fiducial_return 0 + laser_return 1 +) + +window +( + size [ 940 969 ] # in pixels + scale 156.247 # pixels per meter + center [ -0.207 -0.418] + rotate [ 0.000 -90.000 ] + show_data 0 +) + +floorplan +( + name "Street" + bitmap "bitmaps/oval_circuit.png" + size [6.300 4.500 0.000] # Map size in meters + pose [0.000 0.000 0.000 0.000] # Orientation of the map + obstacle_return 0 # Whether robots can hit obstacles +) + +Chimera +( + pose [-1.30 -1.98 0.000 0.000] + size [ 0.420 0.280 0.150 ] + name "Chimera" +) + +obstacle( pose [ 0.5 -1.62 0 180.000 ] color "green") +obstacle( pose [ 0 1.98 0 180.000 ] color "red") \ No newline at end of file diff --git a/world/street.world b/world/street.world new file mode 100644 index 0000000..f3c507b --- /dev/null +++ b/world/street.world @@ -0,0 +1,50 @@ +# Straight street for camera calibration. +# Copyright 2025 ARRK Engineering GmbH +include "include/chimera.inc" + +resolution 0.05 +interval_sim 50 + +define floorplan model +( + color "gray30" + + boundary 0 + gui_move 0 + + gui_nose 1 + gui_grid 1 + gui_outline 0 + gripper_return 1 + fiducial_return 0 + laser_return 1 +) + +window +( + size [ 940 969 ] # in pixels + scale 156.247 # pixels per meter + center [ -0.207 -0.418] + rotate [ 0.000 -90.000 ] + + show_data 0 +) + +floorplan +( + name "Street" + bitmap "bitmaps/street.png" + size [9.000 2.700 0.000] # Map size in meters + pose [0.000 0.000 0.000 0.000] # Orientation of the map + obstacle_return 0 # Whether robots can hit obstacles + +) + +Chimera +( + pose [-4.703 -0.161 0.000 0.000] + size [ 0.420 0.300 0.100 ] + name "Chimera" +) +obstacle( pose [ 5 4 0 180.000 ] color "green") +obstacle( pose [ 5 4 0 180.000 ] color "red") \ No newline at end of file