├── .gitattributes ├── CMakeLists.txt ├── README.md ├── config └── rviz │ └── example.rviz ├── include └── stage_ros2 │ ├── stage_node.hpp │ └── visibility.h ├── launch ├── my_house.launch.py └── my_house_multi.launch.py ├── package.xml ├── src ├── camera.cpp ├── main.cpp ├── ranger.cpp ├── stage_node.cpp └── vehicle.cpp └── world ├── maps ├── autolab.png ├── cave.png ├── frieburg.png ├── ghost.png ├── hospital_section.png ├── human_outline.png ├── line.png ├── my_house.jpg ├── simple_rooms.png ├── space_invader.png └── uoa_robotics_lab.png ├── my_house.world ├── my_house_multi.world └── robot ├── camera.inc ├── car_base.inc ├── laser.inc └── mycar.inc /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(stage_ros2) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | find_package(ament_cmake REQUIRED) 14 | find_package(rclcpp REQUIRED) 15 | find_package(rclcpp_components REQUIRED) 16 | find_package(sensor_msgs REQUIRED) 17 | find_package(geometry_msgs REQUIRED) 18 | find_package(nav_msgs REQUIRED) 19 | find_package(std_msgs REQUIRED) 20 | find_package(stage REQUIRED) 21 | find_package(tf2_ros REQUIRED) 22 | find_package(tf2_geometry_msgs REQUIRED) 23 | find_package(std_srvs REQUIRED) 24 | 25 | include_directories( 26 | ./include 27 | ${STAGE_INCLUDE_DIRS} 28 | ) 29 | 30 | add_library(stage_node SHARED 31 | src/stage_node.cpp src/vehicle.cpp src/ranger.cpp src/camera.cpp) 32 | target_compile_definitions(stage_node PRIVATE "STAGE_ROS2_PACKAGE_DLL") 33 | ament_target_dependencies(stage_node rclcpp rclcpp_components 34 | sensor_msgs geometry_msgs nav_msgs std_msgs tf2_ros std_srvs tf2_geometry_msgs) 35 | 36 | 37 | # This package installs libraries without exporting them. 38 | # Export the library path to ensure that the installed libraries are available. 39 | if(NOT WIN32) 40 | ament_environment_hooks( 41 | "${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH}") 42 | endif() 43 | 44 | add_executable(stage_ros2 src/main.cpp) 45 | target_include_directories(stage_ros2 PUBLIC 46 | $ 47 | $) 48 | target_compile_features(stage_ros2 PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 49 | target_link_libraries(stage_ros2 stage_node ${STAGE_LIBRARIES}) 50 | ament_target_dependencies(stage_ros2 rclcpp) 51 | 52 | install( 53 | DIRECTORY 54 | launch 55 | world 56 | config 57 | DESTINATION 58 | share/${PROJECT_NAME}/) 59 | 60 | install(TARGETS 61 | stage_node 62 | ARCHIVE DESTINATION lib 63 | LIBRARY DESTINATION lib 64 | RUNTIME DESTINATION bin) 65 | 66 | install(TARGETS 67 | stage_ros2 68 | DESTINATION lib/${PROJECT_NAME}) 69 | 70 | if(BUILD_TESTING) 71 | find_package(ament_lint_auto REQUIRED) 72 | # the following line skips the linter which checks for copyrights 73 | # comment the line when a copyright and license is added to all source files 74 | set(ament_cmake_copyright_FOUND TRUE) 75 | # the following line skips cpplint (only works in a git repo) 76 | # comment the line when this package is in a git repo and when 77 | # a copyright and license is added to all source files 78 | set(ament_cmake_cpplint_FOUND TRUE) 79 | ament_lint_auto_find_test_dependencies() 80 | endif() 81 | 82 | ament_package() 83 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # stage_ros2 使用说明 2 | 3 | stage_ros2是一个基于ROS2的2D机器人模拟器（Ubuntu22.04 + ROS2 humble）。 4 | 5 | ## 1.安装 6 | 7 | 请先调用如下指令安装依赖： 8 | 9 | ``` 10 | sudo apt-get install git cmake g++ libjpeg8-dev libpng-dev libglu1-mesa-dev libltdl-dev libfltk1.1-dev 11 | ``` 12 | 13 | 进入ROS2工作空间的src目录，调用如下指令下载相关仓库： 14 | 15 | ``` 16 | git clone https://github.com/damuxt/Stage.git 17 | git clone https://github.com/damuxt/stage_ros2.git 18 | ``` 19 | 20 | 进入工作空间目录，使用`colcon build`指令进行构建。 21 | 22 | ## 2.使用 23 | 24 | 在功能包下，已经内置了使用示例，终端下可以执行如下指令启动示例： 25 | 26 | ``` 27 | ros2 launch stage_ros2 my_house.launch.py 28 | ``` 29 | 30 | 2D模拟器以及rviz2将被启动，并且在rviz2中可以显示里程计、激光雷达以及深度相机等相关信息。 31 | 32 | -------------------------------------------------------------------------------- /config/rviz/example.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1/Frames1 10 | - /TF1/Tree1 11 | - /Image2 12 | Splitter Ratio: 0.5 13 | Tree Height: 127 14 | - Class: rviz_common/Selection 15 | Name: Selection 16 | - Class: rviz_common/Tool Properties 17 | Expanded: 18 | - /2D Goal Pose1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz_common/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz_common/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: LaserScan 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz_default_plugins/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.029999999329447746 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Class: rviz_default_plugins/TF 54 | Enabled: true 55 | Frame Timeout: 15 56 | Frames: 57 | All Enabled: true 58 | base_link: 59 | Value: true 60 | camera: 61 | Value: true 62 | laser: 63 | Value: true 64 | odom: 65 | Value: true 66 | Marker Scale: 1 67 | Name: TF 68 | Show Arrows: true 69 | Show Axes: true 70 | Show Names: true 71 | Tree: 72 | odom: 73 | base_link: 74 | camera: 75 | {} 76 | laser: 77 | {} 78 | Update Interval: 0 79 | Value: true 80 | - Angle Tolerance: 0.10000000149011612 81 | Class: rviz_default_plugins/Odometry 82 | Covariance: 83 | Orientation: 84 | Alpha: 0.5 85 | Color: 255; 255; 127 86 | Color Style: Unique 87 | Frame: Local 88 | Offset: 1 89 | Scale: 1 90 | Value: true 91 | Position: 92 | Alpha: 0.30000001192092896 93 | Color: 204; 51; 204 94 | Scale: 1 95 | Value: true 96 | Value: true 97 | Enabled: true 98 | Keep: 100 99 | Name: Odometry 100 | Position Tolerance: 0.10000000149011612 101 | Shape: 102 | Alpha: 1 103 | Axes Length: 1 104 | Axes Radius: 0.10000000149011612 105 | Color: 255; 25; 0 106 | Head Length: 0.30000001192092896 107 | Head Radius: 0.10000000149011612 108 | Shaft Length: 1 109 | Shaft Radius: 0.05000000074505806 110 | Value: Arrow 111 | Topic: 112 | Depth: 5 113 | Durability Policy: Volatile 114 | Filter size: 10 115 | History Policy: Keep Last 116 | Reliability Policy: Reliable 117 | Value: /odom 118 | Value: true 119 | - Alpha: 1 120 | Autocompute Intensity Bounds: true 121 | Autocompute Value Bounds: 122 | Max Value: 10 123 | Min Value: -10 124 | Value: true 125 | Axis: Z 126 | Channel Name: intensity 127 | Class: rviz_default_plugins/LaserScan 128 | Color: 255; 255; 255 129 | Color Transformer: Intensity 130 | Decay Time: 0 131 | Enabled: true 132 | Invert Rainbow: false 133 | Max Color: 255; 255; 255 134 | Max Intensity: 1 135 | Min Color: 0; 0; 0 136 | Min Intensity: 1 137 | Name: LaserScan 138 | Position Transformer: XYZ 139 | Selectable: true 140 | Size (Pixels): 3 141 | Size (m): 0.029999999329447746 142 | Style: Flat Squares 143 | Topic: 144 | Depth: 5 145 | Durability Policy: Volatile 146 | Filter size: 10 147 | History Policy: Keep Last 148 | Reliability Policy: Reliable 149 | Value: /scan 150 | Use Fixed Frame: true 151 | Use rainbow: true 152 | Value: true 153 | - Class: rviz_default_plugins/Image 154 | Enabled: true 155 | Max Value: 1 156 | Median window: 5 157 | Min Value: 0 158 | Name: Image 159 | Normalize Range: true 160 | Topic: 161 | Depth: 5 162 | Durability Policy: Volatile 163 | History Policy: Keep Last 164 | Reliability Policy: Reliable 165 | Value: /image 166 | Value: true 167 | - Class: rviz_default_plugins/Image 168 | Enabled: true 169 | Max Value: 5 170 | Median window: 5 171 | Min Value: 0 172 | Name: Image 173 | Normalize Range: false 174 | Topic: 175 | Depth: 5 176 | Durability Policy: Volatile 177 | History Policy: Keep Last 178 | Reliability Policy: Reliable 179 | Value: /depth 180 | Value: true 181 | Enabled: true 182 | Global Options: 183 | Background Color: 48; 48; 48 184 | Fixed Frame: odom 185 | Frame Rate: 30 186 | Name: root 187 | Tools: 188 | - Class: rviz_default_plugins/Interact 189 | Hide Inactive Objects: true 190 | - Class: rviz_default_plugins/MoveCamera 191 | - Class: rviz_default_plugins/Select 192 | - Class: rviz_default_plugins/FocusCamera 193 | - Class: rviz_default_plugins/Measure 194 | Line color: 128; 128; 0 195 | - Class: rviz_default_plugins/SetInitialPose 196 | Covariance x: 0.25 197 | Covariance y: 0.25 198 | Covariance yaw: 0.06853891909122467 199 | Topic: 200 | Depth: 5 201 | Durability Policy: Volatile 202 | History Policy: Keep Last 203 | Reliability Policy: Reliable 204 | Value: /initialpose 205 | - Class: rviz_default_plugins/SetGoal 206 | Topic: 207 | Depth: 5 208 | Durability Policy: Volatile 209 | History Policy: Keep Last 210 | Reliability Policy: Reliable 211 | Value: /goal_pose 212 | - Class: rviz_default_plugins/PublishPoint 213 | Single click: true 214 | Topic: 215 | Depth: 5 216 | Durability Policy: Volatile 217 | History Policy: Keep Last 218 | Reliability Policy: Reliable 219 | Value: /clicked_point 220 | Transformation: 221 | Current: 222 | Class: rviz_default_plugins/TF 223 | Value: true 224 | Views: 225 | Current: 226 | Class: rviz_default_plugins/Orbit 227 | Distance: 11.84657096862793 228 | Enable Stereo Rendering: 229 | Stereo Eye Separation: 0.05999999865889549 230 | Stereo Focal Distance: 1 231 | Swap Stereo Eyes: false 232 | Value: false 233 | Focal Point: 234 | X: 5.315735340118408 235 | Y: -3.0535218715667725 236 | Z: 6.4047441482543945 237 | Focal Shape Fixed Size: true 238 | Focal Shape Size: 0.05000000074505806 239 | Invert Z Axis: false 240 | Name: Current View 241 | Near Clip Distance: 0.009999999776482582 242 | Pitch: 1.2353979349136353 243 | Target Frame: 244 | Value: Orbit (rviz) 245 | Yaw: 3.5454020500183105 246 | Saved: ~ 247 | Window Geometry: 248 | Displays: 249 | collapsed: false 250 | Height: 683 251 | Hide Left Dock: false 252 | Hide Right Dock: true 253 | Image: 254 | collapsed: false 255 | QMainWindow State: 000000ff00000000fd0000000400000000000002330000020dfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000010a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000014d0000004b0000002800fffffffb0000000a0049006d006100670065010000019e000000ac0000002800ffffff00000001000001ae0000020dfc0200000006fb0000001200720030002000430061006d006500720061000000003d000000910000000000000000fb000000180072003000200049006d0061006700650020005200470042000000003d000000e30000000000000000fb0000001c0072003000200049006d006100670065002000440065007000740068000000003d0000020d0000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000780000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b60000003efc0100000002fb0000000800540069006d00650100000000000004b6000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000027d0000020d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 256 | Selection: 257 | collapsed: false 258 | Time: 259 | collapsed: false 260 | Tool Properties: 261 | collapsed: false 262 | Views: 263 | collapsed: false 264 | Width: 1206 265 | X: 74 266 | Y: 0 267 | -------------------------------------------------------------------------------- /include/stage_ros2/stage_node.hpp: -------------------------------------------------------------------------------- 1 | #ifndef STAGE_ROS2_PKG__STAGE_ROS_HPP_ 2 | #define STAGE_ROS2_PKG__STAGE_ROS_HPP_ 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | // libstage 14 | #include 15 | 16 | // roscpp 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include "stage_ros2/visibility.h" 32 | 33 | // Our node 34 | class StageNode : public rclcpp::Node 35 | { 36 | public: 37 | STAGE_ROS2_PACKAGE_PUBLIC StageNode(rclcpp::NodeOptions options); 38 | 39 | private: 40 | // A mutex to lock access to fields that are used in message callbacks 41 | std::mutex msg_lock; 42 | 43 | // a structure representing a robot inthe simulator 44 | class Vehicle 45 | { 46 | public: 47 | class Ranger 48 | { 49 | 50 | size_t id_; 51 | Stg::ModelRanger * model; 52 | std::shared_ptr vehicle; 53 | StageNode * node; 54 | std::string topic_name; 55 | std::string frame_base; 56 | std::string frame_id; 57 | geometry_msgs::msg::TransformStamped::SharedPtr transform; 58 | rclcpp::Publisher::SharedPtr pub; 59 | sensor_msgs::msg::LaserScan::SharedPtr msg; 60 | bool prepare_msg(); 61 | bool prepare_tf(); 62 | double gaussRand(double mu, double sigma); 63 | public: 64 | Ranger( 65 | unsigned int id, Stg::ModelRanger * m, std::shared_ptr & vehicle, 66 | StageNode * node); 67 | void init(bool add_id_to_topic); 68 | unsigned int id() const; 69 | void publish_msg(); 70 | void publish_tf(); 71 | }; 72 | class Camera 73 | { 74 | size_t id_; 75 | Stg::ModelCamera * model; 76 | std::shared_ptr vehicle; 77 | StageNode * node; 78 | geometry_msgs::msg::TransformStamped::SharedPtr transform; 79 | rclcpp::Publisher::SharedPtr pub_image; // multiple images 80 | sensor_msgs::msg::Image::SharedPtr msg_image; 81 | rclcpp::Publisher::SharedPtr pub_depth; // multiple depths 82 | sensor_msgs::msg::Image::SharedPtr msg_depth; 83 | rclcpp::Publisher::SharedPtr pub_camera; // multiple cameras 84 | sensor_msgs::msg::CameraInfo::SharedPtr msg_camera; 85 | bool prepare_msg(); 86 | bool prepare_msg_image(); 87 | bool prepare_msg_depth(); 88 | bool prepare_msg_camera(); 89 | bool prepare_tf(); 90 | 91 | public: 92 | Camera( 93 | unsigned int id, Stg::ModelCamera * m, std::shared_ptr & vehicle, 94 | StageNode * node); 95 | void init(bool add_id_to_topic); 96 | unsigned int id() const; 97 | void publish_msg(); 98 | void publish_tf(); 99 | std::string topic_name_image; 100 | std::string topic_name_depth; 101 | std::string topic_name_camera_info; 102 | std::string frame_id; 103 | }; 104 | 105 | private: 106 | size_t id_; 107 | Stg::Pose initial_pose_; 108 | std::string name_; /// used for the ros publisher 109 | StageNode * node_; 110 | Stg::World * world_; 111 | rclcpp::Time time_last_cmd_received_; 112 | rclcpp::Time timeout_cmd_; /// if no command is received befor the vehicle is stopped 113 | // Last time we saved global position (for velocity calculation). 114 | rclcpp::Time time_last_pose_update_; 115 | 116 | std::string name_space_; 117 | std::string topic_name_cmd_; 118 | 119 | std::string topic_name_odom_; 120 | std::string topic_name_ground_truth_; 121 | std::string frame_id_odom_; 122 | std::string frame_id_world_; 123 | std::string frame_id_base_link_; 124 | nav_msgs::msg::Odometry msg_odom_; 125 | std::shared_ptr global_pose_; 126 | 127 | public: 128 | Vehicle(size_t id, const Stg::Pose & pose, const std::string & name, StageNode * node); 129 | 130 | void soft_reset(); 131 | size_t id() const; 132 | const std::string & name() const; 133 | const std::string & name_space() const; 134 | void init(bool use_model_name); 135 | void callback_cmd(const geometry_msgs::msg::Twist::SharedPtr msg); 136 | void publish_msg(); 137 | void publish_tf(); 138 | void check_watchdog_timeout(); 139 | 140 | // stage related models 141 | Stg::ModelPosition * positionmodel; // one position 142 | std::vector> rangers_; // multiple rangers per position 143 | std::vector> cameras_; // multiple cameras per position 144 | 145 | // ros publishers 146 | rclcpp::Publisher::SharedPtr pub_odom_; // one odom 147 | rclcpp::Publisher::SharedPtr pub_ground_truth_; // one ground truth 148 | rclcpp::Subscription::SharedPtr sub_cmd_; // one cmd_vel subscriber 149 | 150 | std::shared_ptr tf_static_broadcaster_; 151 | std::shared_ptr tf_broadcaster_; 152 | }; 153 | 154 | /// vector to hold the simulated vehicles with ros interfaces 155 | std::vector> vehicles_; 156 | 157 | 158 | bool isDepthCanonical_; /// ROS parameter 159 | bool use_model_names; /// ROS parameter 160 | bool enable_gui_; /// ROS parameter 161 | bool publish_ground_truth_; /// ROS parameter 162 | bool use_static_transformations_; /// ROS parameter 163 | std::string world_file_; /// ROS parameter 164 | std::string frame_id_odom_name_; /// ROS parameter 165 | std::string frame_id_world_name_; /// ROS parameter 166 | std::string frame_id_base_link_name_; /// ROS parameter 167 | 168 | // TF broadcaster to publish the robot odom 169 | std::shared_ptr tf_; 170 | 171 | // Service to listening on soft reset signals 172 | rclcpp::Service::SharedPtr srv_reset_; 173 | 174 | // publisher for the simulated clock 175 | rclcpp::Publisher::SharedPtr clock_pub_; 176 | 177 | /// called only ones to init the models and to crate for each model a link to ROS 178 | static int callback_init_stage_model(Stg::Model * mod, StageNode * node); 179 | 180 | /// called on every simulation interation 181 | static int callback_update_stage_world(Stg::World * world, StageNode * node); 182 | 183 | public: 184 | ~StageNode(); 185 | // Constructor 186 | void init(int argc, char ** argv); 187 | 188 | // declares ros parameters 189 | void declare_parameters(); 190 | 191 | // int ros parameters for the startup 192 | void update_parameters(); 193 | 194 | // callback to check changes on the parameters 195 | void callback_update_parameters(); 196 | 197 | // timer to check regulary for parameter changes 198 | rclcpp::TimerBase::SharedPtr timer_update_parameter_; 199 | 200 | // Subscribe to models of interest. Currently, we find and subscribe 201 | // to the first 'laser' model and the first 'position' model. Returns 202 | // 0 on success (both models subscribed), -1 otherwise. 203 | int SubscribeModels(); 204 | 205 | // Do one update of the world. May pause if the next update time 206 | // has not yet arrived. 207 | bool UpdateWorld(); 208 | 209 | // Service callback for soft reset 210 | bool cb_reset_srv(const std_srvs::srv::Empty::Request::SharedPtr, 211 | std_srvs::srv::Empty::Response::SharedPtr); 212 | 213 | // The main simulator object 214 | Stg::World * world; 215 | 216 | rclcpp::Duration base_watchdog_timeout_; 217 | 218 | // Current simulation time 219 | rclcpp::Time sim_time_; 220 | 221 | private: 222 | static geometry_msgs::msg::TransformStamped create_transform_stamped( 223 | const tf2::Transform & in, 224 | const rclcpp::Time & timestamp, const std::string & frame_id, 225 | const std::string & child_frame_id); 226 | static geometry_msgs::msg::Quaternion createQuaternionMsgFromYaw(double yaw); 227 | 228 | }; 229 | 230 | #endif // STAGE_ROS2_PKG__STAGE_ROS_HPP_ 231 | -------------------------------------------------------------------------------- /include/stage_ros2/visibility.h: -------------------------------------------------------------------------------- 1 | #ifndef STAGE_ROS2_PKG__VISIBILITY_H_ 2 | #define STAGE_ROS2_PKG__VISIBILITY_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" 6 | { 7 | #endif 8 | 9 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 10 | // https://gcc.gnu.org/wiki/Visibility 11 | 12 | #if defined _WIN32 || defined __CYGWIN__ 13 | 14 | #ifdef __GNUC__ 15 | #define STAGE_ROS2_PACKAGE_EXPORT __attribute__ ((dllexport)) 16 | #define STAGE_ROS2_PACKAGE_IMPORT __attribute__ ((dllimport)) 17 | #else 18 | #define STAGE_ROS2_PACKAGE_EXPORT __declspec(dllexport) 19 | #define STAGE_ROS2_PACKAGE_IMPORT __declspec(dllimport) 20 | #endif 21 | 22 | #ifdef STAGE_ROS2_PACKAGE_DLL 23 | #define STAGE_ROS2_PACKAGE_PUBLIC STAGE_ROS2_PACKAGE_EXPORT 24 | #else 25 | #define STAGE_ROS2_PACKAGE_PUBLIC STAGE_ROS2_PACKAGE_IMPORT 26 | #endif 27 | 28 | #define STAGE_ROS2_PACKAGE_PUBLIC_TYPE STAGE_ROS2_PACKAGE_PUBLIC 29 | 30 | #define STAGE_ROS2_PACKAGE_LOCAL 31 | 32 | #else 33 | 34 | #define STAGE_ROS2_PACKAGE_EXPORT __attribute__ ((visibility("default"))) 35 | #define STAGE_ROS2_PACKAGE_IMPORT 36 | 37 | #if __GNUC__ >= 4 38 | #define STAGE_ROS2_PACKAGE_PUBLIC __attribute__ ((visibility("default"))) 39 | #define STAGE_ROS2_PACKAGE_LOCAL __attribute__ ((visibility("hidden"))) 40 | #else 41 | #define STAGE_ROS2_PACKAGE_PUBLIC 42 | #define STAGE_ROS2_PACKAGE_LOCAL 43 | #endif 44 | 45 | #define STAGE_ROS2_PACKAGE_PUBLIC_TYPE 46 | #endif 47 | 48 | #ifdef __cplusplus 49 | } 50 | #endif 51 | 52 | #endif // STAGE_ROS2_PKG__VISIBILITY_H_ 53 | -------------------------------------------------------------------------------- /launch/my_house.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | import os 4 | 5 | from ament_index_python.packages import get_package_share_directory 6 | from launch import LaunchDescription 7 | from launch.substitutions import LaunchConfiguration, TextSubstitution 8 | from launch.actions import DeclareLaunchArgument, OpaqueFunction, SetLaunchConfiguration 9 | from launch_ros.actions import Node 10 | 11 | 12 | def generate_launch_description(): 13 | 14 | this_directory = get_package_share_directory('stage_ros2') 15 | use_sim_time = LaunchConfiguration('use_sim_time', default='true') 16 | 17 | stage_world_arg = DeclareLaunchArgument( 18 | 'world', 19 | default_value=TextSubstitution(text='my_house'), 20 | description='World file relative to the project world file, without .world') 21 | 22 | def stage_world_configuration(context): 23 | file = os.path.join( 24 | this_directory, 25 | 'world', 26 | context.launch_configurations['world'] + '.world') 27 | return [SetLaunchConfiguration('world_file', file)] 28 | 29 | stage_world_configuration_arg = OpaqueFunction(function=stage_world_configuration) 30 | 31 | return LaunchDescription([ 32 | stage_world_arg, 33 | stage_world_configuration_arg, 34 | Node( 35 | package='stage_ros2', 36 | executable='stage_ros2', 37 | name='stage', 38 | parameters=[{ 39 | "world_file": [LaunchConfiguration('world_file')]}], 40 | remappings=[("/base_scan","/scan")] 41 | ), 42 | Node( 43 | package='rviz2', 44 | executable='rviz2', 45 | name='rviz2', 46 | arguments=['-d', os.path.join( 47 | this_directory, 48 | 'config/rviz/example.rviz')], 49 | ) 50 | ]) 51 | -------------------------------------------------------------------------------- /launch/my_house_multi.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | import os 4 | 5 | from ament_index_python.packages import get_package_share_directory 6 | from launch import LaunchDescription 7 | from launch.substitutions import LaunchConfiguration, TextSubstitution 8 | from launch.actions import DeclareLaunchArgument, OpaqueFunction, SetLaunchConfiguration 9 | from launch_ros.actions import Node 10 | 11 | 12 | def generate_launch_description(): 13 | 14 | this_directory = get_package_share_directory('stage_ros2') 15 | 16 | stage_world_arg = DeclareLaunchArgument( 17 | 'world', 18 | default_value=TextSubstitution(text='my_house_multi'), 19 | description='World file relative to the project world file, without .world') 20 | 21 | def stage_world_configuration(context): 22 | file = os.path.join( 23 | this_directory, 24 | 'world', 25 | context.launch_configurations['world'] + '.world') 26 | return [SetLaunchConfiguration('world_file', file)] 27 | 28 | stage_world_configuration_arg = OpaqueFunction(function=stage_world_configuration) 29 | 30 | return LaunchDescription([ 31 | stage_world_arg, 32 | stage_world_configuration_arg, 33 | Node( 34 | package='stage_ros2', 35 | executable='stage_ros2', 36 | name='stage', 37 | parameters=[{ 38 | "world_file": [LaunchConfiguration('world_file')]}], 39 | remappings=[("/base_scan","/scan")] 40 | ) 41 | ]) 42 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | stage_ros2 5 | 0.0.1 6 | ROS2 wrapper for Stage 7 | Markus Bader 8 | BSD 9 | 10 | ament_cmake 11 | 12 | 13 | rclcpp 14 | rclcpp_components 15 | Stage 16 | sensor_msgs 17 | geometry_msgs 18 | nav_msgs 19 | std_msgs 20 | tf 21 | 22 | 23 | rclcpp 24 | rclcpp_components 25 | Stage 26 | sensor_msgs 27 | geometry_msgs 28 | nav_msgs 29 | std_msgs 30 | tf 31 | std_srvs 32 | 33 | ament_lint_auto 34 | ament_lint_common 35 | ament_cmake_clang_format 36 | ament_cmake_cppcheck 37 | ament_cmake_pycodestyle 38 | 39 | 40 | ament_cmake 41 | 42 | 43 | -------------------------------------------------------------------------------- /src/camera.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #define TOPIC_IMAGE "image" 8 | #define TOPIC_DEPTH "depth" 9 | #define TOPIC_CAMERA_INFO "camera_info" 10 | #define FRAME_CAMERA "camera" 11 | 12 | using std::placeholders::_1; 13 | 14 | StageNode::Vehicle::Camera::Camera( 15 | unsigned int id, Stg::ModelCamera * m, 16 | std::shared_ptr & v, StageNode * n) 17 | : id_(id), model(m), vehicle(v), node(n) {} 18 | 19 | unsigned int StageNode::Vehicle::Camera::id() const 20 | { 21 | return id_; 22 | } 23 | 24 | void StageNode::Vehicle::Camera::init(bool add_id_to_topic) 25 | { 26 | model->Subscribe(); 27 | topic_name_image = vehicle->name_space_ + TOPIC_IMAGE; 28 | topic_name_camera_info = vehicle->name_space_ + TOPIC_CAMERA_INFO; 29 | topic_name_depth = vehicle->name_space_ + TOPIC_DEPTH; 30 | frame_id = vehicle->name_space_ + FRAME_CAMERA; 31 | 32 | if (add_id_to_topic) { 33 | topic_name_image += std::to_string(id()); 34 | topic_name_camera_info += std::to_string(id()); 35 | topic_name_depth += std::to_string(id()); 36 | frame_id += std::to_string(id()); 37 | } 38 | 39 | pub_image = node->create_publisher(topic_name_image, 10); 40 | pub_camera = node->create_publisher(topic_name_camera_info, 10); 41 | pub_depth = node->create_publisher(topic_name_depth, 10); 42 | } 43 | bool StageNode::Vehicle::Camera::prepare_msg_image() 44 | { 45 | if (msg_image) { 46 | return true; 47 | } 48 | if (!model->FrameColor()) { 49 | return false; 50 | } 51 | msg_image = std::make_shared(); 52 | 53 | msg_image->height = model->getHeight(); 54 | msg_image->width = model->getWidth(); 55 | msg_image->encoding = "rgba8"; 56 | // node->imageMsgs[r].is_bigendian=""; 57 | msg_image->step = msg_image->width * 4; 58 | msg_image->data.resize(msg_image->width * msg_image->height * 4); 59 | msg_image->header.frame_id = frame_id; 60 | 61 | return true; 62 | } 63 | bool StageNode::Vehicle::Camera::prepare_msg_depth() 64 | { 65 | if (msg_depth) { 66 | return true; 67 | } 68 | if (!model->FrameDepth()) { 69 | return false; 70 | } 71 | msg_depth = std::make_shared(); 72 | msg_depth->height = model->getHeight(); 73 | msg_depth->width = model->getWidth(); 74 | msg_depth->encoding = 75 | node->isDepthCanonical_ ? sensor_msgs::image_encodings::TYPE_32FC1 : sensor_msgs:: 76 | image_encodings::TYPE_16UC1; 77 | // node->depthMsgs[r].is_bigendian=""; 78 | int sz = node->isDepthCanonical_ ? sizeof(float) : sizeof(uint16_t); 79 | size_t len = msg_depth->width * msg_depth->height; 80 | msg_depth->step = msg_depth->width * sz; 81 | msg_depth->data.resize(len * sz); 82 | return true; 83 | } 84 | bool StageNode::Vehicle::Camera::prepare_msg_camera() 85 | { 86 | 87 | if (msg_camera) { 88 | return true; 89 | } 90 | if (!(model->FrameColor() || model->FrameDepth())) { 91 | return false; 92 | } 93 | msg_camera = std::make_shared(); 94 | msg_camera->header.frame_id = frame_id; 95 | msg_camera->header.stamp = node->sim_time_; 96 | msg_camera->height = model->getHeight(); 97 | msg_camera->width = model->getWidth(); 98 | 99 | double fx, fy, cx, cy; 100 | cx = msg_camera->width / 2.0; 101 | cy = msg_camera->height / 2.0; 102 | double fovh = model->getCamera().horizFov() * M_PI / 180.0; 103 | double fovv = model->getCamera().vertFov() * M_PI / 180.0; 104 | // double fx_ = 1.43266615300557*node->models[r]->getWidth()/tan(fovh); 105 | // double fy_ = 1.43266615300557*node->models[r]->getHeight()/tan(fovv); 106 | fx = model->getWidth() / (2 * tan(fovh / 2)); 107 | fy = model->getHeight() / (2 * tan(fovv / 2)); 108 | 109 | // ROS_INFO("fx=%.4f,%.4f; fy=%.4f,%.4f", fx, fx_, fy, fy_); 110 | 111 | msg_camera->d.resize(4, 0.0); 112 | 113 | msg_camera->k[0] = fx; 114 | msg_camera->k[2] = cx; 115 | msg_camera->k[4] = fy; 116 | msg_camera->k[5] = cy; 117 | msg_camera->k[8] = 1.0; 118 | 119 | msg_camera->r[0] = 1.0; 120 | msg_camera->r[4] = 1.0; 121 | msg_camera->r[8] = 1.0; 122 | 123 | msg_camera->p[0] = fx; 124 | msg_camera->p[2] = cx; 125 | msg_camera->p[5] = fy; 126 | msg_camera->p[6] = cy; 127 | msg_camera->p[10] = 1.0; 128 | return true; 129 | } 130 | 131 | bool StageNode::Vehicle::Camera::prepare_msg() 132 | { 133 | if (!prepare_msg_image()) {return false;} 134 | if (!prepare_msg_depth()) {return false;} 135 | return true; 136 | } 137 | 138 | void StageNode::Vehicle::Camera::publish_msg() 139 | { 140 | // Translate into ROS message format and publish 141 | if (prepare_msg_image()) { 142 | 143 | memcpy(&(msg_image->data[0]), model->FrameColor(), msg_image->width * msg_image->height * 4); 144 | 145 | // invert the opengl weirdness 146 | char * temp = new char[msg_image->step]; 147 | for (unsigned int y = 0; y < (msg_image->height + 1) / 2; y++) { 148 | memcpy(temp, &msg_image->data[y * msg_image->step], msg_image->step); 149 | memcpy( 150 | &(msg_image->data[y * msg_image->step]), 151 | &(msg_image->data[(msg_image->height - y - 1) * msg_image->step]), msg_image->step); 152 | memcpy( 153 | &(msg_image->data[(msg_image->height - y - 1) * msg_image->step]), temp, 154 | msg_image->step); 155 | } 156 | msg_image->header.stamp = node->sim_time_; 157 | pub_image->publish(*msg_image); 158 | } 159 | 160 | // Translate into ROS message format and publish 161 | if (prepare_msg_depth()) { 162 | // processing data according to REP118 163 | if (node->isDepthCanonical_) { 164 | float nearClip = model->getCamera().nearClip(); 165 | float farClip = model->getCamera().farClip(); 166 | memcpy(&(msg_depth->data[0]), model->FrameDepth(), msg_depth->data.size()); 167 | float * data = (float *)&(msg_depth->data[0]); 168 | size_t len = msg_depth->width * msg_depth->height; 169 | for (size_t i = 0; i < len; ++i) { 170 | if (data[i] <= nearClip) { 171 | data[i] = -INFINITY; 172 | } else if (data[i] >= farClip) { 173 | data[i] = INFINITY; 174 | } 175 | } 176 | } else { 177 | int nearClip = (int)(model->getCamera().nearClip() * 1000); 178 | int farClip = (int)(model->getCamera().farClip() * 1000); 179 | size_t len = msg_depth->width * msg_depth->height; 180 | for (size_t i = 0; i < len; ++i) { 181 | int v = (int)(model->FrameDepth()[i] * 1000); 182 | if (v <= nearClip || v >= farClip) { 183 | v = 0; 184 | } 185 | ((uint16_t *)&(msg_depth->data[0]))[i] = 186 | (uint16_t)((v <= nearClip || v >= farClip) ? 0 : v); 187 | } 188 | } 189 | 190 | // invert the opengl weirdness 191 | char * temp = new char[msg_depth->step]; 192 | for (unsigned int y = 0; y < msg_depth->height / 2; y++) { 193 | memcpy(temp, &msg_depth->data[y * msg_depth->step], msg_depth->step); 194 | memcpy( 195 | &(msg_depth->data[y * msg_depth->step]), 196 | &(msg_depth->data[(msg_depth->height - 1 - y) * msg_depth->step]), msg_depth->step); 197 | memcpy( 198 | &(msg_depth->data[(msg_depth->height - 1 - y) * msg_depth->step]), temp, 199 | msg_depth->step); 200 | } 201 | 202 | msg_depth->header.frame_id = frame_id; 203 | msg_depth->header.stamp = node->sim_time_; 204 | pub_depth->publish(*msg_depth); 205 | } 206 | 207 | // Translate into ROS message format and publish 208 | if (prepare_msg_camera()) { 209 | msg_camera->header.stamp = node->sim_time_; 210 | pub_camera->publish(*msg_camera); 211 | } 212 | } 213 | bool StageNode::Vehicle::Camera::prepare_tf() 214 | { 215 | if (transform) {return true;} 216 | 217 | if (!(model->FrameColor() || model->FrameDepth())) {return false;} 218 | transform = std::make_shared(); 219 | 220 | Stg::Pose pose = model->GetPose(); 221 | tf2::Quaternion quternion; 222 | quternion.setRPY( 223 | (model->getCamera().pitch() * M_PI / 180.0) - M_PI, 224 | 0.0, 225 | pose.a + (model->getCamera().yaw() * M_PI / 180.0) - vehicle->positionmodel->GetPose().a); 226 | tf2::Transform tr = 227 | tf2::Transform( 228 | quternion, 229 | tf2::Vector3(pose.x, pose.y, vehicle->positionmodel->GetGeom().size.z + pose.z)); 230 | *transform = 231 | create_transform_stamped(tr, node->sim_time_, vehicle->frame_id_base_link_, frame_id); 232 | if (node->use_static_transformations_) { 233 | vehicle->tf_static_broadcaster_->sendTransform(*transform); 234 | } 235 | return true; 236 | } 237 | void StageNode::Vehicle::Camera::publish_tf() 238 | { 239 | if (prepare_tf()) { 240 | 241 | if (node->use_static_transformations_) {return;} 242 | 243 | // use tf publsiher only if use_static_transformations_ is false 244 | transform->header.stamp = node->sim_time_; 245 | vehicle->tf_broadcaster_->sendTransform(*transform); 246 | } 247 | } 248 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "stage_ros2/stage_node.hpp" 3 | #include "rclcpp/rclcpp.hpp" 4 | 5 | int main(int argc, char ** argv) 6 | { 7 | rclcpp::init(argc, argv); 8 | auto node = std::make_shared(rclcpp::NodeOptions()); 9 | node->init(argc - 1, argv); 10 | if (node->SubscribeModels() != 0) {exit(-1);} 11 | std::thread t = std::thread([&node]() {rclcpp::spin(node);}); 12 | node->world->Start(); 13 | Stg::World::Run(); 14 | return 0; 15 | } 16 | -------------------------------------------------------------------------------- /src/ranger.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #define TOPIC_LASER "base_scan" 8 | #define FRAME_LASER "laser" 9 | 10 | using std::placeholders::_1; 11 | 12 | StageNode::Vehicle::Ranger::Ranger( 13 | unsigned int id, Stg::ModelRanger * m, 14 | std::shared_ptr & v, StageNode * n) 15 | : id_(id), model(m), vehicle(v), node(n) {} 16 | 17 | unsigned int StageNode::Vehicle::Ranger::id() const 18 | { 19 | return id_; 20 | } 21 | void StageNode::Vehicle::Ranger::init(bool add_id_to_topic) 22 | { 23 | model->Subscribe(); 24 | topic_name = vehicle->name_space_ + TOPIC_LASER; 25 | frame_id = vehicle->name_space_ + FRAME_LASER; 26 | if (add_id_to_topic) { 27 | topic_name += std::to_string(id()); 28 | frame_id += std::to_string(id()); 29 | } 30 | 31 | pub = node->create_publisher(topic_name, 10); 32 | } 33 | 34 | bool StageNode::Vehicle::Ranger::prepare_msg() 35 | { 36 | if (msg) {return true;} 37 | if (model->GetSensors().size() > 1) { 38 | RCLCPP_WARN(node->get_logger(), "ROS Stage currently supports rangers with 1 sensor only."); 39 | } 40 | const Stg::ModelRanger::Sensor & sensor = model->GetSensors()[0]; // we use the first sensor data 41 | if (sensor.ranges.size() == 0) {return false;} 42 | 43 | msg = std::make_shared(); 44 | msg->angle_min = -sensor.fov / 2.0; 45 | msg->angle_max = +sensor.fov / 2.0; 46 | msg->angle_increment = sensor.fov / (double)(sensor.sample_count - 1); 47 | msg->range_min = sensor.range.min; 48 | msg->range_max = sensor.range.max; 49 | msg->ranges.resize(sensor.ranges.size()); 50 | msg->intensities.resize(sensor.intensities.size()); 51 | msg->header.frame_id = frame_id; 52 | 53 | return true; 54 | } 55 | 56 | bool StageNode::Vehicle::Ranger::prepare_tf() 57 | { 58 | if (transform) {return true;} 59 | 60 | transform = std::make_shared(); 61 | 62 | Stg::Pose pose = model->GetPose(); 63 | tf2::Quaternion quternion; 64 | quternion.setRPY(0.0, 0.0, pose.a); 65 | tf2::Transform txLaser = 66 | tf2::Transform( 67 | quternion, 68 | tf2::Vector3(pose.x, pose.y, vehicle->positionmodel->GetGeom().size.z + pose.z)); 69 | *transform = create_transform_stamped( 70 | txLaser, node->sim_time_, vehicle->frame_id_base_link_, 71 | frame_id); 72 | if (node->use_static_transformations_) { 73 | vehicle->tf_static_broadcaster_->sendTransform(*transform); 74 | } 75 | return true; 76 | } 77 | 78 | void StageNode::Vehicle::Ranger::publish_msg() 79 | { 80 | if (model->GetSensors().size() > 1) { 81 | RCLCPP_WARN(node->get_logger(), "ROS Stage currently supports rangers with 1 sensor only."); 82 | } 83 | 84 | // for now we access only the zeroth sensor of the ranger - good 85 | // enough for most laser models that have a single beam origin 86 | const Stg::ModelRanger::Sensor & sensor = model->GetSensors()[0]; // we use the first sensor data 87 | 88 | if (prepare_msg()) { 89 | msg->header.stamp = node->sim_time_; 90 | for (unsigned int i = 0; i < sensor.ranges.size(); i++) { 91 | msg->ranges[i] = sensor.ranges[i] + gaussRand(0.0,0.01); 92 | msg->intensities[i] = sensor.intensities[i]; 93 | } 94 | pub->publish(*msg); 95 | } 96 | } 97 | 98 | double StageNode::Vehicle::Ranger::gaussRand(double mu, double sigma){ 99 | static double V1, V2, S; 100 | static int phase = 0; 101 | double X; 102 | 103 | if ( phase == 0 ) { 104 | do { 105 | double U1 = (double)rand() / RAND_MAX; 106 | double U2 = (double)rand() / RAND_MAX; 107 | 108 | V1 = 2 * U1 - 1; 109 | V2 = 2 * U2 - 1; 110 | S = V1 * V1 + V2 * V2; 111 | } while(S >= 1 || S == 0); 112 | 113 | X = V1 * sqrt(-2 * log(S) / S); 114 | } else 115 | X = V2 * sqrt(-2 * log(S) / S); 116 | 117 | phase = 1 - phase; 118 | return (X*sigma + mu); 119 | } 120 | 121 | void StageNode::Vehicle::Ranger::publish_tf() 122 | { 123 | if (prepare_tf()) { 124 | 125 | if (node->use_static_transformations_) {return;} 126 | 127 | // use tf publsiher only if use_static_transformations_ is false 128 | transform->header.stamp = node->sim_time_; 129 | vehicle->tf_broadcaster_->sendTransform(*transform); 130 | } 131 | } 132 | -------------------------------------------------------------------------------- /src/stage_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | StageNode::StageNode(rclcpp::NodeOptions options) 8 | : Node("stage_ros2", options), base_watchdog_timeout_(0, 0) 9 | { 10 | tf_ = std::make_shared(this); 11 | declare_parameters(); 12 | } 13 | 14 | StageNode::~StageNode() 15 | { 16 | } 17 | 18 | void StageNode::declare_parameters() 19 | { 20 | this->set_parameter(rclcpp::Parameter("use_sim_time", true)); 21 | auto param_desc_enable_gui = rcl_interfaces::msg::ParameterDescriptor{}; 22 | param_desc_enable_gui.description = "Enable GUI!"; 23 | this->declare_parameter("enable_gui", true, param_desc_enable_gui); 24 | 25 | auto param_desc_model_names = rcl_interfaces::msg::ParameterDescriptor{}; 26 | param_desc_model_names.description = 27 | "USE model name as name space prefix! True on more than one vehicle!"; 28 | this->declare_parameter("use_model_names", false, param_desc_model_names); 29 | 30 | auto param_desc_use_static_transformations_ = rcl_interfaces::msg::ParameterDescriptor{}; 31 | param_desc_use_static_transformations_.description = 32 | "use static transformations for sensor frames!"; 33 | this->declare_parameter( 34 | "use_static_transformations", true, 35 | param_desc_use_static_transformations_); 36 | 37 | auto param_desc_watchdog_timeout = rcl_interfaces::msg::ParameterDescriptor{}; 38 | param_desc_watchdog_timeout.description = 39 | "timeout after which a vehicle stopps if no command is received!"; 40 | this->declare_parameter("base_watchdog_timeout", 5, param_desc_watchdog_timeout); 41 | 42 | auto param_desc_is_depth_canonical = rcl_interfaces::msg::ParameterDescriptor{}; 43 | param_desc_is_depth_canonical.description = "USE depth canonical!"; 44 | this->declare_parameter("is_depth_canonical", true, param_desc_is_depth_canonical); 45 | 46 | auto param_desc_publish_ground_truth = rcl_interfaces::msg::ParameterDescriptor{}; 47 | param_desc_publish_ground_truth.description = "publishes on true a ground truth tf!"; 48 | this->declare_parameter("publish_ground_truth", true, param_desc_publish_ground_truth); 49 | 50 | auto param_desc_world_file = rcl_interfaces::msg::ParameterDescriptor{}; 51 | param_desc_world_file.description = "USE model names!"; 52 | this->declare_parameter("world_file", "cave.world", param_desc_world_file); 53 | 54 | auto param_desc_frame_id_odom_name_ = rcl_interfaces::msg::ParameterDescriptor{}; 55 | param_desc_frame_id_odom_name_.description = 56 | "odom frame name or postfix in case of multiple robots"; 57 | this->declare_parameter("frame_id_odom", "odom", param_desc_frame_id_odom_name_); 58 | 59 | auto param_desc_frame_id_world_name_ = rcl_interfaces::msg::ParameterDescriptor{}; 60 | param_desc_frame_id_world_name_.description = "world frame name for ground truth odom data"; 61 | this->declare_parameter("frame_id_world", "world", param_desc_frame_id_world_name_); 62 | 63 | auto param_desc_frame_id_base_link_name_ = rcl_interfaces::msg::ParameterDescriptor{}; 64 | param_desc_frame_id_base_link_name_.description = 65 | "base link frame name or postfix in case of multiple robots"; 66 | this->declare_parameter( 67 | "frame_id_base_link", "base_link", 68 | param_desc_frame_id_base_link_name_); 69 | } 70 | 71 | void StageNode::update_parameters() 72 | { 73 | double base_watchdog_timeout_sec{5.0}; 74 | this->get_parameter("enable_gui", this->enable_gui_); 75 | this->get_parameter("use_model_names", this->use_model_names); 76 | this->get_parameter("base_watchdog_timeout", base_watchdog_timeout_sec); 77 | this->base_watchdog_timeout_ = rclcpp::Duration::from_seconds(base_watchdog_timeout_sec); 78 | this->get_parameter("is_depth_canonical", this->isDepthCanonical_); 79 | this->get_parameter("publish_ground_truth", this->publish_ground_truth_); 80 | this->get_parameter("frame_id_odom", this->frame_id_odom_name_); 81 | this->get_parameter("frame_id_world", this->frame_id_world_name_); 82 | this->get_parameter("frame_id_base_link", this->frame_id_base_link_name_); 83 | 84 | this->get_parameter("world_file", this->world_file_); 85 | if (!std::filesystem::exists(this->world_file_)) { 86 | RCLCPP_FATAL( 87 | this->get_logger(), "The world file %s does not exist.", 88 | this->world_file_.c_str()); 89 | exit(0); 90 | } 91 | 92 | callback_update_parameters(); 93 | 94 | using namespace std::chrono_literals; 95 | timer_update_parameter_ = 96 | this->create_wall_timer(1000ms, std::bind(&StageNode::callback_update_parameters, this)); 97 | } 98 | 99 | void StageNode::callback_update_parameters() 100 | { 101 | double base_watchdog_timeout_sec; 102 | this->get_parameter("base_watchdog_timeout", base_watchdog_timeout_sec); 103 | this->base_watchdog_timeout_ = rclcpp::Duration::from_seconds(base_watchdog_timeout_sec); 104 | 105 | this->get_parameter("use_static_transformations", use_static_transformations_); 106 | 107 | this->get_parameter("publish_ground_truth", this->publish_ground_truth_); 108 | // RCLCPP_INFO(this->get_logger(), "callback_update_parameter"); 109 | } 110 | 111 | /** 112 | * Is called only ones after the simulation starts with each model 113 | * The function fills the vehicle vector with pointers to the stage models 114 | * @param mod stage model 115 | * @param node pointer to this class 116 | */ 117 | int StageNode::callback_init_stage_model(Stg::Model * mod, StageNode * node) 118 | { 119 | if (dynamic_cast(mod)) { 120 | Stg::ModelPosition * position = dynamic_cast(mod); 121 | RCLCPP_INFO(node->get_logger(), "New Vehicle \"%s\"", mod->TokenStr().c_str()); 122 | auto vehicle = std::make_shared( 123 | node->vehicles_.size(), 124 | position->GetGlobalPose(), mod->TokenStr(), node); 125 | node->vehicles_.push_back(vehicle); 126 | vehicle->positionmodel = position; 127 | } 128 | 129 | if (dynamic_cast(mod)) { 130 | Stg::ModelPosition * parent = dynamic_cast(mod->Parent()); 131 | for (std::shared_ptr vehcile: node->vehicles_) { 132 | if (parent == vehcile->positionmodel) { 133 | auto ranger = 134 | std::make_shared( 135 | vehcile->rangers_.size() + 1, 136 | dynamic_cast(mod), vehcile, node); 137 | vehcile->rangers_.push_back(ranger); 138 | } 139 | } 140 | } 141 | if (dynamic_cast(mod)) { 142 | Stg::ModelPosition * parent = dynamic_cast(mod->Parent()); 143 | for (std::shared_ptr vehcile: node->vehicles_) { 144 | if (parent == vehcile->positionmodel) { 145 | auto camera = 146 | std::make_shared( 147 | vehcile->cameras_.size() + 1, 148 | dynamic_cast(mod), vehcile, node); 149 | vehcile->cameras_.push_back(camera); 150 | } 151 | } 152 | } 153 | return 0; 154 | } 155 | 156 | int StageNode::callback_update_stage_world(Stg::World * world, StageNode * node) 157 | { 158 | // We return false to indicate that we want to be called again (an 159 | // odd convention, but that's the way that Stage works). 160 | if (!rclcpp::ok()) { 161 | RCLCPP_INFO(node->get_logger(), "rclcpp::ok() is false. Quitting."); 162 | node->world->QuitAll(); 163 | return 1; 164 | } 165 | 166 | std::scoped_lock lock(node->msg_lock); 167 | 168 | 169 | node->sim_time_ = rclcpp::Time(world->SimTimeNow() * 1e3); 170 | // We're not allowed to publish clock==0, because it used as a special 171 | // value in parts of ROS, #4027. 172 | if (int(node->sim_time_.nanoseconds()) == 0) { 173 | RCLCPP_DEBUG( 174 | node->get_logger(), "Skipping initial simulation step, to avoid publishing clock==0"); 175 | return 0; 176 | } 177 | // loop on the robot models 178 | for (size_t r = 0; r < node->vehicles_.size(); ++r) { 179 | auto vehicle = node->vehicles_[r]; 180 | vehicle->check_watchdog_timeout(); 181 | vehicle->publish_msg(); 182 | vehicle->publish_tf(); 183 | 184 | // loop on the ranger devices for the current robot 185 | for (auto ranger: vehicle->rangers_) { 186 | ranger->publish_msg(); 187 | ranger->publish_tf(); 188 | } 189 | 190 | 191 | // loop on the camera devices for the current robot 192 | for (auto camera: vehicle->cameras_) { 193 | camera->publish_msg(); 194 | camera->publish_tf(); 195 | } 196 | } 197 | rosgraph_msgs::msg::Clock clock_msg; 198 | clock_msg.clock = node->sim_time_; 199 | node->clock_pub_->publish(clock_msg); 200 | return 0; 201 | } 202 | 203 | bool StageNode::cb_reset_srv( 204 | const std_srvs::srv::Empty::Request::SharedPtr, 205 | std_srvs::srv::Empty::Response::SharedPtr) 206 | { 207 | RCLCPP_INFO(this->get_logger(), "Resetting stage!"); 208 | for (auto vehicle: this->vehicles_) { 209 | vehicle->soft_reset(); 210 | } 211 | return true; 212 | } 213 | 214 | void StageNode::init(int argc, char ** argv) 215 | { 216 | 217 | this->sim_time_ = rclcpp::Time(0, 0); 218 | update_parameters(); 219 | 220 | 221 | // initialize the libstage 222 | Stg::Init(&argc, &argv); 223 | 224 | if (this->enable_gui_) { 225 | this->world = new Stg::WorldGui(600, 400, "Stage (ROS)"); 226 | } else { 227 | this->world = new Stg::World(); 228 | } 229 | 230 | this->world->Load(world_file_.c_str()); 231 | this->world->AddUpdateCallback((Stg::world_callback_t)callback_update_stage_world, this); 232 | this->world->ForEachDescendant((Stg::model_callback_t)callback_init_stage_model, this); 233 | } 234 | 235 | // Subscribe to models of interest. Currently, we find and subscribe 236 | // to the first 'laser' model and the first 'position' model. Returns 237 | // 0 on success (both models subscribed), -1 otherwise. 238 | // 239 | // Eventually, we should provide a general way to map stage models onto ROS 240 | // topics, similar to Player .cfg files. 241 | int StageNode::SubscribeModels() 242 | { 243 | for (std::shared_ptr vehicle: this->vehicles_) { 244 | // init topics and use the stage models names if there are more than one vehicle in the world 245 | vehicle->init(this->use_model_names || (vehicles_.size() > 1)); 246 | } 247 | 248 | // create the clock publisher 249 | clock_pub_ = this->create_publisher("/clock", 10); 250 | 251 | // advertising reset service 252 | srv_reset_ = this->create_service( 253 | "reset_positions", 254 | [this](const std_srvs::srv::Empty::Request::SharedPtr request, 255 | std_srvs::srv::Empty::Response::SharedPtr response) 256 | {this->cb_reset_srv(request, response);}); 257 | 258 | return 0; 259 | } 260 | 261 | bool StageNode::UpdateWorld() 262 | { 263 | return this->world->UpdateAll(); 264 | } 265 | 266 | // helper functions 267 | geometry_msgs::msg::TransformStamped StageNode::create_transform_stamped( 268 | const tf2::Transform & in, 269 | const rclcpp::Time & timestamp, const std::string & frame_id, const std::string & child_frame_id) 270 | { 271 | geometry_msgs::msg::TransformStamped out; 272 | out.header.stamp = timestamp; 273 | out.header.frame_id = frame_id; 274 | out.child_frame_id = child_frame_id; 275 | out.transform.translation.x = in.getOrigin().getX(); 276 | out.transform.translation.y = in.getOrigin().getY(); 277 | out.transform.translation.z = in.getOrigin().getZ(); 278 | out.transform.rotation.w = in.getRotation().getW(); 279 | out.transform.rotation.x = in.getRotation().getX(); 280 | out.transform.rotation.y = in.getRotation().getY(); 281 | out.transform.rotation.z = in.getRotation().getZ(); 282 | return out; 283 | } 284 | 285 | geometry_msgs::msg::Quaternion StageNode::createQuaternionMsgFromYaw(double yaw) 286 | { 287 | tf2::Quaternion q; 288 | q.setRPY(0, 0, yaw); 289 | return tf2::toMsg(q); 290 | } 291 | -------------------------------------------------------------------------------- /src/vehicle.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #define TOPIC_ODOM "odom" 8 | #define TOPIC_GROUND_TRUTH "ground_truth" 9 | #define TOPIC_CMD_VEL "cmd_vel" 10 | 11 | using std::placeholders::_1; 12 | 13 | StageNode::Vehicle::Vehicle( 14 | size_t id, const Stg::Pose & pose, const std::string & name, 15 | StageNode * node) 16 | : id_(id), initial_pose_(pose), name_(name), node_(node) 17 | { 18 | tf_static_broadcaster_ = std::make_shared(node_); 19 | tf_broadcaster_ = std::make_shared(node_); 20 | time_last_pose_update_ = rclcpp::Time(0, 0); 21 | time_last_cmd_received_ = rclcpp::Time(0, 0); 22 | timeout_cmd_ = rclcpp::Time(0, 0); 23 | } 24 | 25 | size_t StageNode::Vehicle::id() const 26 | { 27 | return id_; 28 | } 29 | void StageNode::Vehicle::soft_reset() 30 | { 31 | positionmodel->SetPose(this->initial_pose_); 32 | positionmodel->SetStall(false); 33 | } 34 | 35 | const std::string & StageNode::Vehicle::name() const 36 | { 37 | return name_; 38 | } 39 | 40 | void StageNode::Vehicle::init(bool use_model_name) 41 | { 42 | name_space_ = std::string(); 43 | if (use_model_name) {name_space_ = name() + "/";} 44 | frame_id_base_link_ = name_space_ + node_->frame_id_base_link_name_; 45 | frame_id_odom_ = name_space_ + node_->frame_id_odom_name_; 46 | frame_id_world_ = name_space_ + node_->frame_id_world_name_; 47 | 48 | topic_name_odom_ = name_space_ + TOPIC_ODOM; 49 | topic_name_ground_truth_ = name_space_ + TOPIC_GROUND_TRUTH; 50 | topic_name_cmd_ = name_space_ + TOPIC_CMD_VEL; 51 | 52 | pub_odom_ = node_->create_publisher(topic_name_odom_, 10); 53 | pub_ground_truth_ = 54 | node_->create_publisher(topic_name_ground_truth_, 10); 55 | sub_cmd_ = 56 | node_->create_subscription( 57 | topic_name_cmd_, 10, 58 | std::bind(&StageNode::Vehicle::callback_cmd, this, _1)); 59 | 60 | positionmodel->Subscribe(); 61 | 62 | for (std::shared_ptr ranger: rangers_) { 63 | ranger->init(rangers_.size() > 1); 64 | } 65 | 66 | for (std::shared_ptr camera: cameras_) { 67 | camera->init(rangers_.size() > 1); 68 | } 69 | } 70 | 71 | void StageNode::Vehicle::publish_msg() 72 | { 73 | 74 | // Get latest odometry data 75 | // Translate into ROS message format and publish 76 | msg_odom_.pose.pose.position.x = positionmodel->est_pose.x; 77 | msg_odom_.pose.pose.position.y = positionmodel->est_pose.y; 78 | msg_odom_.pose.pose.orientation = createQuaternionMsgFromYaw(positionmodel->est_pose.a); 79 | Stg::Velocity v = positionmodel->GetVelocity(); 80 | msg_odom_.twist.twist.linear.x = v.x; 81 | msg_odom_.twist.twist.linear.y = v.y; 82 | msg_odom_.twist.twist.angular.z = v.a; 83 | msg_odom_.header.frame_id = frame_id_odom_; 84 | msg_odom_.header.stamp = node_->sim_time_; 85 | msg_odom_.child_frame_id = frame_id_base_link_; 86 | 87 | pub_odom_->publish(msg_odom_); 88 | 89 | // Also publish the ground truth pose and velocity 90 | Stg::Pose gpose = positionmodel->GetGlobalPose(); 91 | tf2::Quaternion q_gpose; 92 | q_gpose.setRPY(0.0, 0.0, gpose.a); 93 | tf2::Transform gt(q_gpose, tf2::Vector3(gpose.x, gpose.y, 0.0)); 94 | // Velocity is 0 by default and will be set only if there is previous pose and time delta>0 95 | // @ToDo uising the positionmodel->GetVelocity() a self computed delta 96 | Stg::Velocity gvel(0, 0, 0, 0); 97 | if (global_pose_) { 98 | double dT = (node_->sim_time_ - time_last_pose_update_).seconds(); 99 | if (dT > 0) { 100 | gvel = Stg::Velocity( 101 | (gpose.x - global_pose_->x) / dT, 102 | (gpose.y - global_pose_->y) / dT, 103 | (gpose.z - global_pose_->z) / dT, 104 | Stg::normalize(gpose.a - global_pose_->a) / dT); 105 | } 106 | *global_pose_ = gpose; 107 | } else { 108 | // There are no previous readings, adding current pose... 109 | global_pose_ = std::make_shared(gpose); 110 | } 111 | nav_msgs::msg::Odometry ground_truth_msg; 112 | ground_truth_msg.pose.pose.position.x = gt.getOrigin().x(); 113 | ground_truth_msg.pose.pose.position.y = gt.getOrigin().y(); 114 | ground_truth_msg.pose.pose.position.z = gt.getOrigin().z(); 115 | ground_truth_msg.pose.pose.orientation.x = gt.getRotation().x(); 116 | ground_truth_msg.pose.pose.orientation.y = gt.getRotation().y(); 117 | ground_truth_msg.pose.pose.orientation.z = gt.getRotation().z(); 118 | ground_truth_msg.pose.pose.orientation.w = gt.getRotation().w(); 119 | ground_truth_msg.twist.twist.linear.x = gvel.x; 120 | ground_truth_msg.twist.twist.linear.y = gvel.y; 121 | ground_truth_msg.twist.twist.linear.z = gvel.z; 122 | ground_truth_msg.twist.twist.angular.z = gvel.a; 123 | 124 | ground_truth_msg.header.frame_id = frame_id_world_; 125 | ground_truth_msg.header.stamp = node_->sim_time_; 126 | 127 | pub_ground_truth_->publish(ground_truth_msg); 128 | time_last_pose_update_ = node_->sim_time_; 129 | 130 | } 131 | void StageNode::Vehicle::publish_tf() 132 | { 133 | 134 | // broadcast odometry transform 135 | tf2::Quaternion quaternion = tf2::Quaternion( 136 | msg_odom_.pose.pose.orientation.x, 137 | msg_odom_.pose.pose.orientation.y, 138 | msg_odom_.pose.pose.orientation.z, 139 | msg_odom_.pose.pose.orientation.w); 140 | tf2::Transform transform(quaternion, 141 | tf2::Vector3(msg_odom_.pose.pose.position.x, msg_odom_.pose.pose.position.y, 0.0)); 142 | node_->tf_->sendTransform( 143 | create_transform_stamped( 144 | transform, node_->sim_time_, 145 | frame_id_odom_, 146 | frame_id_base_link_)); 147 | } 148 | 149 | void StageNode::Vehicle::check_watchdog_timeout() 150 | { 151 | 152 | if ((timeout_cmd_ != rclcpp::Time(0, 0)) && (node_->sim_time_ > timeout_cmd_)) { 153 | Stg::Velocity v = positionmodel->GetVelocity(); 154 | // stopping makes only sense if the vehicle drives 155 | if (!positionmodel->GetVelocity().IsZero()) { 156 | this->positionmodel->SetSpeed(0.0, 0.0, 0.0); 157 | RCLCPP_INFO(node_->get_logger(), "watchdog timeout on %s", name().c_str()); 158 | } 159 | } 160 | } 161 | void StageNode::Vehicle::callback_cmd(const geometry_msgs::msg::Twist::SharedPtr msg) 162 | { 163 | std::scoped_lock lock(node_->msg_lock); 164 | this->positionmodel->SetSpeed( 165 | msg->linear.x, 166 | msg->linear.y, 167 | msg->angular.z); 168 | time_last_cmd_received_ = node_->sim_time_; 169 | timeout_cmd_ = time_last_cmd_received_ + node_->base_watchdog_timeout_; 170 | 171 | } 172 | -------------------------------------------------------------------------------- /world/maps/autolab.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/damuxt/stage_ros2/7860df3ee160575338d3449787eb731c0d663672/world/maps/autolab.png -------------------------------------------------------------------------------- /world/maps/cave.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/damuxt/stage_ros2/7860df3ee160575338d3449787eb731c0d663672/world/maps/cave.png -------------------------------------------------------------------------------- /world/maps/frieburg.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/damuxt/stage_ros2/7860df3ee160575338d3449787eb731c0d663672/world/maps/frieburg.png -------------------------------------------------------------------------------- /world/maps/ghost.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/damuxt/stage_ros2/7860df3ee160575338d3449787eb731c0d663672/world/maps/ghost.png -------------------------------------------------------------------------------- /world/maps/hospital_section.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/damuxt/stage_ros2/7860df3ee160575338d3449787eb731c0d663672/world/maps/hospital_section.png -------------------------------------------------------------------------------- /world/maps/human_outline.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/damuxt/stage_ros2/7860df3ee160575338d3449787eb731c0d663672/world/maps/human_outline.png -------------------------------------------------------------------------------- /world/maps/line.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/damuxt/stage_ros2/7860df3ee160575338d3449787eb731c0d663672/world/maps/line.png -------------------------------------------------------------------------------- /world/maps/my_house.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/damuxt/stage_ros2/7860df3ee160575338d3449787eb731c0d663672/world/maps/my_house.jpg -------------------------------------------------------------------------------- /world/maps/simple_rooms.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/damuxt/stage_ros2/7860df3ee160575338d3449787eb731c0d663672/world/maps/simple_rooms.png -------------------------------------------------------------------------------- /world/maps/space_invader.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/damuxt/stage_ros2/7860df3ee160575338d3449787eb731c0d663672/world/maps/space_invader.png -------------------------------------------------------------------------------- /world/maps/uoa_robotics_lab.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/damuxt/stage_ros2/7860df3ee160575338d3449787eb731c0d663672/world/maps/uoa_robotics_lab.png -------------------------------------------------------------------------------- /world/my_house.world: -------------------------------------------------------------------------------- 1 | 2 | # ----------------------------------------------------------------------------- 3 | # 设置窗体 4 | # 设置模拟器地图分辨率(以 米/像素 为单位) 5 | resolution 0.02 6 | # 设置模拟器的时间步长(以 毫秒 为单位) 7 | interval_sim 100 8 | 9 | # 配置窗体参数f 10 | window 11 | ( 12 | size [ 635.000 666.000 ] # 窗体尺寸(以 像素 为单位) 13 | scale 35 # 缩放比 14 | center [ 0 0 ] # 地图相对于窗体的偏移量(以 米 为单位) 15 | rotate [ 0 0 ] # 地图旋转角度 16 | 17 | show_data 1 # 是否显示传感器数据 1=on 0=off 18 | ) 19 | 20 | # ----------------------------------------------------------------------------- 21 | # 设置地图(定义模型) 22 | define floorplan model 23 | ( 24 | color "gray30" # 颜色 25 | boundary 1 # 为地图设置边框 26 | 27 | gui_nose 0 28 | gui_grid 0 29 | 30 | gui_outline 0 31 | gripper_return 0 32 | fiducial_return 0 33 | laser_return 1 34 | ) 35 | 36 | # 加载地图 37 | floorplan 38 | ( 39 | name "my_house" 40 | size [16.000 16.000 0.800] 41 | pose [0 0 0 0] 42 | bitmap "maps/my_house.jpg" 43 | gui_move 0 44 | ) 45 | 46 | # ----------------------------------------------------------------------------- 47 | # 添加障碍物 48 | # define a block 49 | define my_block model 50 | ( 51 | size [1.0 1.0 1.0] 52 | gui_nose 0 53 | gui_grid 0 54 | gui_outline 0 55 | ) 56 | 57 | # throw in a block 58 | my_block( pose [ 2 -6 0 0 ] color "green") 59 | my_block( pose [ 5 0 0 360 ] color "red" bitmap "maps/ghost.png") 60 | 61 | 62 | # ----------------------------------------------------------------------------- 63 | # 生成机器人 64 | # 文件包含 65 | include "robot/mycar.inc" 66 | 67 | car_base_with_laser_camera( 68 | name "robot_0" 69 | color "red" 70 | pose [ -7 -7 0 45 ] 71 | ) 72 | -------------------------------------------------------------------------------- /world/my_house_multi.world: -------------------------------------------------------------------------------- 1 | 2 | # ----------------------------------------------------------------------------- 3 | # 设置窗体 4 | # 设置模拟器地图分辨率(以 米/像素 为单位) 5 | resolution 0.02 6 | # 设置模拟器的时间步长(以 毫秒 为单位) 7 | interval_sim 100 8 | 9 | # 配置窗体参数f 10 | window 11 | ( 12 | size [ 635.000 666.000 ] # 窗体尺寸(以 像素 为单位) 13 | scale 35 # 缩放比 14 | center [ 0 0 ] # 地图相对于窗体的偏移量(以 米 为单位) 15 | rotate [ 0 0 ] # 地图旋转角度 16 | 17 | show_data 1 # 是否显示传感器数据 1=on 0=off 18 | ) 19 | 20 | # ----------------------------------------------------------------------------- 21 | # 设置地图(定义模型) 22 | define floorplan model 23 | ( 24 | color "gray30" # 颜色 25 | boundary 1 # 为地图设置边框 26 | 27 | gui_nose 0 28 | gui_grid 0 29 | 30 | gui_outline 0 31 | gripper_return 0 32 | fiducial_return 0 33 | laser_return 1 34 | ) 35 | 36 | # 加载地图 37 | floorplan 38 | ( 39 | name "my_house" 40 | size [16.000 16.000 0.800] 41 | pose [0 0 0 0] 42 | bitmap "maps/my_house.jpg" 43 | gui_move 0 44 | ) 45 | 46 | # ----------------------------------------------------------------------------- 47 | # 添加障碍物 48 | # define a block 49 | define my_block model 50 | ( 51 | size [1.0 1.0 1.0] 52 | gui_nose 0 53 | gui_grid 0 54 | gui_outline 0 55 | ) 56 | 57 | # throw in a block 58 | my_block( pose [ 2 -6 0 0 ] color "green") 59 | my_block( pose [ 5 0 0 360 ] color "red" bitmap "maps/ghost.png") 60 | 61 | 62 | # ----------------------------------------------------------------------------- 63 | # 生成机器人 64 | # 文件包含 65 | include "robot/mycar.inc" 66 | 67 | car_base_with_laser_camera( 68 | name "robot_0" 69 | color "red" 70 | pose [ -7 -7 0 45 ] 71 | ) 72 | 73 | car_base_with_laser_camera( 74 | name "robot_1" 75 | color "yellow" 76 | pose [ -5 -5 0 0 ] 77 | ) -------------------------------------------------------------------------------- /world/robot/camera.inc: -------------------------------------------------------------------------------- 1 | define my_camera camera 2 | ( 3 | range [ 0.3 3.0 ] # 相机采样范围 4 | resolution [ 160 90 ] #相机分辨率 1280 × 720 / 8 5 | fov [ 87 58 ] # 相机视场 6 | pantilt [ 0 0 ] # 相机姿态 7 | alwayson 1 # 是否一直处于启动状态 8 | size [ 0.025 0.09 0.025 ] # 相机尺寸 9 | color "gray" # 相机颜色 10 | ) -------------------------------------------------------------------------------- /world/robot/car_base.inc: -------------------------------------------------------------------------------- 1 | # 机器人底盘配置 2 | define car_base position 3 | ( 4 | color "red" # 车身颜色 5 | drive "diff" # 车辆运动学模型 6 | gui_nose 1 7 | obstacle_return 1 8 | ranger_return 0.5 9 | blob_return 1 10 | fiducial_return 1 11 | 12 | localization "odom" # 定位方式 13 | odom_error [ 0.05 0.05 0.0 0.1 ] # 里程计误差 14 | # localization_origin [0 0 0 0] # 定位原点，默认为机器人的初始位置。 15 | 16 | # [ xmin xmax ymin ymax zmin zmax amin amax ] 17 | velocity_bounds [-1 1 0 0 0 0 -45.0 45.0 ] # 速度最值 18 | acceleration_bounds [-0.5 0.5 0 0 0 0 -45 45.0 ] # 加速度最值 19 | 20 | 21 | size [0.44 0.38 0.22] # 车体尺寸 22 | origin [0 0 0 0] # 旋转中心与车体中心的偏移量 23 | mass 23.0 # 车体质量，单位时 kg 24 | 25 | gui_nose 0 # 是否绘制方向指示标记 26 | 27 | 28 | # 车身形状 29 | #block( 30 | # points 8 31 | # point[0] [-0.2 0.12] 32 | # point[1] [-0.2 -0.12] 33 | # point[2] [-0.12 -0.2555] 34 | # point[3] [0.12 -0.2555] 35 | # point[4] [0.2 -0.12] 36 | # point[5] [0.2 0.12] 37 | # point[6] [0.12 0.2555] 38 | # point[7] [-0.12 0.2555] 39 | # z [0 0.22] 40 | #) 41 | 42 | block( 43 | points 8 44 | point[0] [-0.2 0.18] 45 | point[1] [-0.2 -0.18] 46 | point[2] [-0.15 -0.27] 47 | point[3] [0.12 -0.23] 48 | point[4] [0.2 -0.12] 49 | point[5] [0.2 0.12] 50 | point[6] [0.12 0.23] 51 | point[7] [-0.15 0.27] 52 | z [0 0.22] 53 | ) 54 | ) 55 | -------------------------------------------------------------------------------- /world/robot/laser.inc: -------------------------------------------------------------------------------- 1 | define my_laser ranger 2 | ( 3 | sensor( 4 | pose [ 0 0 0 0 ] 5 | size [ 0.07 0.07 0.05 ] 6 | range [ 0.0 15.0 ] # 雷达数据采集区间 7 | fov 360.0 # 视角 8 | samples 720 # 采样数 9 | color_rgba [ 0 0 1 0.15 ] # 可视化光束颜色以及透明度 10 | ) 11 | model # 雷达外观 12 | ( 13 | pose [ 0 0 0 0 ] # 雷达位姿 14 | size [ 0.07 0.07 0.05 ] # 雷达尺寸信息 15 | color "blue" # 雷达颜色 16 | ) 17 | ) -------------------------------------------------------------------------------- /world/robot/mycar.inc: -------------------------------------------------------------------------------- 1 | # 车辆底盘与传感器集成 2 | 3 | include "robot/car_base.inc" 4 | include "robot/laser.inc" 5 | include "robot/camera.inc" 6 | 7 | # 底盘集成单个雷达 8 | define car_base_with_laser car_base 9 | ( 10 | my_laser() 11 | ) 12 | # 底盘集成单个摄像头 13 | define car_base_with_camera car_base 14 | ( 15 | my_camera() 16 | ) 17 | # 底盘集成两个雷达 18 | define car_base_with_two_laser car_base 19 | ( 20 | my_laser(pose [0.15 0 0 0]) 21 | my_laser(pose [-0.15 0 0 -180]) 22 | ) 23 | # 底盘集成一个雷达，一个摄像头 24 | define car_base_with_laser_camera car_base 25 | ( 26 | my_laser() 27 | my_camera(pose [0.15 0 0 0]) 28 | ) 29 | --------------------------------------------------------------------------------