├── README.md ├── TEACHERS.md ├── ex01_first_package ├── CMakeLists.txt ├── README.md ├── dat │ └── radio_ros_simple.png ├── include │ └── ex01_first_package │ │ └── my_publisher.hpp ├── package.xml └── src │ ├── my_publisher.cpp │ ├── publisher.cpp │ ├── publisher_class.cpp │ ├── publisher_lib.cpp │ ├── subscriber.cpp │ ├── subscriber_class.cpp │ └── subscriber_lambda.cpp ├── ex02_gazebo_simulation ├── CMakeLists.txt ├── README.md ├── package.xml └── src │ ├── scan_to_pcl.cpp │ └── transform_pcl.cpp ├── ex03_state_estimation ├── CMakeLists.txt ├── README.md └── package.xml ├── ex04_navigation ├── README.md ├── dat │ ├── action_server_client.png │ ├── amcl.png │ └── nav2.png ├── ex04_actions │ ├── CMakeLists.txt │ ├── package.xml │ └── src │ │ ├── wait_for_rviz_pose_action_server.cpp │ │ ├── wait_for_x_action_server.cpp │ │ ├── wait_for_x_client.cpp │ │ └── wait_for_x_client_sync.cpp ├── ex04_msgs │ ├── CMakeLists.txt │ ├── action │ │ ├── WaitForRvizPose.action │ │ └── WaitForX.action │ └── package.xml └── ex04_nav2_client │ ├── CMakeLists.txt │ ├── package.xml │ └── src │ └── nav2_client.cpp ├── ex05_behavior_trees ├── README.md ├── dat │ └── smach.png ├── ex05_behavior_trees │ ├── CMakeLists.txt │ ├── include │ │ └── ex05_behavior_trees │ │ │ └── dummy_nodes.hpp │ ├── package.xml │ └── src │ │ ├── bt_dummy.cpp │ │ ├── bt_dummy_groot.cpp │ │ └── lib │ │ └── dummy_nodes.cpp └── ex05_bt_plugins │ ├── CMakeLists.txt │ ├── ex05_bt_plugins.xml │ ├── include │ └── ex05_bt_plugins │ │ └── dummy_nodes.hpp │ ├── package.xml │ └── src │ └── dummy_nodes.cpp └── media └── ceres_vid_preview.png /README.md: -------------------------------------------------------------------------------- 1 | # ros2_tutorial 2 | 3 | ROS2 humble - Tutorials for the [KBS](https://kbs.informatik.uos.de/) robotics labs in wintersemester 2023/24; Osnabrück University. A computer is needed with: 4 | 5 | - Ubuntu 22. Only native! Dual-boot is OK. 6 | - ROS2 humble (see first tutorial) 7 | 8 | All examples are implemented using C++ programming language. The course is at a very beginner level. In the end you will be able to let a robot autonomously drive from A to B as shown here: 9 | 10 | [![Ceres Robot Nav](media/ceres_vid_preview.png)](https://www.youtube.com/watch?v=zTn5MSbwYQ4) 11 | 12 | ## [ex01_first_package](./ex01_first_package/) 13 | 14 | - Learning ROS concepts 15 | - Writing a simple subscriber and publisher 16 | - Learning C++ and CMake by implementing subscribers and publishers in different ways 17 | - Learn to use `vim` 18 | - Environment variables, `.bashrc` 19 | 20 | ## [ex02_gazebo_simulation](./ex02_gazebo_simulation/) 21 | 22 | - Start a robot! Virtually 23 | - More ROS packaging 24 | - RViz2 25 | - New message: LaserScan 26 | - Transformations: Concept, Implementation 27 | - New message: PointCloud2 28 | - ROS2 parameters + remapping 29 | - XML launch files 30 | - Fun: Steer the robot 31 | - New message: Twist 32 | 33 | ## [ex03_state_estimation](./ex03_state_estimation/) 34 | 35 | - Concept: State Estimation via 36 | - Wheel Odometry 37 | - IMU 38 | - LiDAR 39 | - Camera 40 | - Fusion 41 | - Kalman-Filter 42 | - `robot_localization`-package 43 | - Start a robot! In reality. 44 | - Tool: `tmux` 45 | - Mapping 46 | - Maps 47 | - SLAM 48 | 49 | ## [ex04_navigation](./ex04_navigation/) 50 | 51 | - Localization in given maps 52 | - Nav2 53 | - Actions: Interfaces, Servers, Clients 54 | - Trigger Nav2 action servers via C++-Nodes 55 | 56 | ## [ex05_behavior_trees](./ex05_behavior_trees/) 57 | 58 | !! Unfinished !! 59 | 60 | - Overview: Deliberation Layer 61 | - BehaviorTree.CPP 62 | - Simple Behavior Tree 63 | - Simple Behavior Tree with Groot monitor 64 | - Writing Behavior Tree Plugins 65 | 66 | TODOs: 67 | 68 | - Editing/creating trees using Groot 69 | - Using the Blackboard 70 | 71 | More TODOs in ex05 README. 72 | 73 | 74 | ## Information for Teachers 75 | 76 | Further information for teachers: [Click here](TEACHERS.md). -------------------------------------------------------------------------------- /TEACHERS.md: -------------------------------------------------------------------------------- 1 | # Information for Teachers 2 | 3 | When the students reached certain points, I explained the theory and some practical things in more detail on the blackboard. This included, for example: 4 | 5 | - Introductions 6 | - Components of a robot: sensors, motors, motor-controller 7 | - Transformations (A lot) 8 | - Kalman-Filter (beginning with 1D example) 9 | - SLAM pose graph incl. different uncertainties for linear and rotational motion 10 | - MCL, kidnapped robot problem, multimodal distributions 11 | - Nav2, layers, search algorithms, ... 12 | 13 | Furthermore, I am aware of more nice ROS tools that help to finish things faster. However, I did not mention them because the students had to learn enough commands anyways. 14 | 15 | These tutorials were tested by 12 undergraduate students of Osnabrück Univertity. They were in their 3rd or 5th semester and did not have much experience with C++, nor with CMake, or Linux. It took them 6 days to finish with `ex04_navigation`. They then had to work on a project for about 1.5 weeks and give a 30-minute presentation including a demonstration at the end. The remaining days were spent on documentation. In total, it took 3 weeks full-time. 16 | -------------------------------------------------------------------------------- /ex01_first_package/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(ex01_first_package) 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(std_msgs REQUIRED) 16 | 17 | add_executable(publisher 18 | src/publisher.cpp) 19 | ament_target_dependencies(publisher 20 | rclcpp 21 | std_msgs) 22 | 23 | add_executable(publisher_class 24 | src/publisher_class.cpp) 25 | ament_target_dependencies(publisher_class 26 | rclcpp 27 | std_msgs) 28 | 29 | 30 | add_executable(subscriber 31 | src/subscriber.cpp) 32 | ament_target_dependencies(subscriber 33 | rclcpp 34 | std_msgs) 35 | 36 | add_executable(subscriber_lambda 37 | src/subscriber_lambda.cpp) 38 | ament_target_dependencies(subscriber_lambda 39 | rclcpp 40 | std_msgs) 41 | 42 | add_executable(subscriber_class 43 | src/subscriber_class.cpp) 44 | ament_target_dependencies(subscriber_class 45 | rclcpp 46 | std_msgs) 47 | 48 | install(TARGETS 49 | publisher 50 | publisher_class 51 | subscriber 52 | subscriber_lambda 53 | subscriber_class 54 | DESTINATION lib/${PROJECT_NAME}) 55 | 56 | 57 | 58 | ########### 59 | ## ADDITIONAL: EXAMPLE FOR PUBLISHER NODE 60 | ## - Header-File, Source-File, Main-Function-File 61 | ## Create a library called 'my_publisher' 62 | ## Use the library in 'publisher_lib' 63 | 64 | ## 1. LIBRARY 65 | add_library(my_publisher 66 | src/my_publisher.cpp 67 | ) 68 | 69 | target_include_directories(my_publisher PUBLIC 70 | "$" 71 | "$" 72 | ) 73 | 74 | ament_target_dependencies(my_publisher 75 | rclcpp 76 | std_msgs 77 | ) 78 | 79 | install(TARGETS my_publisher 80 | ARCHIVE DESTINATION lib/${PROJECT_NAME} 81 | LIBRARY DESTINATION lib/${PROJECT_NAME} 82 | RUNTIME DESTINATION bin/${PROJECT_NAME} 83 | ) 84 | 85 | ## 2. Node that uses library 86 | ## Node knows only the header file and the pre-compiled 87 | ## library code of my_publisher 88 | add_executable(publisher_lib 89 | src/publisher_lib.cpp 90 | ) 91 | 92 | target_link_libraries(publisher_lib 93 | my_publisher 94 | ) 95 | 96 | ament_target_dependencies(publisher_lib 97 | rclcpp 98 | ) 99 | 100 | install(TARGETS 101 | publisher_lib 102 | DESTINATION lib/${PROJECT_NAME}) 103 | 104 | ament_package() -------------------------------------------------------------------------------- /ex01_first_package/README.md: -------------------------------------------------------------------------------- 1 | # ex01_first_package 2 | 3 | ROS is short for Robot Operating System. It is a framework/middleware for the quick development of software related to robotics. 4 | It provides the infrastructure for sensor and actuator communications, sensor data handling, serialization, visualizations, and software packaging. 5 | All these things don't change for different types of robots. 6 | Roboticists are happy 7 | - to not implement everything from scratch for every robot they have, 8 | - that they can use other software that was already implemented somewhere in the world. 9 | 10 | ROS1 was invented in 2007. It was mostly used in research and is completely open-source. Some design choices were considered bad. In addition, it had no guarantees in real-time executions nor in how safe communications were. Recently ROS2 was released in 2017, seeking to overcome those and other issues. 11 | 12 | With the following set of repositories, we try to explain the basic ROS concepts by example. 13 | 14 | ## ROS2 15 | 16 | To continue a computer with natively installed Ubuntu 22.04 is required. Come back if you find one. 17 | 18 | Great to see you have found one. Now install ROS2 humble with the following steps (official instructions: https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) 19 | 20 | Open a terminal: 21 | 22 | ### Useful packages 23 | 24 | ``` 25 | sudo apt-get install vim 26 | ``` 27 | 28 | ### Preparation 29 | 30 | ```console 31 | locale # check for UTF-8 32 | 33 | sudo apt update && sudo apt install locales 34 | sudo locale-gen en_US en_US.UTF-8 35 | sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 36 | export LANG=en_US.UTF-8 37 | 38 | locale # verify settings 39 | ``` 40 | 41 | ```console 42 | sudo apt install software-properties-common 43 | sudo add-apt-repository universe 44 | ``` 45 | 46 | ```console 47 | sudo apt update && sudo apt install curl -y 48 | sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg 49 | ``` 50 | 51 | ```console 52 | echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null 53 | ``` 54 | 55 | Refresh the package sources 56 | 57 | ```console 58 | sudo apt update 59 | ``` 60 | 61 | Upgrade all packages you already have 62 | 63 | ```console 64 | sudo apt upgrade 65 | ``` 66 | 67 | ### Installation 68 | 69 | Now start the ROS2-humble installation: 70 | 71 | ```console 72 | sudo apt install ros-humble-desktop 73 | ``` 74 | 75 | Some additional dev tools: 76 | ```console 77 | sudo apt install ros-dev-tools 78 | ``` 79 | 80 | ### Environment Variables 81 | 82 | In many situations, ROS2 will need additional configuration via setting environment variables. A good way is to add additional environment variables to the `~/.bashrc` file (`~` is the path to your home folder). Then they will be set every time a new terminal is started. Open the file with Vim (yes with vim) and add the following line to the end of the file: 83 | 84 | ```console 85 | vi ~/.bashrc 86 | ``` 87 | 88 | Try to insert this at the end of the file: 89 | 90 | ```console 91 | source /opt/ros/humble/setup.bash 92 | ``` 93 | 94 | Exit Vim: https://stackoverflow.com/questions/11828270/how-do-i-exit-vim. The sooner you learn Vim the better. As soon as you restart your terminal this command will set up everything you will need for your global ROS2. Test your installation via 95 | 96 | ```console 97 | ros2 run demo_nodes_cpp talker 98 | ``` 99 | 100 | open another terminal (Ctrl+Shift+T) and execute: 101 | 102 | ```console 103 | ros2 run demo_nodes_py listener 104 | ``` 105 | 106 | The listener should hear what the talker is saying, even though they are not the same process, and not written in the same programming language. How this works comes soon. 107 | 108 | ## Package 109 | 110 | A package is a piece of software that has a closed purpose. For example, one package could be a driver for a sensor, another package could handle the detection of objects in images. Now you will create your very first package. But where to put it? Into a ROS workspace. 111 | 112 | It doesn't matter how you call it. We mostly name the workspace after the robots we are using. Since we are later using the "ceres" robot we are calling the workspace `ceres_ws`: 113 | 114 | ``` 115 | user@pc:~$ mkdir ceres_ws 116 | ``` 117 | 118 | all packages will be placed within a `src` folder inside of this package: 119 | 120 | ``` 121 | ceres_ws/ 122 | src/ 123 | package1/ 124 | package.xml 125 | CMakeLists.txt 126 | ... 127 | package2/ 128 | package.xml 129 | ... 130 | folder/ 131 | package3/ 132 | package.xml 133 | CMakeLists.txt 134 | ... 135 | package4/ 136 | package.xml 137 | ... 138 | ... 139 | ``` 140 | 141 | 142 | ```console 143 | user@pc:~$ cd ceres_ws 144 | user@pc:~/ceres_ws$ mkdir src 145 | ``` 146 | 147 | Source your new workspace by adding the following line to the end of your `.bashrc`: 148 | 149 | ``` 150 | source ~/ceres_ws/install/setup.bash 151 | ``` 152 | 153 | Then create a package: 154 | 155 | ```console 156 | user@pc:~/ceres_ws$ cd src 157 | user@pc:~/ceres_ws/src$ mkdir ex01_first_package 158 | user@pc:~/ceres_ws/src$ cd ex01_first_package 159 | ``` 160 | 161 | A folder becomes a ROS package once you put a package.xml into it. 162 | 163 | ```console 164 | user@pc:~/ceres_ws/src/ex01_first_package$ touch package.xml 165 | ``` 166 | 167 | ```console 168 | user@pc:~/ceres_ws/src/ex01_first_package$ vi package.xml 169 | ``` 170 | 171 | Open the `package.xml` file of this repository to see its contents. 172 | 173 | Everything until `license` are tags providing meta-information about the package. 174 | The rest describes what the package needs to compile successfully. 175 | The `rclcpp` package contains everything we need to write C++ code for a robot. 176 | So if we decide to write our software in C++, we usually depend on `rclcpp`. 177 | The second package is `std_msgs`. 178 | It is a package that only consists of message definitions. 179 | 180 | ## Messages & Topics 181 | 182 | In ROS-Messages we can store and transfer data to other nodes. 183 | For example, the message `std_msgs/msg/String` is defined here. 184 | 185 | It is a rather simple message because it only contains one field. 186 | The description of a message is compiled into types that can be used in any supported programming language. 187 | Like this, we can use the same message in C++ or Python and communicate between programs written in Python and C++. 188 | For example, we can write something into the message using C++ and send it 189 | 190 | ```cpp 191 | #include 192 | 193 | ... 194 | 195 | std_msgs::msg::String message; 196 | message.data = "Hello!"; 197 | // send message ... (publisher) 198 | ``` 199 | 200 | and read the data using Python: 201 | 202 | ```python 203 | import std_msgs.msg.String 204 | 205 | ... 206 | 207 | msg = std_msgs.msg.String() 208 | # receive message ... (subscriber) 209 | print(msg.data) 210 | ``` 211 | 212 | Communications between nodes are not done directly but with so-called ROS-topics in the middle. 213 | So a `Publisher` is publishing a message on a topic. One ore multiple nodes can subscribe on the topic to receive the ROS-Message that was published on the topic before. This scheme is exceptionally well suited for sensor data: 214 | An example from reality: ROS-sensor drivers usually access the sensor on low-level and then publish the sensor data to a topic. 215 | For example, a USB webcam driver is accessing the USB interface of the camera and reads the images. The driver doesn't care about *who* exactly needs the images, so it just publishes the image data to a topic. 216 | Then one node wants to detect cats in the images, so it subscribes on the topic, another node wants to filter the image so that only edges remain, so it subscribes on the topic. Maybe in the future, someone implements some new nodes doing cool things with the images, so they will subscribe to the image topic. 217 | But the image driver never needs to be changed for that. 218 | And on the other side, the image processing nodes will never know there is a USB-interface existing. 219 | 220 | Another example is a Radio station where someone is broadcasting something without caring about who is actually receiving it: 221 | 222 | ![radio-ros](./dat/radio_ros_simple.png) 223 | 224 | ### Publisher 225 | 226 | The next task deals with implementing our own publisher node for our package. It should publish a `std_msgs/String` on a topic called `important_messages`. So the Node acts similarly to the sensor-driver-Node described before (except accessing a USB-interface). 227 | 228 | Read `src/publisher.cpp` and try to replicate the contents into your own node. Try without copy-paste. 229 | 230 | After writing this file we need to add some instructions to compile the C++ project. ROS internally using CMake for that. So create a `CMakeLists.txt` at the base folder of your package and add the following contents: 231 | 232 | ```cmake 233 | cmake_minimum_required(VERSION 3.8) 234 | project(ex01_first_package) 235 | 236 | ########################### 237 | # Not important to know start 238 | 239 | # Default to C++14 240 | if(NOT CMAKE_CXX_STANDARD) 241 | set(CMAKE_CXX_STANDARD 14) 242 | endif() 243 | 244 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 245 | add_compile_options(-Wall -Wextra -Wpedantic) 246 | endif() 247 | 248 | # Not important to know end 249 | ########################### 250 | 251 | # 'find_package' trys to find packages 252 | # usually all dependencies of the package.xml file 253 | # should occour here too 254 | find_package(ament_cmake REQUIRED) 255 | find_package(rclcpp REQUIRED) 256 | find_package(std_msgs REQUIRED) 257 | 258 | # Compile our node! 259 | add_executable(publisher src/publisher.cpp) 260 | 261 | ###### 262 | # Properly add ROS dependencies 263 | # - header files become available in code 264 | # - libraries are linked 265 | ament_target_dependencies(publisher 266 | rclcpp 267 | std_msgs) 268 | 269 | ####### 270 | # at execution time ROS2 only knows about things exist 271 | # in the install directory 'ceres_ws/install/...' 272 | # So we install our executables to 'ceres_ws/install/...' 273 | # via 274 | install(TARGETS 275 | publisher 276 | DESTINATION lib/${PROJECT_NAME}) 277 | 278 | # and finish with do ament_package cleanup 279 | ament_package() 280 | ``` 281 | 282 | The comments explain roughly what is going on during compilation. 283 | If you are interested you can read them. But from my experience, you will be good if you just copy-paste those commands. If you understand everything else you can come back to it. 284 | Now go to your workspace's base directory and call 285 | 286 | ```console 287 | user@pc:~/ceres_ws$ colcon build 288 | ``` 289 | 290 | If no errors occur the compilation has been successful. After that, you can run your node by calling 291 | 292 | ```console 293 | ros2 run ex01_first_package publisher 294 | ``` 295 | 296 | Open a second terminal (Ctrl + Shift + T) and enter 297 | 298 | ```console 299 | ros2 topic list 300 | ``` 301 | 302 | to list all the topics that are available. The topic `important_messages` should appear. With 303 | 304 | ```console 305 | ros2 topic echo /important_messages 306 | ``` 307 | 308 | You can print the (important) messages from the topic. 309 | 310 | ### Subscriber 311 | 312 | Now we want to receive the message in a self-written program. Imagine you want to write a Node that subscribes to an Image to detect cats. But in this example it is a stream of strings: 313 | 314 | Read `src/subscriber.cpp` and try to replicate the contents into your own node. Try without copy-paste. 315 | 316 | Afterward, try adding your cpp file to your `CMakeLists.txt` for compilation. Then call `colcon build` again. If you are not manage to fill out the `CMakeLists.txt` correctly, get yourself help by reading the `CMakeLists.txt` from this repository. 317 | 318 | When it compiles, test your program by calling 319 | 320 | First Terminal: 321 | ```console 322 | ros2 run ex01_first_package publisher 323 | ``` 324 | 325 | Second Terminal: 326 | ```console 327 | ros2 run ex01_first_package subscriber 328 | ``` 329 | 330 | The subscriber should always print the most recent messages that were sent by the publisher. Even after starting ending and restarting the subscriber. 331 | 332 | ### Better Publisher & Subscriber 333 | 334 | Further examples are in `src` folder of this repository. All publishers are doing exactly the same. As well as all subscribers are doing exactly the same. `publisher_class.cpp` and `subsriber_class.cpp` are the recommended styles of writing nodes. It's done by inheriting from `rclcpp::Node`. 335 | 336 | Read it, understand it, execute it. 337 | 338 | ## Last Words 339 | 340 | One node does not need to be a subscriber or publisher alone. A node can also be composed of multiple subscribers and publishers. 341 | ROS has more useful tools like `ros2 topic list` or `ros2 topic echo`. 342 | To see them just type `ros2 topic` and press enter. 343 | Test the commands with your topics. 344 | 345 | 346 | ### Data Distribution Services 347 | 348 | **Important**: This step is required when working with our robots!!! 349 | 350 | The default DDS that is used by ROS is `FastDDS`. In our tests, it tends to be unstable in networks. Luckily, we can switch to another implementation called `CylconeDDS` by installing it: 351 | 352 | ```console 353 | sudo apt install ros-humble-rmw-cyclonedds-cpp 354 | ``` 355 | 356 | And enabling it by adding the following lines to your `.bashrc`: 357 | 358 | ```bash 359 | # export RMW_IMPLEMENTATION=rmw_fastrtps_cpp 360 | export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp 361 | ``` 362 | 363 | Then restart your computer or the ROS daemon 364 | 365 | ```console 366 | ros2 daemon stop 367 | ros2 daemon start 368 | ``` 369 | 370 | Now try if your nodes are still working. 371 | 372 | 373 | Not required but just in case: If you want to recover the default `FastDDS` use: 374 | ```bash 375 | export RMW_IMPLEMENTATION=rmw_fastrtps_cpp 376 | ``` 377 | 378 | ### Addition: CPP tutorial 379 | 380 | In this package, I placed another method of writing a publisher. With `src/my_publisher.cpp` and `include/ex01_first_package/my_publisher.hpp` a library is built named `my_publisher`. This library is installed and can be used by every other package. As an example, the main node `src/publisher_lib.cpp` includes the header and links to the library. 381 | -------------------------------------------------------------------------------- /ex01_first_package/dat/radio_ros_simple.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uos/ros2_tutorial/718bea91b9dd87ac364b35945622fc4fe1c64148/ex01_first_package/dat/radio_ros_simple.png -------------------------------------------------------------------------------- /ex01_first_package/include/ex01_first_package/my_publisher.hpp: -------------------------------------------------------------------------------- 1 | #ifndef EX01_FIRST_PACKAGE_MY_PUBLISHER_HPP 2 | #define EX01_FIRST_PACKAGE_MY_PUBLISHER_HPP 3 | // ^- include guards 4 | // -> someone can include this header twice without breaking everything 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "rclcpp/rclcpp.hpp" 12 | #include "std_msgs/msg/string.hpp" 13 | 14 | // namespace: 15 | // to make sure we have no name conflicts. 16 | // maybe someone else want to name his class MyPublisher too. 17 | // if its in another namespace, no problem 18 | namespace ex01_first_package 19 | { 20 | 21 | class MyPublisher : public rclcpp::Node 22 | { 23 | public: 24 | MyPublisher(); 25 | 26 | private: 27 | void timer_callback(); 28 | 29 | rclcpp::TimerBase::SharedPtr timer_; 30 | rclcpp::Publisher::SharedPtr publisher_; 31 | size_t count_; 32 | }; 33 | 34 | } // namespace ex01_first_package 35 | 36 | #endif // EX01_FIRST_PACKAGE_MY_PUBLISHER_HPP -------------------------------------------------------------------------------- /ex01_first_package/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | ex01_first_package 7 | 0.0.0 8 | Install ROS. Create a workspace. Create a package. 9 | Alexander Mock 10 | Alexander Mock 11 | BSD 12 | 13 | ament_cmake 14 | rclcpp 15 | std_msgs 16 | 17 | 18 | ament_cmake 19 | 20 | -------------------------------------------------------------------------------- /ex01_first_package/src/my_publisher.cpp: -------------------------------------------------------------------------------- 1 | // Include header 2 | #include "ex01_first_package/my_publisher.hpp" 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "rclcpp/rclcpp.hpp" 10 | #include "std_msgs/msg/string.hpp" 11 | 12 | using namespace std::chrono_literals; 13 | 14 | namespace ex01_first_package 15 | { 16 | 17 | MyPublisher::MyPublisher() 18 | : rclcpp::Node("publisher") 19 | , count_(0) 20 | { 21 | publisher_ = this->create_publisher("important_messages", 10); 22 | timer_ = this->create_wall_timer( 23 | 500ms, std::bind(&MyPublisher::timer_callback, this)); 24 | } 25 | 26 | void MyPublisher::timer_callback() 27 | { 28 | std_msgs::msg::String message; 29 | message.data = "Hello, world! " + std::to_string(count_++); 30 | RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); 31 | publisher_->publish(message); 32 | } 33 | 34 | } // namespace ex01_first_package 35 | -------------------------------------------------------------------------------- /ex01_first_package/src/publisher.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "rclcpp/rclcpp.hpp" 7 | #include "std_msgs/msg/string.hpp" 8 | 9 | // only to write something like 10 | // 10ms, 100s 11 | using namespace std::chrono_literals; 12 | 13 | 14 | int main(int argc, char** argv) 15 | { 16 | rclcpp::init(argc, argv); 17 | 18 | // create new ROS node with name "publisher" 19 | auto node = std::make_shared("publisher"); 20 | auto publisher = node->create_publisher("important_messages", 10); 21 | 22 | rclcpp::executors::SingleThreadedExecutor executor; 23 | executor.add_node(node); 24 | 25 | size_t count = 0; 26 | while(rclcpp::ok()) 27 | { 28 | std_msgs::msg::String message; 29 | message.data = "Hello, world! " + std::to_string(count++); 30 | RCLCPP_INFO(node->get_logger(), "Publishing: '%s'", message.data.c_str()); 31 | publisher->publish(message); 32 | 33 | node->get_clock()->sleep_for(500ms); 34 | executor.spin_some(); 35 | } 36 | 37 | rclcpp::shutdown(); 38 | 39 | return 0; 40 | } -------------------------------------------------------------------------------- /ex01_first_package/src/publisher_class.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "rclcpp/rclcpp.hpp" 7 | #include "std_msgs/msg/string.hpp" 8 | 9 | using namespace std::chrono_literals; 10 | 11 | 12 | class MyPublisher : public rclcpp::Node 13 | { 14 | public: 15 | MyPublisher() 16 | : rclcpp::Node("publisher") 17 | , count_(0) 18 | { 19 | publisher_ = this->create_publisher("important_messages", 10); 20 | timer_ = this->create_wall_timer( 21 | 500ms, std::bind(&MyPublisher::timer_callback, this)); 22 | } 23 | 24 | private: 25 | void timer_callback() 26 | { 27 | std_msgs::msg::String message; 28 | message.data = "Hello, world! " + std::to_string(count_++); 29 | RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); 30 | publisher_->publish(message); 31 | } 32 | 33 | rclcpp::TimerBase::SharedPtr timer_; 34 | rclcpp::Publisher::SharedPtr publisher_; 35 | size_t count_; 36 | }; 37 | 38 | int main(int argc, char* argv[]) 39 | { 40 | rclcpp::init(argc, argv); 41 | rclcpp::spin(std::make_shared()); 42 | rclcpp::shutdown(); 43 | return 0; 44 | } -------------------------------------------------------------------------------- /ex01_first_package/src/publisher_lib.cpp: -------------------------------------------------------------------------------- 1 | // Include header 2 | #include "ex01_first_package/my_publisher.hpp" 3 | // only include header. source code is not known 4 | 5 | #include "rclcpp/rclcpp.hpp" 6 | 7 | using namespace ex01_first_package; 8 | 9 | int main(int argc, char** argv) 10 | { 11 | rclcpp::init(argc, argv); 12 | rclcpp::spin(std::make_shared()); 13 | rclcpp::shutdown(); 14 | 15 | return 0; 16 | } -------------------------------------------------------------------------------- /ex01_first_package/src/subscriber.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "rclcpp/rclcpp.hpp" 7 | #include "std_msgs/msg/string.hpp" 8 | 9 | // only to write something like 10 | // 10ms, 100s 11 | using namespace std::chrono_literals; 12 | 13 | // this function is called everytime a message appears 14 | // on the topic 'important_messages' 15 | void topic_callback(const std_msgs::msg::String & msg) 16 | { 17 | std::cout << "I heard: " << msg.data << std::endl; 18 | } 19 | 20 | int main(int argc, char** argv) 21 | { 22 | rclcpp::init(argc, argv); 23 | 24 | // create new ROS node with name "publisher" 25 | auto node = std::make_shared("subscriber"); 26 | auto subscriber = node->create_subscription( 27 | "important_messages", 10, topic_callback 28 | ); 29 | 30 | rclcpp::spin(node); 31 | 32 | return 0; 33 | } -------------------------------------------------------------------------------- /ex01_first_package/src/subscriber_class.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "rclcpp/rclcpp.hpp" 6 | #include "std_msgs/msg/string.hpp" 7 | 8 | using std::placeholders::_1; 9 | 10 | class MySubscriber : public rclcpp::Node 11 | { 12 | public: 13 | MySubscriber() 14 | :Node("subscriber") 15 | { 16 | subscription_ = this->create_subscription( 17 | "important_messages", 10, std::bind(&MySubscriber::topic_callback, this, _1)); 18 | } 19 | 20 | private: 21 | void topic_callback(const std_msgs::msg::String & msg) const 22 | { 23 | RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg.data.c_str()); 24 | } 25 | 26 | rclcpp::Subscription::SharedPtr subscription_; 27 | 28 | }; 29 | 30 | int main(int argc, char** argv) 31 | { 32 | rclcpp::init(argc, argv); 33 | rclcpp::spin(std::make_shared()); 34 | rclcpp::shutdown(); 35 | 36 | return 0; 37 | } -------------------------------------------------------------------------------- /ex01_first_package/src/subscriber_lambda.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "rclcpp/rclcpp.hpp" 6 | #include "std_msgs/msg/string.hpp" 7 | 8 | int main(int argc, char** argv) 9 | { 10 | rclcpp::init(argc, argv); 11 | 12 | // create new ROS node with name "publisher" 13 | auto node = std::make_shared("subscriber"); 14 | auto subscriber = node->create_subscription( 15 | "important_messages", 10, 16 | [&](const std_msgs::msg::String& msg){ 17 | std::cout << "I heard: " << msg.data << std::endl; 18 | } 19 | ); 20 | 21 | rclcpp::spin(node); 22 | 23 | return 0; 24 | } -------------------------------------------------------------------------------- /ex02_gazebo_simulation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(ex02_gazebo_simulation) 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(std_msgs REQUIRED) 16 | find_package(sensor_msgs REQUIRED) 17 | find_package(geometry_msgs REQUIRED) 18 | find_package(tf2 REQUIRED) 19 | find_package(tf2_ros REQUIRED) 20 | find_package(tf2_eigen REQUIRED) 21 | 22 | find_package (Eigen3 3.3 REQUIRED NO_MODULE) 23 | 24 | add_executable(scan_to_pcl 25 | src/scan_to_pcl.cpp) 26 | ament_target_dependencies(scan_to_pcl 27 | rclcpp 28 | std_msgs 29 | sensor_msgs 30 | geometry_msgs 31 | ) 32 | 33 | install(TARGETS 34 | scan_to_pcl 35 | DESTINATION lib/${PROJECT_NAME}) 36 | 37 | ######## 38 | ## Transform Pointcloud using Eigen 39 | add_executable(transform_pcl 40 | src/transform_pcl.cpp 41 | ) 42 | 43 | target_link_libraries(transform_pcl 44 | Eigen3::Eigen 45 | ) 46 | 47 | ament_target_dependencies(transform_pcl 48 | rclcpp 49 | tf2 50 | tf2_ros 51 | tf2_eigen 52 | geometry_msgs 53 | sensor_msgs 54 | ) 55 | 56 | install(TARGETS 57 | transform_pcl 58 | DESTINATION lib/${PROJECT_NAME}) 59 | 60 | ament_package() -------------------------------------------------------------------------------- /ex02_gazebo_simulation/README.md: -------------------------------------------------------------------------------- 1 | # ex02_gazebo_simulation 2 | 3 | 4 | ## Download 5 | 6 | Git-Clone the following packages into your ROS-workspace `src` directory using git: 7 | You will need some package to run the simulation of the robot. The same software packages are installed on the robot as well. 8 | 9 | - https://github.com/uos/ceres_robot 10 | - https://github.com/uos/uos_tools 11 | - https://github.com/uos/epos2_motor_controller 12 | - https://github.com/uos/volksbot_driver 13 | 14 | The current main branch of those software packages should be compatible with ROS2 humble. To make sure, switch all the repositories to "humble" branch instead: Go to a cloned repository folder and run `git checkout humble`. After you did that for every repository, compile your workspace. If errors occur: 15 | - ROS-Package is missing: `sudo apt-get install ros-humble-MISSING-PACKAGE` 16 | - System package is missing: `sudo apt-get install libftdipp1-dev` 17 | 18 | ### What to do when packages are missing? 19 | 20 | Let's assume the package `missing_package` is missing from your computer. Try to install it in this order, finish when one command has succeeded: 21 | 22 | 1. Install via apt: `sudo apt install ros-humble-missing-package` 23 | 2. Install via Github: Search for the package on github. Check if it's a valid ROS2-humble package. Download `missing_package` into your `src`-folder and compile it 24 | 3. Install via Gitlab: Search for the package on UOS-Gitlab. Check if it's a valid ROS2-humble package. Download `missing_package` into your `src`-folder and compile it 25 | 4. Go to Alex 26 | 27 | ## Run the Simulation 28 | 29 | ```console 30 | ros2 launch ceres_gazebo ceres_gazebo_launch.py 31 | ``` 32 | 33 | A graphical interface starts with a robot in it, spawned in an office environment. Try to add a cube. Try to move the robot. Make yourself familiar with the functionalities. The simulation, as the name tells, simulates a real robot. So when you learn to control the robot in the simulation, you will be able to control the robot in the real world. The simulation will generate the sensor data and publish them on some topics. Find out which topics. Print out some of the messages that are streamed on those topics. 34 | 35 | ## Control the robot 36 | 37 | ```console 38 | ros2 launch uos_diffdrive_teleop key.launch 39 | ``` 40 | 41 | A window will open. As long as it is the activate window you can control the robot via the shown keys. Move the robot through the office world. 42 | 43 | ## Visualize the Sensor Data 44 | 45 | ```console 46 | ros2 run rviz2 rviz2 47 | ``` 48 | 49 | or the shortcut: 50 | 51 | ```console 52 | rviz2 53 | ``` 54 | 55 | The robot is equipped with a laser scanner. Try to visualize the corresponding topic. Set the fixed frame to 'base_link'. What a fixed frame is will be explained soon. 56 | Move the robot around and see how the sensor data changes. RViz will be used with the real robot to monitor processes. Gazebo not. 57 | 58 | ## Use the sensor data 59 | 60 | Write a node that filters noise from the scan. Subscribe to the topic `scan`, average the distances with the direct neighboring measurements (average of 3 measurements in total) and publish the results on a new topic `scan_filtered`. Visualize the results with RViz. 61 | 62 | ### Point Clouds 63 | 64 | Messages on the topic are in polar coordinates. Write a node that converts the LaserScan to a `PointCloud2` message and publishes it on the topic `scan_cloud`. Don't use external libraries for the conversions. Copy the header of the `LaserScan` message to the header of the `PointCloud2` message. After publishing, visualize the results with RViz. 65 | 66 | 67 | ## TF 68 | 69 | Make yourself familiar with the concept of transformations. Visualize the transformations via RViz. In ROS2 there is a library called tf2 that handles all the transformations. Internally they present all the transformations as a tree. You can visualize it by calling 70 | 71 | ```console 72 | ros2 run rqt_tf_tree rqt_tf_tree 73 | ``` 74 | 75 | If it's not installed you can install it via `sudo apt install ros-humble-rqt-tf-tree`. 76 | Read the docs: https://docs.ros.org/en/foxy/Tutorials/Intermediate/Tf2/Introduction-To-Tf2.html 77 | 78 | Every piece of sensor data was recorded in a certain reference system at a certain time. Therefore, sensor data messages usually have a field `std_msgs/Header`: 79 | 80 | ```cpp 81 | ... 82 | string frame_id 83 | time stamp 84 | ``` 85 | 86 | Sometimes messages have the postfix `Stamped` if they are extending a message only by a header. For example, there is a message `geometry_msgs/Point`: 87 | 88 | ```cpp 89 | float64 x 90 | float64 y 91 | float64 z 92 | ``` 93 | 94 | And the corresponding `geometry_msgs/PointStamped`: 95 | 96 | ```cpp 97 | std_msgs/Header header 98 | geometry_msgs/Point point 99 | ``` 100 | 101 | Only if the stamped message is published, RViz knows where to visualize the point correctly. It transforms this point into the `fixed frame` that is configured in the GUI. 102 | 103 | ### Use TF 104 | 105 | Node: `transform_pcl`. 106 | Short Description: Transform a PointCloud2. Read a cloud from the recently generated point cloud topic and transform it into another coordinate frame. What you will need: 107 | 108 | - tf2 transform listener: to receive the transformation between two frames. Google for examples 109 | - apply the transformation to each of the point cloud's points 110 | - create a new point cloud from the transformed points 111 | - change the header of the new point cloud to the new coordinate frame 112 | - publish the resulting cloud on the topic `scan_cloud_transformed`. 113 | 114 | Show the results in RViz. Explain the results. 115 | 116 | ## ROS2 params and remapping 117 | 118 | ROS has also a mechanism to change parameters during runtime. For example, we want to enable the user to decide at runtime to which frame the cloud should be transformed. Make yourself familiar with ROS2 parameters. Add a parameter called `target_frame` to your `transform_pcl` node. Call your node from the command line and change the parameter: 119 | 120 | 121 | ```console 122 | ros2 run ex02_gazebo_simulation transform_pcl --ros-args -p target_frame:=imu 123 | ``` 124 | 125 | How to exactly use parameters in your source code is explained here: https://docs.ros.org/en/foxy/Tutorials/Beginner-Client-Libraries/Using-Parameters-In-A-Class-CPP.html 126 | 127 | Now the node should transform the point cloud to a different coordinate system. Check it by showing the results in RViz and printing the topic's header: 128 | 129 | ```console 130 | ros2 topic echo /scan --field header 131 | ``` 132 | 133 | The next mechanism is called "remapping". Imagine one user wants to use different topics than those you wrote into your source code. Remapping can change those topics at runtime. For example, if you want your `scan_to_pcl` node to publish its results on `scan_cloud2` instead of `scan_cloud`: 134 | 135 | ```console 136 | ros2 run ex02_gazebo_simulation scan_to_pcl --ros-args -r scan_cloud:=scan_cloud2 137 | ``` 138 | 139 | The same can be done for the input scan: 140 | 141 | ```console 142 | ros2 run ex02_gazebo_simulation scan_to_pcl --ros-args -r scan:=scan2 143 | ``` 144 | 145 | The node will subscribe on the topic `scan2` instead of `scan`. Like this, you can convert scans to clouds from different topics without changing the code. 146 | 147 | See the docs more in-depths explanations: https://docs.ros.org/en/foxy/How-To-Guides/Node-arguments.html 148 | 149 | ## Launch Files 150 | 151 | You might have been confused about the `ros2 launch ...`-commands. With `ros2 run` you can start simple nodes. With `launch` you can start so-called launch-files instead. 152 | Launch files are placed within a package's launch folder. Create a launch file called `launch_my_nodes.xml`: 153 | 154 | ```bash 155 | ex02_gazebo_simulation/ 156 | launch/ 157 | launch_my_nodes.xml 158 | ``` 159 | 160 | With launch-files you can capsule node runs into one file. One advantage of this is that you will save a lot of terminal tabs. 161 | 162 | Task: Try to put all your nodes into the launch file and start it. A tutorial how to do it: https://docs.ros.org/en/foxy/How-To-Guides/Launch-file-different-formats.html. 163 | Change your parameters and test how to do remapping inside of a launch file. 164 | 165 | 166 | After creating the launch file, install the launch folder by adding the following lines to your `CMakeLists.txt`: 167 | 168 | ```cmake 169 | install(DIRECTORY launch 170 | DESTINATION share/${PROJECT_NAME} 171 | ) 172 | ``` 173 | 174 | Then compile your workspace and run your launch file with 175 | 176 | ```console 177 | ros2 launch ex02_gazebo_simulation my_launch_file.xml 178 | ``` 179 | 180 | ## Drive the robot (Fun part) 181 | 182 | Write a node that at first just publishes a message of type `geometry_msgs/Twist` on the topic `cmd_vel`. Figure out what fields of the message are responsible for letting the robot move. 183 | 184 | ### Addition: Free Space 185 | 186 | Use the laser scan to find free space. Steer the robot towards it and drive. What is needed: 187 | 188 | - Subscriber to `scan` 189 | - Publisher on `cmd_vel` 190 | -------------------------------------------------------------------------------- /ex02_gazebo_simulation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | ex02_gazebo_simulation 7 | 0.0.0 8 | Use simulated ceres robot. LaserScans and PointClouds. Learn TF. 9 | Alexander Mock 10 | Alexander Mock 11 | BSD 12 | 13 | ament_cmake 14 | rclcpp 15 | std_msgs 16 | sensor_msgs 17 | geometry_msgs 18 | tf2 19 | tf2_ros 20 | tf2_eigen 21 | 22 | 23 | ament_cmake 24 | 25 | -------------------------------------------------------------------------------- /ex02_gazebo_simulation/src/scan_to_pcl.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | using std::placeholders::_1; 11 | 12 | struct Point32 13 | { 14 | float x; 15 | float y; 16 | float z; 17 | }; 18 | 19 | class ScanToPclNode : public rclcpp::Node 20 | { 21 | public: 22 | ScanToPclNode() 23 | :Node("scan_to_pcl") 24 | { 25 | sub_ = this->create_subscription( 26 | "scan", 10, std::bind(&ScanToPclNode::scan_callback, this, _1)); 27 | 28 | pub_ = this->create_publisher( 29 | "scan_cloud", 10 30 | ); 31 | } 32 | 33 | private: 34 | 35 | bool convert( 36 | const sensor_msgs::msg::LaserScan& scan, //in 37 | sensor_msgs::msg::PointCloud2& cloud) const // out 38 | { 39 | cloud.header = scan.header; 40 | 41 | cloud.data.resize(0); 42 | size_t valid_points = 0; 43 | 44 | for(size_t i=0; i(i) * scan.angle_increment; 48 | float range = scan.ranges[i]; 49 | 50 | if(range < scan.range_max && range > scan.range_min) 51 | { 52 | Point32 p; 53 | p.x = cos(angle) * range; 54 | p.y = sin(angle) * range; 55 | p.z = 0.0; 56 | 57 | // push back one byte after another 58 | Point32* p_ptr = &p; 59 | uint8_t* p_bytes_ptr = reinterpret_cast(p_ptr); 60 | for(size_t j=0; jget_logger(), "Scan received. Convert to PCL"); 101 | sensor_msgs::msg::PointCloud2 cloud; 102 | 103 | // convert 104 | convert(msg, cloud); 105 | 106 | pub_->publish(cloud); 107 | } 108 | 109 | rclcpp::Subscription::SharedPtr sub_; 110 | rclcpp::Publisher::SharedPtr pub_; 111 | 112 | }; 113 | 114 | int main(int argc, char** argv) 115 | { 116 | rclcpp::init(argc, argv); 117 | rclcpp::spin(std::make_shared()); 118 | rclcpp::shutdown(); 119 | 120 | return 0; 121 | } -------------------------------------------------------------------------------- /ex02_gazebo_simulation/src/transform_pcl.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "tf2/exceptions.h" 10 | #include "tf2_ros/transform_listener.h" 11 | #include "tf2_ros/buffer.h" 12 | #include "tf2/convert.h" 13 | #include "tf2_eigen/tf2_eigen.hpp" 14 | 15 | #include 16 | 17 | using std::placeholders::_1; 18 | 19 | struct Point32 20 | { 21 | float x; 22 | float y; 23 | float z; 24 | }; 25 | 26 | class TransformPclNode : public rclcpp::Node 27 | { 28 | public: 29 | TransformPclNode() 30 | :Node("transform_pcl") 31 | { 32 | sub_ = this->create_subscription( 33 | "scan_cloud", 10, std::bind(&TransformPclNode::cloud_callback, this, _1)); 34 | 35 | pub_ = this->create_publisher( 36 | "scan_cloud_transformed", 10 37 | ); 38 | 39 | tf_buffer_ = 40 | std::make_unique(this->get_clock()); 41 | tf_listener_ = 42 | std::make_shared(*tf_buffer_); 43 | 44 | // TODO: make ROS parameter from this 45 | frame_id_to_ = "base_footprint"; 46 | } 47 | 48 | private: 49 | 50 | sensor_msgs::msg::PointCloud2 doTransform( 51 | const sensor_msgs::msg::PointCloud2& cloud_in, 52 | const geometry_msgs::msg::TransformStamped& T) const 53 | { 54 | 55 | // convert Transform to Eigen type 56 | Eigen::Isometry3f Teig = tf2::transformToEigen(T.transform).cast(); 57 | // Eigen::Isometry3f Te 58 | 59 | // most of the fields stay the same: copy everything 60 | sensor_msgs::msg::PointCloud2 cloud_out = cloud_in; 61 | 62 | const sensor_msgs::msg::PointField* field_x = nullptr; 63 | const sensor_msgs::msg::PointField* field_y = nullptr; 64 | const sensor_msgs::msg::PointField* field_z = nullptr; 65 | 66 | // search for x y z fields 67 | for(size_t i=0; iname == "x") 72 | { 73 | field_x = field; 74 | } else if(field->name == "y") { 75 | field_y = field; 76 | } else if(field->name == "z") { 77 | field_z = field; 78 | } 79 | } 80 | 81 | if(!field_x || !field_y || !field_z) 82 | { 83 | RCLCPP_ERROR(this->get_logger(), "COULD NOT FIND X Y or Z"); 84 | throw std::runtime_error("COULD NOT FIND X Y or Z"); 85 | } 86 | 87 | const uint8_t* src_ptr = &cloud_in.data[0]; 88 | uint8_t* tgt_ptr = &cloud_out.data[0]; 89 | 90 | for(size_t i=0; ioffset; 96 | size_t biy = bi + field_y->offset; 97 | size_t biz = bi + field_z->offset; 98 | 99 | // get a point from bytes 100 | const float* x_src = reinterpret_cast(src_ptr + bix); 101 | const float* y_src = reinterpret_cast(src_ptr + biy); 102 | const float* z_src = reinterpret_cast(src_ptr + biz); 103 | 104 | // create eigen vector 105 | Eigen::Vector3f Pl; 106 | Pl.x() = *x_src; 107 | Pl.y() = *y_src; 108 | Pl.z() = *z_src; 109 | 110 | // transform single point from laser (l) to e.g. base (b) 111 | Eigen::Vector3f Pb = Teig * Pl; 112 | 113 | // pointer to target cloud 114 | float* x_tgt = reinterpret_cast(tgt_ptr + bix); 115 | float* y_tgt = reinterpret_cast(tgt_ptr + biy); 116 | float* z_tgt = reinterpret_cast(tgt_ptr + biz); 117 | 118 | // write to target cloud 119 | *x_tgt = Pb.x(); 120 | *y_tgt = Pb.y(); 121 | *z_tgt = Pb.z(); 122 | } 123 | 124 | // set new coordinate system 125 | cloud_out.header.frame_id = T.header.frame_id; 126 | 127 | return cloud_out; 128 | } 129 | 130 | void cloud_callback(const sensor_msgs::msg::PointCloud2& cloud_in) const 131 | { 132 | RCLCPP_INFO(this->get_logger(), "PCL received. Transform to PCL"); 133 | // sensor_msgs::msg::PointCloud2 cloud_out; 134 | 135 | geometry_msgs::msg::TransformStamped T; 136 | 137 | try { 138 | T = tf_buffer_->lookupTransform( 139 | frame_id_to_, cloud_in.header.frame_id, 140 | cloud_in.header.stamp); 141 | } catch (const tf2::TransformException & ex) { 142 | RCLCPP_INFO( 143 | this->get_logger(), "Could not transform %s to %s: %s", 144 | frame_id_to_.c_str(), cloud_in.header.frame_id.c_str(), ex.what()); 145 | return; 146 | } 147 | 148 | // Transform 149 | // - from: T.child_frame_id 150 | // - to: T.header.frame_id 151 | 152 | // transform all points 153 | sensor_msgs::msg::PointCloud2 cloud_out = doTransform(cloud_in, T); 154 | pub_->publish(cloud_out); 155 | } 156 | 157 | rclcpp::Subscription::SharedPtr sub_; 158 | rclcpp::Publisher::SharedPtr pub_; 159 | 160 | std::shared_ptr tf_listener_; 161 | std::unique_ptr tf_buffer_; 162 | 163 | std::string frame_id_to_; 164 | 165 | }; 166 | 167 | int main(int argc, char** argv) 168 | { 169 | rclcpp::init(argc, argv); 170 | rclcpp::spin(std::make_shared()); 171 | rclcpp::shutdown(); 172 | 173 | return 0; 174 | } -------------------------------------------------------------------------------- /ex03_state_estimation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(ex03_state_estimation) 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(std_msgs REQUIRED) 16 | 17 | ament_package() -------------------------------------------------------------------------------- /ex03_state_estimation/README.md: -------------------------------------------------------------------------------- 1 | # ex03_state_estimation 2 | 3 | Theory Lesson: state estimation 4 | 5 | (TODO write it down here) 6 | 7 | ## Sensors for State Estimation 8 | 9 | ### Wheel Odometry 10 | 11 | How far did my robot travel based on the number of revolutions my motor did? Calculate it for the simplest case by hand: Only driving forwards. 12 | Think about what changes if the robot has a linear and an angular velocity. 13 | 14 | 15 | What to know about wheel odometry: 16 | - Depends on motion model. Equations are different for Ackermann and differential steering. 17 | - Normally depends on the ground surface, but this is ignored in most software 18 | 1. assumption: The ground is flat 19 | 2. assumption: Wheels have infinite friction. They do not slip over the ground 20 | 21 | Task: Find, print and understand the wheel odometry data. What message of which package is used? 22 | 23 | ### IMU 24 | 25 | Why is it hard to use linear accelerations for translational changes? 26 | 27 | - Integration problems. 28 | 29 | Task: Find, print and understand the IMU data. What message of which package is used? 30 | 31 | 32 | ### LiDAR 33 | 34 | Start Gazebo open RViz and visualize the `scan` topic in fixed frame `base_link`. Now drive the robot forward. Can you estimate how far the robot has moved? Hint: The default size of a grid cell in RViz is 1x1 m. 35 | 36 | What you have done by intuition is exactly what the task of point cloud registration is trying to solve. Search the internet for "ICP". Small overview: 37 | 38 | - Find correspondences between a data set and a model by finding the closest points 39 | - Estimate the transformation parameters by Umeyama or non-linear optimization 40 | - Static environment 41 | 42 | Locally registering scans is sometimes referred to as LiDAR odometry. It is a core concept of modern SLAM solutions. We do that later. 43 | 44 | ### Camera 45 | 46 | It is also possible to use cameras to completely estimate the state of the robot in 3D. A good overview is given by [Wikipedia](https://en.wikipedia.org/wiki/Visual_odometry) in the Egomotion estimation GIF: 47 | 48 | ![visual-slam](https://upload.wikimedia.org/wikipedia/commons/e/e1/Egomotion-odometry.gif) 49 | 50 | 51 | ## Fusion 52 | 53 | If we have different sensors that all can measure the state of the robot we need to fuse them appropriately. In this case "appropriately" means that we want some measurements to influence parts of our state estimation more than others. 54 | For example: We can estimate the translational speed by using the linear acceleration of the IMU or the linear velocity of the wheel odometry. 55 | Since we already know linear accelerations are not very reliable when it comes to integrating them to positions, we don't want them to have much influence during fusion. 56 | And do not forget: All measurements are noisy. 57 | For this, the so-called "Kalman Filter" was invented. 58 | It is a simple filter that is a special case of a Bayes-Filter. 59 | 60 | This page gives a good overview of how a linear Kalman-Filter works in 1D: https://www.kalmanfilter.net/kalman1d.html : 61 | 62 | ![kalman1](https://www.kalmanfilter.net/img/OneD/HighKalmanGain.png) 63 | 64 | ![kalman2](https://www.kalmanfilter.net/img/OneD/LowKalmanGain.png) 65 | 66 | Linear Kalman-Filters have optimal filtering properties given that 67 | - all the sensors are pefectly modelled 68 | - the system can be modelled by linear transitions 69 | - sensor noise is normal distributed 70 | - the belief state is normal distributed and unimodal 71 | 72 | But this is rarely the case in reality. At some point, linear Kalman-Filters are becoming inaccurate because we cannot model the reality well enough. Then one usually switches to Extended Kalman Filters (EKF) or Unscented Kalman Filters (UKF). 73 | 74 | On mobile robots, there are most probably running at least one EKF as well. It is oftentimes used to fuse internal sensors, such as IMU and wheel odometry. A Kalman-Filter in general is not something that is the same for every robot. It has to be configured properly to fit the robot's sensors. For example, the ceres robot can be modelled as: 75 | 76 | - Use the `cmd_vel` topic as action / as prior 77 | - Use the `odom` topic as measurement / as posterior. Linear velocity is less noisy than the rotational velocity 78 | - Use the `imu/data_raw` as measurement / as posterior. Rotational velocity is far less noisy compared to linear acceleration 79 | 80 | Oftentimes the linear acceleration is completely ignored to estimate translational state components. 81 | Instead, the linear acceleration is used to improve the angular velocities using e.g. a Madgwick Filter. 82 | 83 | 84 | With the simulation, we already started a pre-configured EKF from the package [`robot_localization`](http://docs.ros.org/en/melodic/api/robot_localization/html/index.html) 85 | 86 | The result is the state of the robot given as `odometry/filtered` topic or as tf-transform `odom` -> `base_footprint`. 87 | 88 | Task: Open RViz. Switch to fixed frame `odom` and enable the laser scan. Drive around and see that the robot is localizing well in RViz. 89 | 90 | Make yourself familiar with the way the EKF was started and parameterized. New things that you should see: 91 | 92 | - Launch files, written in Python 93 | - Parameters defined in YAML files 94 | 95 | Task 1: Try to change those parameters a little. For example: Try to enable the linear acceleration of the IMU as an additional measurement 96 | 97 | Task 2: Try to integrate such a robot_localization launch and configuration files into this package 98 | 99 | ## Real Robot 100 | 101 | Caution !!!: To continue you will be required to press the red emergency stop button at least once. 102 | 103 | Now it's time to go to the real robot. You will need at least one laptop, with everything installed, and registered in the same WiFi as the robot (RoboNet). 104 | 105 | First try to ping the robot in the network: 106 | 107 | ```console 108 | ping flamara.informatik.uos.de 109 | ``` 110 | 111 | Then connect via SSH 112 | 113 | ``` 114 | ssh robot@flamara.informatik.uos.de 115 | ``` 116 | 117 | Figure out the `ROS_DOMAIN_ID` of the robot and set your laptop to the same. 118 | 119 | ### TMUX 120 | 121 | Next, you need to learn a tool called `tmux`. It should be already installed on the robot. You do not necessarily need to install it on your own system. But I recommend installing it anyway in case you want to learn the commands on your own machine first. 122 | 123 | ```console 124 | sudo apt-get install tmux 125 | ``` 126 | 127 | Why tmux? Accessing the robot via SSH alone has a few drawbacks: 128 | - If your SSH connection dies so is all you have started 129 | - If you start some nodes, only you can kill or restart them easily 130 | 131 | Fortunately, `tmux` has you covered: 132 | 133 | - Start tmux session by calling `tmux new -s mysession` 134 | - You will enter the tmux environment: You can recognize it via a green line at the bottom of the terminal 135 | - Leave a session and keep it alive: `[CTRL+b]` to enter the command mode. Then press `[d]` (d for detach). 136 | - To enter it again use `tmux a` (a for attach) and press enter. 137 | - Leave it again and try to create a second session 138 | - Enter it and switch sessions via `[CTRL+b]` to enter the command mode. Then press `[s]`. To open a list of available sessions. Select one via the arrow keys and press enter to enter it. 139 | 140 | There are more commands but these are the most essential you will need. Such tmux sessions keep alive even after you have lost your SSH connections. Furthermore, its possible to enter one session with more than one user. 141 | 142 | Task: One person is creating a tmux session the other person is accessing it. Try to edit a file together. 143 | 144 | ### Bringup 145 | 146 | To start everything on the real robot, start a tmux session and call 147 | 148 | ```console 149 | ros2 launch ceres_bringup ceres_launch.py 150 | ``` 151 | 152 | This launch file starts all the sensor drivers and an EKF for state estimation. The created topics have the same name and message types as the Gazebo simulation was creating. 153 | So all of your already written nodes should work without changing the code. 154 | 155 | Task: Test some of your nodes. First, start them on your laptop. Second, transfer the source code to the robot (scp), build it on the robot, and run it on the robot. What is better? 156 | 157 | Control the robot via `ros2 launch uos_diffdrive_teleop key.launch`. One person always(!) has to be ready to push the red button. 158 | 159 | ### RViz with real robot 160 | 161 | Start RViz to visualize the real robot's sensor data. Check if the localization via EKF works. 162 | 163 | 164 | 165 | ### Simulation and Reality 166 | 167 | Because of the available ROS2 abstractions, it becomes possible to implement every piece of software just by using the simulation. So home office is possible, isn't it? No. simulated worlds are usually very much simplified and sensor data includes way less noise than reality. Most of the robotics algorithms have to be tested in reality! However, experienced robotics engineers can write software so that it is almost always working in reality. 168 | 169 | 170 | ## Preview Mapping 171 | 172 | Using either simulated or real robot. Try the following: 173 | 174 | Open RViz, set the fixed frame to `odom`. Visualize a laser scan or a point cloud. Increase the "decay time" to 100. Move around. 175 | 176 | Although no active matching of the scans is done, it looks as if the scans match very well. After a time it looks like we are building some kind of a floor plan: a 2D map. 177 | 178 | ## Maps 179 | 180 | A map is useful for a robot. It uses it for more robust localization and efficient navigation planning. Maps can exist in various forms. Most of the robots in the world use so-called 2D grid maps, including your vacuum cleaners. More interesting for research, however, are the 3D map formats such as: 181 | 182 | - point cloud maps 183 | - voxel maps 184 | - tsdf maps 185 | - mesh maps 186 | - NERFs, NDFs 187 | 188 | Try to figure out what is behind those map formats. 189 | 190 | For simplicity, we stay in a 2D world. What message is used for a 2D grid map? 191 | 192 | ## SLAM 193 | 194 | Simultaneous localization and mapping (SLAM) deal with localizing the robot and at the same time creating a globally consistent map. A quick overview of what has worked out pretty well so far: 195 | 196 | 1. Using some kind of intrinsic state estimation as prior (Wheel & IMU - EKF) 197 | 2. Using scan registration as posterior 198 | 3. 1 & 2 tend to drift: Try to detect loops and correct the drift afterward using e.g. pose graph optimization 199 | 200 | Assumptions of (most) SLAM: 201 | - Belief state must be unimodal -> first pose has to be known 202 | - What if not? We come back to that later (see MCL) 203 | 204 | Ceres robot has implemented a SLAM. Task: Start it by executing: 205 | 206 | ```console 207 | ros2 launch ceres_localization slam_toolbox_launch.py 208 | ``` 209 | 210 | Open the tf tree 211 | 212 | ```console 213 | ros2 run rqt_tf_tree rqt_tf_tree 214 | ``` 215 | 216 | and explain what has been added. Then run RViz and visualize the map while driving the robot around. Try to save the map. 217 | -------------------------------------------------------------------------------- /ex03_state_estimation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | ex03_state_estimation 7 | 0.0.0 8 | State estimation of ceres robot 9 | Alexander Mock 10 | Alexander Mock 11 | BSD 12 | 13 | ament_cmake 14 | rclcpp 15 | std_msgs 16 | sensor_msgs 17 | geometry_msgs 18 | tf2_ros 19 | 20 | 21 | ament_cmake 22 | 23 | 24 | -------------------------------------------------------------------------------- /ex04_navigation/README.md: -------------------------------------------------------------------------------- 1 | # ex04_navigation 2 | 3 | In exercise 03 you managed to build a 2d grid map from the environment and store it on the disk. We already did that so we continue to use our pre-recorded map from now on. It is located in 4 | 5 | - `uos_tools/uos_maps/maps/cic.png` 6 | - `uos_tools/uos_maps/maps/cic.yaml` 7 | 8 | and for the gazebo simulation its: 9 | 10 | - `uos_tools/uos_maps/maps/avz.pgm` 11 | - `uos_tools/uos_maps/maps/avz.yaml` 12 | 13 | Make yourself familiar with the map format. What is written in the yaml files? What message type is published on `/map` while doing SLAM? 14 | 15 | ## Localization 16 | 17 | In an existing map we normally do [monte carlo localization](https://en.wikipedia.org/wiki/Monte_Carlo_localization) (MCL). You can start it by calling 18 | 19 | ```console 20 | ros2 launch ceres_gazebo amcl_avz_launch.xml 21 | ``` 22 | 23 | or on the real robot with 24 | 25 | ```console 26 | ros2 launch ceres_bringup amcl_cic_launch.xml 27 | ``` 28 | 29 | Use the preconfigured RViz to load everything you need into your RViz window: 30 | 31 | ```console 32 | rviz2 -d ceres_ws/src/ceres_robot/ceres_util/rviz/nav2_default_view.rviz 33 | ``` 34 | 35 | Then press `2D Pose Estimate` to give the robot a rough estimate of its pose in the map. Now you should see the scan appearing as well as a green particle cloud. 36 | 37 | ![amcl](./dat/amcl.png) 38 | 39 | 40 | The particle cloud represents the state of the robot inside of AMCL. The more the particles are spreading, the more the robot is uncertain about its state in the map. As soon as you start to drive the robot around, the particle cloud should become smaller and the scan aligns the map better. Task: Try that. 41 | 42 | ### Theory 43 | 44 | A belief state is the state of the robot that is estimated based on actions and measurements. 45 | Once the robot is successfully localized, the belief state matches the real state. 46 | Markow localization is a Bayes filter applied to the localization problem. 47 | A special implementation is the Kalman filter (KF), which has the limitations of the Belief state to be unimodal. 48 | Grid Localization comes up with a multimodal state (one probability per grid cell), and therefore can be applied to situations where a unimodal belief state (like a Kalman filter has) insufficiently models the localization problem. 49 | Grid Localization has a multimodal belief state representation but has the disadvantage, that the state is only allowed to be discrete. 50 | Monte Carlo localization (MCL) overcomes this disadvantage by utilizing a particle filter. 51 | MCL uses a set of particles to describe the belief state. This table gives an overview: 52 | 53 | 54 | | Algorithm | Belief State | State space | 55 | |:---------:|:------------:|:-----------:| 56 | | Kalman filter (KF) | unimodal | continuous | 57 | | SLAM | unimodal * | continuous | 58 | | Grid Localization | multimodal | discrete | 59 | | Monte Carlo localization (MCL) | multimodal | continuous | 60 | 61 | In many cases, it is not required to have multiple modes in the belief state. But sometimes it is. For example when doing global localization. Normally, the computational effort increases when doing MCL over SLAM. 62 | 63 | (*) The term SLAM - simultaneous localization and mapping - does not exclude the usage of multimodal belief states. However, most of the recent SLAM software has an underlying unimodal belief state. Some SLAM approaches add past pose estimates to the belief state; its dimensionality is growing with each pose added. If interested, search for pose graphs or factor graphs. 64 | 65 | 66 | MCL ([Wikipedia](https://en.wikipedia.org/wiki/Monte_Carlo_localization)): 67 | 68 | ![mcl](https://upload.wikimedia.org/wikipedia/commons/thumb/6/68/Mcl_t_2_3.svg/1920px-Mcl_t_2_3.svg.png) 69 | 70 | Bimodal Distribution ([Wikipedia](https://en.wikipedia.org/wiki/Multimodal_distribution)): 71 | 72 | ![bimodal](https://upload.wikimedia.org/wikipedia/commons/b/bc/Bimodal_geological.PNG) 73 | 74 | This gives a very short overview of state estimation algorithms known in the field of probabilistic robotics. To get a more comprehensive overview we suggest reading the book "Probabilistic Robotics" by Thrun et al. 75 | 76 | ## Nav2 77 | 78 | Given the localization in the map, it becomes possible to plan and follow paths from the robot's position to a given goal somewhere else in the map. There is a package exactly handling that: nav2. Browse a bit on their website: https://navigation.ros.org/. In this exercise, you will start a preconfigured nav2 launch file to let the robot for the first time autonomously drive through the map. 79 | 80 | ```console 81 | ros2 launch ceres_navigation nav2_launch.py 82 | ``` 83 | 84 | Now open the preconfigured RViz again. Once the robot is localized, you can set a goal pose somewhere in the map using `Nav2 Goal` or `2D Goal Pose`. Then the robot should drive safely from A to B while avoiding dynamic obstacles. 85 | 86 | ![amcl](./dat/nav2.png) 87 | 88 | You can test obstacle avoidance by placing random objects in the Gazebo simulation or in the real world. 89 | 90 | 91 | ### Theory 92 | 93 | Navigation in 2D Grid Maps is usually done by first planning paths from A to B and after that following the paths using controller. Planning paths is done by using so-called "Global Planner". Letting the robot follow the path is done by so-called "Local Planner". Both global planner and local planner implementation can be switched inside the nav2 parameters. Search for the nav2 config file that is used by the ceres robot and find out which global and local planner algorithm is used. 94 | 95 | Additionally, nav2 uses so-called cost-layers on top of the regular grid map. With a cost-layer dangerous zones can be defined. 96 | 97 | 98 | #### Inflation Layer 99 | 100 | To make computations simpler, the walls in the map are inflated by the robot's radius. Like this, the robot can be considered as a point. Assuming the robots are looking like circles. 101 | 102 | ## Actions 103 | 104 | What you implemented so far are programs that run forever. For example, converting messages from topics to another topic. But some tasks of the robot have an end. For example, driving from A to B. Such a task is also referred to as an "action". For this ROS2 implements action interfaces to use. It is splitted into action servers and action client. The action server is always executing the action (like driving from A to B). The action client is triggering the action and can receive feedback by the action server of the current progress of the action: 105 | 106 | ![action_server_client](./dat/action_server_client.png) 107 | 108 | Additionally, the client can send signals to the server to cancel the action. For example, if during the drive from A to B the plans have changed. So the action client oftentimes acts as a monitor. 109 | 110 | 111 | ### Own action 112 | 113 | Together with messages, actions are one part of the ROS2 interface system. 114 | You can find the following code samples in the package `ex04_msgs`. 115 | 116 | The minimum information that is shipped with an action is if it was successful, aborted or canceled. In addition, we can define custom fields for the goal, result, and feedback: 117 | 118 | `ex04_msgs/actions/WaitForX.action`: 119 | ``` 120 | # goal 121 | float32 duration 122 | --- 123 | # result 124 | --- 125 | # feedback 126 | ``` 127 | 128 | The package `ex04_msgs` shows a minimal example of how to compile those actions. Help yourself by looking into the `CMakeLists.txt` and `package.xml`. 129 | 130 | ### Own action server 131 | 132 | `WaitForX` and `WaitForRvizPose` are action interfaces. This means they can be used independently from the programming language to trigger an action server to do something. In this task, you will write an action server that will handle the logic around: 133 | 134 | `WaitForXActionServer`: 135 | - `WaitForX` action goal comes from someone (client) 136 | - The action server will wait for X seconds 137 | - Returns `SUCCEEDED` once it has finished 138 | - File: `ex04_actions/src/wait_for_x_action_server.cpp` 139 | 140 | `WaitForRvizPoseActionServer`: 141 | - `WaitForRvizPose` action goal comes from someone (client) 142 | - The action server will wait until something is published on `/goal_pose`. The `2D Nav Goal` of RViz is publishing a `geometry_msgs/PoseStamped` on this topic. 143 | - If a pose was received. Return it with the outcome `SUCCEEDED` 144 | - If no pose was received and a certain time was exceeded return `ABORTED` 145 | - File: `ex04_actions/src/wait_for_rviz_pose_action_server.cpp` 146 | 147 | Start the action servers via: 148 | 149 | ```console 150 | ros2 run ex04_actions wait_for_x_action_server 151 | ``` 152 | 153 | You can test your server implementations by using the following command line tools: 154 | 155 | ```console 156 | ros2 action send_goal /wait_for_x ex04_msgs/action/WaitForX "{duration: 4.0}" 157 | ``` 158 | 159 | Try the same with the `/wait_for_rviz_pose` action server. 160 | 161 | ### Own action client 162 | 163 | Instead of using the command line tools. In this task, you will call your action servers with a self-made action client using C++. File: `ex04_actions/src/wait_for_x_client`. Read, understand and execute the code. 164 | 165 | ### Nav2 action client 166 | 167 | Given the knowledge of how to implement action clients you can now start to write code that executes different actions from different packages. Nav2 for example provides action interfaces and servers for different navigation-related actions: 168 | 169 | 1. start the simulation (or the robot). Start AMCL. Start Nav2 170 | 171 | 2. Enter `ros2 action list` to list all the action server available. The most important ones you will need for now are: 172 | 173 | | Action | Type | 174 | |:--------:|:-------:| 175 | | `/compute_path_to_pose` | Executes the global planner. Goal Pose -> Path | 176 | | `/follow_path` | Executes the local planner. Follow a given path. | 177 | | `/navigate_to_pose` | Shortcut: Global and local planner. | 178 | 179 | In our next node we will try to trigger the shortcut action `/navigate_to_pose` so that the robot will drive forever in a loop between certain poses in the map. 180 | 181 | It is already implemented in `ex04_nav2_client/src/nav2_client.cpp` you can start it via 182 | 183 | ```console 184 | ros2 run ex04_nav2_client nav2_client 185 | ``` 186 | 187 | ### Send RViz goal to Nav2 188 | 189 | Write a node that uses a `WaitForRvizPose` action client to retrieve a pose from RViz. Then pass the pose to the Nav2 action client to let the robot drive to that goal. 190 | 191 | 192 | TODO: Figure out how to disable that nav2 receives a pose from RViz (Maybe just a remap?) 193 | -------------------------------------------------------------------------------- /ex04_navigation/dat/action_server_client.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uos/ros2_tutorial/718bea91b9dd87ac364b35945622fc4fe1c64148/ex04_navigation/dat/action_server_client.png -------------------------------------------------------------------------------- /ex04_navigation/dat/amcl.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uos/ros2_tutorial/718bea91b9dd87ac364b35945622fc4fe1c64148/ex04_navigation/dat/amcl.png -------------------------------------------------------------------------------- /ex04_navigation/dat/nav2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uos/ros2_tutorial/718bea91b9dd87ac364b35945622fc4fe1c64148/ex04_navigation/dat/nav2.png -------------------------------------------------------------------------------- /ex04_navigation/ex04_actions/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(ex04_actions) 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_action REQUIRED) 16 | find_package(geometry_msgs REQUIRED) 17 | find_package(ex04_msgs REQUIRED) 18 | 19 | ####### 20 | # Action Server 21 | ###### 22 | add_executable(wait_for_rviz_pose_action_server 23 | src/wait_for_rviz_pose_action_server.cpp) 24 | 25 | ament_target_dependencies(wait_for_rviz_pose_action_server 26 | ex04_msgs 27 | rclcpp 28 | rclcpp_action) 29 | 30 | add_executable(wait_for_x_action_server 31 | src/wait_for_x_action_server.cpp) 32 | 33 | ament_target_dependencies(wait_for_x_action_server 34 | ex04_msgs 35 | rclcpp 36 | rclcpp_action) 37 | 38 | install(TARGETS 39 | wait_for_rviz_pose_action_server wait_for_x_action_server 40 | DESTINATION lib/${PROJECT_NAME}) 41 | 42 | 43 | add_executable(wait_for_x_client 44 | src/wait_for_x_client.cpp) 45 | 46 | ament_target_dependencies(wait_for_x_client 47 | ex04_msgs 48 | rclcpp 49 | rclcpp_action) 50 | 51 | 52 | add_executable(wait_for_x_client_sync 53 | src/wait_for_x_client_sync.cpp) 54 | 55 | ament_target_dependencies(wait_for_x_client_sync 56 | ex04_msgs 57 | rclcpp 58 | rclcpp_action) 59 | 60 | # install all nodes 61 | install(TARGETS 62 | wait_for_rviz_pose_action_server 63 | wait_for_x_action_server 64 | wait_for_x_client 65 | wait_for_x_client_sync 66 | DESTINATION lib/${PROJECT_NAME}) 67 | 68 | 69 | ament_export_dependencies( 70 | ex04_msgs 71 | ) 72 | 73 | ament_package() -------------------------------------------------------------------------------- /ex04_navigation/ex04_actions/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | ex04_actions 7 | 0.0.0 8 | Actions, services and behavior trees 9 | Alexander Mock 10 | Alexander Mock 11 | BSD 12 | 13 | ament_cmake 14 | 15 | rclcpp 16 | std_msgs 17 | geometry_msgs 18 | ex04_msgs 19 | 20 | 21 | ament_cmake 22 | 23 | 24 | -------------------------------------------------------------------------------- /ex04_navigation/ex04_actions/src/wait_for_rviz_pose_action_server.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "rclcpp/rclcpp.hpp" 7 | #include "rclcpp_action/rclcpp_action.hpp" 8 | #include "rclcpp/wait_for_message.hpp" 9 | 10 | #include 11 | #include 12 | 13 | using namespace std::chrono_literals; 14 | 15 | namespace ex04_actions 16 | { 17 | 18 | class WaitForRvizPoseActionServer : public rclcpp::Node 19 | { 20 | public: 21 | using WaitForRvizPose = ex04_msgs::action::WaitForRvizPose; 22 | using GoalHandleWaitForRvizPose = rclcpp_action::ServerGoalHandle; 23 | 24 | WaitForRvizPoseActionServer(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) 25 | : Node("wait_for_rviz_pose_action_server", options) 26 | { 27 | using namespace std::placeholders; 28 | 29 | this->action_server_ = rclcpp_action::create_server( 30 | this, 31 | "wait_for_rviz_pose", 32 | std::bind(&WaitForRvizPoseActionServer::on_goal_received, this, _1, _2), 33 | std::bind(&WaitForRvizPoseActionServer::on_cancel, this, _1), 34 | std::bind(&WaitForRvizPoseActionServer::on_goal_accepted, this, _1)); 35 | 36 | // parameter that defines the rate of cancel request checks inside the execution loop 37 | tick_rate_ = this->declare_parameter("tick_rate", 100.0); 38 | 39 | std::cout << "Action Server Started." << std::endl; 40 | } 41 | 42 | private: 43 | rclcpp_action::Server::SharedPtr action_server_; 44 | 45 | std::atomic_bool goal_canceled_ = false; 46 | double tick_rate_ = 100.0; 47 | 48 | rclcpp_action::GoalResponse on_goal_received( 49 | const rclcpp_action::GoalUUID & uuid, 50 | std::shared_ptr goal) 51 | { 52 | RCLCPP_INFO(this->get_logger(), "Received goal request"); 53 | (void)goal; 54 | (void)uuid; 55 | return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; 56 | } 57 | 58 | rclcpp_action::CancelResponse on_cancel( 59 | const std::shared_ptr goal_handle) 60 | { 61 | RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); 62 | 63 | goal_canceled_ = true; 64 | 65 | (void)goal_handle; 66 | return rclcpp_action::CancelResponse::ACCEPT; 67 | } 68 | 69 | void on_goal_accepted(const std::shared_ptr goal_handle) 70 | { 71 | using namespace std::placeholders; 72 | 73 | // this needs to return quickly to avoid blocking the executor, so spin up a new thread 74 | std::thread{std::bind(&WaitForRvizPoseActionServer::execute, this, _1), goal_handle}.detach(); 75 | } 76 | 77 | void execute(const std::shared_ptr goal_handle) 78 | { 79 | const auto goal = goal_handle->get_goal(); 80 | auto feedback = std::make_shared(); 81 | auto result = std::make_shared(); 82 | 83 | std::chrono::duration d_ticked(0.0); 84 | std::chrono::duration d_tick(1.0 / tick_rate_); 85 | std::chrono::duration d_wait_for(goal->duration); 86 | 87 | bool pose_found = false; 88 | 89 | auto subscriber = this->create_subscription( 90 | "goal_pose", 10, 91 | [&](const geometry_msgs::msg::PoseStamped& msg){ 92 | std::cout << "I heard something!" << std::endl; 93 | pose_found = true; 94 | result->pose = msg; 95 | } 96 | ); 97 | 98 | bool time_exceeded = false; 99 | 100 | // subscriber is running 101 | while(!pose_found) 102 | { 103 | // wait 104 | this->get_clock()->sleep_for(d_tick); 105 | d_ticked += d_tick; 106 | 107 | if(goal_canceled_) 108 | { 109 | break; 110 | } 111 | 112 | if(d_wait_for > 0.0s && d_ticked > d_wait_for) 113 | { 114 | // time exceeded 115 | time_exceeded = true; 116 | break; 117 | } 118 | } 119 | 120 | if(goal_canceled_) 121 | { 122 | goal_handle->canceled(result); 123 | RCLCPP_INFO(this->get_logger(), "Goal canceled."); 124 | goal_canceled_ = false; 125 | } else if(time_exceeded) { 126 | goal_handle->abort(result); 127 | RCLCPP_INFO(this->get_logger(), "Goal canceled. Reason: time exceeded."); 128 | } else { 129 | goal_handle->succeed(result); 130 | RCLCPP_INFO(this->get_logger(), "Goal succeeded!"); 131 | } 132 | } 133 | }; // class WaitForRvizPoseActionServer 134 | 135 | } // namespace ex04_actions 136 | 137 | int main(int argc, char** argv) 138 | { 139 | using namespace ex04_actions; 140 | rclcpp::init(argc, argv); 141 | rclcpp::spin(std::make_shared()); 142 | rclcpp::shutdown(); 143 | return 0; 144 | } -------------------------------------------------------------------------------- /ex04_navigation/ex04_actions/src/wait_for_x_action_server.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "rclcpp/rclcpp.hpp" 7 | #include "rclcpp_action/rclcpp_action.hpp" 8 | #include "rclcpp/wait_for_message.hpp" 9 | 10 | #include 11 | #include 12 | 13 | using namespace std::chrono_literals; 14 | 15 | namespace ex04_actions 16 | { 17 | 18 | class WaitForXActionServer : public rclcpp::Node 19 | { 20 | public: 21 | using WaitForX = ex04_msgs::action::WaitForX; 22 | using GoalHandleWaitForX = rclcpp_action::ServerGoalHandle; 23 | 24 | WaitForXActionServer(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) 25 | : Node("wait_for_x_action_server", options) 26 | { 27 | using namespace std::placeholders; 28 | 29 | this->action_server_ = rclcpp_action::create_server( 30 | this, 31 | "wait_for_x", 32 | std::bind(&WaitForXActionServer::on_goal_received, this, _1, _2), 33 | std::bind(&WaitForXActionServer::on_cancel, this, _1), 34 | std::bind(&WaitForXActionServer::on_goal_accepted, this, _1)); 35 | 36 | // parameter that defines the rate of cancel request checks inside the execution loop 37 | tick_rate_ = this->declare_parameter("tick_rate", 100.0); 38 | 39 | std::cout << "Action Server Started." << std::endl; 40 | } 41 | 42 | private: 43 | rclcpp_action::Server::SharedPtr action_server_; 44 | 45 | std::atomic_bool goal_canceled_ = false; 46 | double tick_rate_ = 100.0; 47 | 48 | rclcpp_action::GoalResponse on_goal_received( 49 | const rclcpp_action::GoalUUID & uuid, 50 | std::shared_ptr goal) 51 | { 52 | RCLCPP_INFO(this->get_logger(), "Received goal request"); 53 | (void)goal; 54 | (void)uuid; 55 | return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; 56 | } 57 | 58 | rclcpp_action::CancelResponse on_cancel( 59 | const std::shared_ptr goal_handle) 60 | { 61 | RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); 62 | 63 | goal_canceled_ = true; 64 | 65 | (void)goal_handle; 66 | return rclcpp_action::CancelResponse::ACCEPT; 67 | } 68 | 69 | void on_goal_accepted(const std::shared_ptr goal_handle) 70 | { 71 | using namespace std::placeholders; 72 | 73 | // this needs to return quickly to avoid blocking the executor, so spin up a new thread 74 | std::thread{std::bind(&WaitForXActionServer::execute, this, _1), goal_handle}.detach(); 75 | } 76 | 77 | void execute(const std::shared_ptr goal_handle) 78 | { 79 | const auto goal = goal_handle->get_goal(); 80 | auto feedback = std::make_shared(); 81 | auto result = std::make_shared(); 82 | 83 | std::chrono::duration d_ticked(0.0); 84 | std::chrono::duration d_tick(1.0 / tick_rate_); 85 | std::chrono::duration d_wait_for(goal->duration); 86 | 87 | // bool pose_found = false; 88 | 89 | // auto subscriber = this->create_subscription( 90 | // "goal_pose", 10, 91 | // [&](const geometry_msgs::msg::PoseStamped& msg){ 92 | // std::cout << "I heard something!" << std::endl; 93 | // pose_found = true; 94 | // result->pose = msg; 95 | // } 96 | // ); 97 | 98 | bool time_exceeded = false; 99 | 100 | // subscriber is running 101 | while(rclcpp::ok()) 102 | { 103 | // wait 104 | this->get_clock()->sleep_for(d_tick); 105 | d_ticked += d_tick; 106 | 107 | if(goal_canceled_) 108 | { 109 | break; 110 | } 111 | 112 | if(d_wait_for > 0.0s && d_ticked > d_wait_for) 113 | { 114 | // time exceeded 115 | time_exceeded = true; 116 | break; 117 | } 118 | } 119 | 120 | if(goal_canceled_) 121 | { 122 | goal_handle->canceled(result); 123 | RCLCPP_INFO(this->get_logger(), "Goal canceled."); 124 | goal_canceled_ = false; 125 | } else if(time_exceeded) { 126 | goal_handle->succeed(result); 127 | RCLCPP_INFO(this->get_logger(), "Goal succeeded!"); 128 | } else { 129 | throw std::runtime_error("This case shouldn't happen!"); 130 | } 131 | } 132 | }; // class WaitForXActionServer 133 | 134 | } // namespace ex04_actions 135 | 136 | int main(int argc, char** argv) 137 | { 138 | using namespace ex04_actions; 139 | rclcpp::init(argc, argv); 140 | rclcpp::spin(std::make_shared()); 141 | rclcpp::shutdown(); 142 | return 0; 143 | } -------------------------------------------------------------------------------- /ex04_navigation/ex04_actions/src/wait_for_x_client.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | #include "ex04_msgs/action/wait_for_x.hpp" 10 | 11 | #include "rclcpp/rclcpp.hpp" 12 | #include "rclcpp/future_return_code.hpp" 13 | #include "rclcpp_action/rclcpp_action.hpp" 14 | 15 | using namespace std::chrono_literals; 16 | 17 | class WaitForXClient : public rclcpp::Node 18 | { 19 | public: 20 | using WaitForX = ex04_msgs::action::WaitForX; 21 | using GoalHandle = rclcpp_action::ClientGoalHandle; 22 | 23 | WaitForXClient(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) 24 | : Node("wait_for_x_action_client", options) 25 | { 26 | this->client_ptr_ = rclcpp_action::create_client( 27 | this, 28 | "wait_for_x"); 29 | } 30 | 31 | GoalHandle::SharedPtr send_goal(float duration) 32 | { 33 | return send_goal_async(duration).get(); 34 | } 35 | 36 | std::shared_future send_goal_async(float duration) 37 | { 38 | using namespace std::placeholders; 39 | 40 | std::cout << "Waiting connection to action server..." << std::endl; 41 | if (!this->client_ptr_->wait_for_action_server()) 42 | { 43 | RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); 44 | rclcpp::shutdown(); 45 | } 46 | 47 | auto goal_msg = WaitForX::Goal(); 48 | goal_msg.duration = duration; 49 | 50 | RCLCPP_INFO(this->get_logger(), "Sending goal"); 51 | 52 | auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); 53 | 54 | send_goal_options.goal_response_callback = 55 | std::bind(&WaitForXClient::goal_response_callback, this, _1); 56 | send_goal_options.feedback_callback = 57 | std::bind(&WaitForXClient::feedback_callback, this, _1, _2); 58 | send_goal_options.result_callback = 59 | std::bind(&WaitForXClient::result_callback, this, _1); 60 | 61 | finished_ = false; 62 | return this->client_ptr_->async_send_goal(goal_msg, send_goal_options); 63 | } 64 | 65 | void spin_until_finished() 66 | { 67 | while(!finished_ && rclcpp::ok()) 68 | { 69 | rclcpp::spin_some(shared_from_this()); 70 | this->get_clock()->sleep_for(10ms); 71 | } 72 | } 73 | 74 | private: 75 | rclcpp_action::Client::SharedPtr client_ptr_; 76 | std::atomic_bool finished_ = false; 77 | 78 | void goal_response_callback(const GoalHandle::SharedPtr& goal_handle) 79 | { 80 | if (!goal_handle) { 81 | RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); 82 | } else { 83 | RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); 84 | } 85 | } 86 | 87 | void feedback_callback( 88 | GoalHandle::SharedPtr, 89 | const std::shared_ptr feedback) 90 | { 91 | // rclcpp::Duration estimated_time_remaining = feedback->estimated_time_remaining; 92 | // RCLCPP_INFO_STREAM(this->get_logger(), "Estimated time remaining: " << estimated_time_remaining.seconds() << "s"); 93 | } 94 | 95 | void result_callback(const GoalHandle::WrappedResult& result) 96 | { 97 | switch (result.code) 98 | { 99 | case rclcpp_action::ResultCode::SUCCEEDED: 100 | break; 101 | case rclcpp_action::ResultCode::ABORTED: 102 | RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); 103 | return; 104 | case rclcpp_action::ResultCode::CANCELED: 105 | RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); 106 | return; 107 | default: 108 | RCLCPP_ERROR(this->get_logger(), "Unknown result code"); 109 | return; 110 | } 111 | std::cout << "Node finished." << std::endl; 112 | finished_ = true; 113 | } 114 | }; // class WaitForXClient 115 | 116 | int main(int argc, char** argv) 117 | { 118 | rclcpp::init(argc, argv); 119 | auto wait_for_x_client = std::make_shared(); 120 | 121 | auto fut = wait_for_x_client->send_goal_async(10.0); 122 | std::cout << "Waiting for 10 seconds..." << std::endl; 123 | wait_for_x_client->spin_until_finished(); 124 | 125 | // This would be better if it worked: 126 | // if(rclcpp::spin_until_future_complete(wait_for_x_client, fut, 10s) 127 | // != rclcpp::FutureReturnCode::SUCCESS) 128 | // { 129 | // std::cout << "ERROR!" << std::endl; 130 | // } 131 | std::cout << "Done." << std::endl; 132 | 133 | rclcpp::shutdown(); 134 | return 0; 135 | } -------------------------------------------------------------------------------- /ex04_navigation/ex04_actions/src/wait_for_x_client_sync.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | #include "ex04_msgs/action/wait_for_x.hpp" 10 | 11 | #include "rclcpp/rclcpp.hpp" 12 | #include "rclcpp/future_return_code.hpp" 13 | #include "rclcpp_action/rclcpp_action.hpp" 14 | 15 | using namespace std::chrono_literals; 16 | 17 | class WaitForXClientSync : public rclcpp::Node 18 | { 19 | public: 20 | using WaitForX = ex04_msgs::action::WaitForX; 21 | using ClientT = rclcpp_action::Client; 22 | using GoalHandle = ClientT::GoalHandle; 23 | using WrappedResult = ClientT::WrappedResult; 24 | 25 | WaitForXClientSync(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) 26 | : Node("wait_for_x_client_sync", options) 27 | { 28 | this->client_ptr_ = rclcpp_action::create_client( 29 | this, 30 | "wait_for_x"); 31 | } 32 | 33 | /** 34 | * Completely synchronized action call 35 | */ 36 | std::optional send_goal(float duration) 37 | { 38 | auto res_fut = send_goal_async(duration); 39 | 40 | if(rclcpp::spin_until_future_complete(this->get_node_base_interface(), res_fut) 41 | != rclcpp::FutureReturnCode::SUCCESS) 42 | { 43 | RCLCPP_ERROR(this->get_logger(), "ERROR waiting for action response"); 44 | return {}; 45 | } 46 | 47 | return res_fut.get(); 48 | } 49 | 50 | /** 51 | * Synchronized goal call, but asynchronous result call 52 | */ 53 | std::shared_future send_goal_async(float duration) 54 | { 55 | using namespace std::placeholders; 56 | 57 | std::cout << "Waiting connection to action server..." << std::endl; 58 | if (!this->client_ptr_->wait_for_action_server()) 59 | { 60 | RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); 61 | rclcpp::shutdown(); 62 | } 63 | 64 | auto goal_msg = WaitForX::Goal(); 65 | goal_msg.duration = duration; 66 | 67 | RCLCPP_INFO(this->get_logger(), "Sending goal"); 68 | 69 | auto send_goal_options = ClientT::SendGoalOptions(); 70 | send_goal_options.feedback_callback = 71 | std::bind(&WaitForXClientSync::feedback_callback, this, _1, _2); 72 | 73 | // this one is almost instantly finished 74 | auto goal_fut = this->client_ptr_->async_send_goal(goal_msg, send_goal_options); 75 | 76 | // this is required to make sure we wait for the result of our goal request 77 | if(rclcpp::spin_until_future_complete(this->get_node_base_interface(), goal_fut) 78 | != rclcpp::FutureReturnCode::SUCCESS) 79 | { 80 | RCLCPP_ERROR(this->get_logger(), "ERROR waiting for goal request"); 81 | } 82 | 83 | return this->client_ptr_->async_get_result(goal_fut.get()); 84 | } 85 | 86 | private: 87 | ClientT::SharedPtr client_ptr_; 88 | std::atomic_bool finished_ = false; 89 | 90 | void feedback_callback( 91 | GoalHandle::SharedPtr, 92 | const std::shared_ptr feedback) 93 | { 94 | (void)feedback; // this disables warnings for unused variables 95 | // rclcpp::Duration estimated_time_remaining = feedback->estimated_time_remaining; 96 | // RCLCPP_INFO_STREAM(this->get_logger(), "Estimated time remaining: " << estimated_time_remaining.seconds() << "s"); 97 | } 98 | 99 | }; // class WaitForXClientSync 100 | 101 | int main(int argc, char** argv) 102 | { 103 | rclcpp::init(argc, argv); 104 | 105 | // Warning: Sometimes there is an issue: 106 | // - async_send_goal returns goal_future 107 | // - goal_future is quickly finished 108 | // - aysnc_get_result returns goal future object 109 | // - ERROR 110 | // My guess: 111 | // - shortly after async_send_goal, the server has finished its work and sends a result 112 | // - after that, aysnc_get_result is called but the result is gone already 113 | 114 | // create the action client node 115 | auto wait_for_x_client = std::make_shared(); 116 | // synchronous call 117 | auto res_opt = wait_for_x_client->send_goal(10.0); 118 | std::cout << "Done." << std::endl; 119 | 120 | if(res_opt) 121 | { 122 | auto res = *res_opt; 123 | // do something dependend on the result 124 | switch (res.code) 125 | { 126 | case rclcpp_action::ResultCode::SUCCEEDED: 127 | break; 128 | case rclcpp_action::ResultCode::ABORTED: 129 | RCLCPP_ERROR(wait_for_x_client->get_logger(), "Goal was aborted"); 130 | return 0; 131 | case rclcpp_action::ResultCode::CANCELED: 132 | RCLCPP_ERROR(wait_for_x_client->get_logger(), "Goal was canceled"); 133 | return 0; 134 | default: 135 | RCLCPP_ERROR(wait_for_x_client->get_logger(), "Unknown result code"); 136 | return 0; 137 | } 138 | } else { 139 | std::cout << "No result" << std::endl; 140 | return 0; 141 | } 142 | 143 | 144 | std::cout << "GO ON" << std::endl; 145 | 146 | rclcpp::shutdown(); 147 | return 0; 148 | } -------------------------------------------------------------------------------- /ex04_navigation/ex04_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(ex04_msgs) 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(rosidl_default_generators REQUIRED) 15 | find_package(geometry_msgs REQUIRED) 16 | find_package(rclcpp REQUIRED) 17 | find_package(rclcpp_action REQUIRED) 18 | 19 | rosidl_generate_interfaces(${PROJECT_NAME} 20 | "action/WaitForRvizPose.action" 21 | "action/WaitForX.action" 22 | DEPENDENCIES builtin_interfaces geometry_msgs 23 | ) 24 | 25 | ament_export_dependencies( 26 | rosidl_default_runtime 27 | geometry_msgs 28 | ) 29 | 30 | ament_package() -------------------------------------------------------------------------------- /ex04_navigation/ex04_msgs/action/WaitForRvizPose.action: -------------------------------------------------------------------------------- 1 | # input 2 | float32 duration -1.0 3 | --- 4 | # result 5 | geometry_msgs/PoseStamped pose 6 | --- 7 | # feedback -------------------------------------------------------------------------------- /ex04_navigation/ex04_msgs/action/WaitForX.action: -------------------------------------------------------------------------------- 1 | # input 2 | float32 duration 3 | --- 4 | # result 5 | --- 6 | # feedback -------------------------------------------------------------------------------- /ex04_navigation/ex04_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | ex04_msgs 7 | 0.0.0 8 | Actions, services and behavior trees 9 | Alexander Mock 10 | Alexander Mock 11 | BSD 12 | 13 | ament_cmake 14 | rosidl_default_generators 15 | rosidl_interface_packages 16 | 17 | rclcpp 18 | std_msgs 19 | geometry_msgs 20 | action_msgs 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | -------------------------------------------------------------------------------- /ex04_navigation/ex04_nav2_client/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(ex04_nav2_client) 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_action REQUIRED) 16 | find_package(geometry_msgs REQUIRED) 17 | find_package(nav2_msgs REQUIRED) 18 | 19 | ####### 20 | # Nav2 Action Client 21 | ###### 22 | add_executable(nav2_client 23 | src/nav2_client.cpp) 24 | 25 | ament_target_dependencies(nav2_client 26 | rclcpp 27 | rclcpp_action 28 | nav2_msgs 29 | geometry_msgs) 30 | 31 | install(TARGETS 32 | nav2_client 33 | DESTINATION lib/${PROJECT_NAME}) 34 | 35 | ament_package() -------------------------------------------------------------------------------- /ex04_navigation/ex04_nav2_client/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | ex04_nav2_client 7 | 0.0.0 8 | Trigger nav2 from your C++ code 9 | Alexander Mock 10 | Alexander Mock 11 | BSD 12 | 13 | ament_cmake 14 | 15 | rclcpp 16 | std_msgs 17 | geometry_msgs 18 | nav2_msgs 19 | 20 | 21 | ament_cmake 22 | 23 | 24 | -------------------------------------------------------------------------------- /ex04_navigation/ex04_nav2_client/src/nav2_client.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | #include "nav2_msgs/action/navigate_to_pose.hpp" 10 | #include "geometry_msgs/msg/pose_stamped.hpp" 11 | 12 | #include "rclcpp/rclcpp.hpp" 13 | #include "rclcpp/future_return_code.hpp" 14 | #include "rclcpp_action/rclcpp_action.hpp" 15 | 16 | using namespace std::chrono_literals; 17 | 18 | namespace ex04_nav2_client 19 | { 20 | 21 | class Nav2ActionClient : public rclcpp::Node 22 | { 23 | public: 24 | using NavigateToPose = nav2_msgs::action::NavigateToPose; 25 | using GoalHandleNavigateToPose = rclcpp_action::ClientGoalHandle; 26 | 27 | Nav2ActionClient(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()) 28 | : Node("nav2_action_client", options) 29 | { 30 | this->client_ptr_ = rclcpp_action::create_client( 31 | this, 32 | "navigate_to_pose"); 33 | } 34 | 35 | GoalHandleNavigateToPose::SharedPtr send_goal(geometry_msgs::msg::PoseStamped pose) 36 | { 37 | return send_goal_async(pose).get(); 38 | } 39 | 40 | std::shared_future send_goal_async(geometry_msgs::msg::PoseStamped pose) 41 | { 42 | using namespace std::placeholders; 43 | 44 | std::cout << "Waiting for connection to action server..." << std::endl; 45 | if (!this->client_ptr_->wait_for_action_server()) 46 | { 47 | RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); 48 | rclcpp::shutdown(); 49 | } 50 | 51 | auto goal_msg = NavigateToPose::Goal(); 52 | goal_msg.pose = pose; 53 | 54 | RCLCPP_INFO(this->get_logger(), "Sending goal"); 55 | 56 | auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); 57 | 58 | send_goal_options.goal_response_callback = 59 | std::bind(&Nav2ActionClient::goal_response_callback, this, _1); 60 | send_goal_options.feedback_callback = 61 | std::bind(&Nav2ActionClient::feedback_callback, this, _1, _2); 62 | send_goal_options.result_callback = 63 | std::bind(&Nav2ActionClient::result_callback, this, _1); 64 | 65 | finished_ = false; 66 | return this->client_ptr_->async_send_goal(goal_msg, send_goal_options); 67 | } 68 | 69 | void spin_until_finished() 70 | { 71 | while(!finished_ && rclcpp::ok()) 72 | { 73 | rclcpp::spin_some(shared_from_this()); 74 | this->get_clock()->sleep_for(10ms); 75 | } 76 | } 77 | 78 | private: 79 | rclcpp_action::Client::SharedPtr client_ptr_; 80 | std::atomic_bool finished_ = false; 81 | 82 | void goal_response_callback(const GoalHandleNavigateToPose::SharedPtr& goal_handle) 83 | { 84 | if (!goal_handle) { 85 | RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); 86 | } else { 87 | RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); 88 | } 89 | } 90 | 91 | void feedback_callback( 92 | GoalHandleNavigateToPose::SharedPtr, 93 | const std::shared_ptr feedback) 94 | { 95 | rclcpp::Duration estimated_time_remaining = feedback->estimated_time_remaining; 96 | RCLCPP_INFO_STREAM(this->get_logger(), "Estimated time remaining: " << estimated_time_remaining.seconds() << "s"); 97 | } 98 | 99 | void result_callback(const GoalHandleNavigateToPose::WrappedResult& result) 100 | { 101 | switch (result.code) 102 | { 103 | case rclcpp_action::ResultCode::SUCCEEDED: 104 | break; 105 | case rclcpp_action::ResultCode::ABORTED: 106 | RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); 107 | return; 108 | case rclcpp_action::ResultCode::CANCELED: 109 | RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); 110 | return; 111 | default: 112 | RCLCPP_ERROR(this->get_logger(), "Unknown result code"); 113 | return; 114 | } 115 | std::cout << "Node finished." << std::endl; 116 | finished_ = true; 117 | } 118 | }; // class Nav2ActionClient 119 | 120 | } // namespace ex04_nav2_client 121 | 122 | int main(int argc, char** argv) 123 | { 124 | using namespace ex04_nav2_client; 125 | rclcpp::init(argc, argv); 126 | auto nav2_client = std::make_shared(); 127 | 128 | 129 | std::vector goal_list; 130 | 131 | // first goal 132 | geometry_msgs::msg::PoseStamped pose1; 133 | pose1.header.frame_id = "map"; 134 | pose1.header.stamp = nav2_client->now(); 135 | pose1.pose.position.x = 15.0; 136 | pose1.pose.position.y = 19.0; 137 | pose1.pose.position.z = 0.0; 138 | pose1.pose.orientation.x = 0.0; 139 | pose1.pose.orientation.y = 0.0; 140 | pose1.pose.orientation.z = 0.0; 141 | pose1.pose.orientation.w = 1.0; 142 | goal_list.push_back(pose1); 143 | 144 | // second goal 145 | geometry_msgs::msg::PoseStamped pose2; 146 | pose2.header.frame_id = "map"; 147 | pose2.header.stamp = nav2_client->now(); 148 | pose2.pose.position.x = 26.0; 149 | pose2.pose.position.y = 15.4; 150 | pose2.pose.position.z = 0.0; 151 | pose2.pose.orientation.x = 0.0; 152 | pose2.pose.orientation.y = 0.0; 153 | pose2.pose.orientation.z = 0.0; 154 | pose2.pose.orientation.w = 1.0; 155 | goal_list.push_back(pose2); 156 | 157 | while(rclcpp::ok()) 158 | { 159 | for(size_t i = 0; inow(); // update time 164 | auto fut = nav2_client->send_goal_async(goal); 165 | nav2_client->spin_until_finished(); 166 | 167 | // TODO: This should be better. Why does it not work? 168 | // if(rclcpp::spin_until_future_complete(nav2_client, fut) 169 | // != rclcpp::FutureReturnCode::SUCCESS) 170 | // { 171 | // std::cout << "ERROR!" << std::endl; 172 | // } 173 | } 174 | } 175 | 176 | rclcpp::shutdown(); 177 | return 0; 178 | } -------------------------------------------------------------------------------- /ex05_behavior_trees/README.md: -------------------------------------------------------------------------------- 1 | # ex05_behavior_trees 2 | 3 | !! unfinished !! Help is appreciated. 4 | 5 | ## Deliberation Layer 6 | 7 | Action clients are normally implemented inside of high level robot control software, sometimes referred to as [deliberation layer](https://github.com/ros-wg-delib/awesome-ros-deliberation). The two most well-known concepts of deliberation are state machines and behavior trees. Most of the states in a state machine are basically action clients that are calling and monitoring actions provided by the robot using a certain set of parameters, the states are connected via transitions: 8 | 9 | ![smach](./dat/smach.png) 10 | 11 | However, the current trend in the ROS community is to use behavior trees. For behavior trees the core concept stays the same. Nodes in a behavior tree trigger and monitor the actions the robot provides. More to this later. We will start with creating own actions and action server. 12 | 13 | ## BehaviorTree.CPP 14 | 15 | ### First Behavior Tree 16 | 17 | Splitted: 18 | - `ex05_bt_plugins`: only for bt-nodes as plugins for the behavior tree library 19 | - `ex05_behavior_tree`: Loading the nodes and execute a behavior tree 20 | 21 | ### Monitor using Groot 22 | 23 | https://github.com/BehaviorTree/Groot.git 24 | 25 | ### Editing using Groot 26 | 27 | TODO: Create a new behavior tree using Groot editor. 28 | - Load the nodes from `ex05_bt_plugins` 29 | - Create a behavior tree 30 | - Put it into a new node and monitor it 31 | 32 | ### Blackboard 33 | 34 | TODO: Trigger own wait for pose action from a new behavior tree node (bt-node). Write the result in a blackboard entry 35 | 36 | Caution! Bt-nodes are not the same as ROS-nodes! 37 | 38 | ### Use nav2 actions 39 | 40 | 41 | 42 | TODO: 43 | - Load the Nav2 plugins into Groot editor 44 | - Load our plugins into the Groot editor 45 | - Create a BT that connects the results of our wait-for-pose bt-node with a Nav2 compute path bt-node 46 | - Try to create a tree that does the following: 47 | - Wait for pose 48 | - Compute path 49 | - Follow path 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /ex05_behavior_trees/dat/smach.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uos/ros2_tutorial/718bea91b9dd87ac364b35945622fc4fe1c64148/ex05_behavior_trees/dat/smach.png -------------------------------------------------------------------------------- /ex05_behavior_trees/ex05_behavior_trees/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(ex05_behavior_trees) 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_action REQUIRED) 16 | find_package(geometry_msgs REQUIRED) 17 | find_package(ex04_msgs REQUIRED) 18 | find_package(ex05_bt_plugins REQUIRED) 19 | find_package(behaviortree_cpp_v3 REQUIRED) 20 | 21 | 22 | add_executable(bt_dummy src/bt_dummy.cpp) 23 | ament_target_dependencies(bt_dummy 24 | rclcpp 25 | rclcpp_action 26 | geometry_msgs 27 | ex04_msgs 28 | ex05_bt_plugins 29 | behaviortree_cpp_v3 30 | ) 31 | 32 | add_executable(bt_dummy_groot src/bt_dummy_groot.cpp) 33 | ament_target_dependencies(bt_dummy_groot 34 | rclcpp 35 | rclcpp_action 36 | geometry_msgs 37 | ex04_msgs 38 | ex05_bt_plugins 39 | behaviortree_cpp_v3 40 | ) 41 | 42 | install(TARGETS 43 | bt_dummy bt_dummy_groot 44 | DESTINATION lib/${PROJECT_NAME}) 45 | 46 | ament_package() -------------------------------------------------------------------------------- /ex05_behavior_trees/ex05_behavior_trees/include/ex05_behavior_trees/dummy_nodes.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SIMPLE_BT_NODES_H 2 | #define SIMPLE_BT_NODES_H 3 | 4 | #include "behaviortree_cpp_v3/behavior_tree.h" 5 | #include "behaviortree_cpp_v3/bt_factory.h" 6 | 7 | namespace DummyNodes 8 | { 9 | 10 | using BT::NodeStatus; 11 | 12 | NodeStatus CheckBattery(); 13 | 14 | NodeStatus CheckTemperature(); 15 | NodeStatus SayHello(); 16 | 17 | class GripperInterface 18 | { 19 | public: 20 | GripperInterface() : _opened(true) 21 | { 22 | } 23 | 24 | NodeStatus open(); 25 | 26 | NodeStatus close(); 27 | 28 | private: 29 | bool _opened; 30 | }; 31 | 32 | //-------------------------------------- 33 | 34 | // Example of custom SyncActionNode (synchronous action) 35 | // without ports. 36 | class ApproachObject : public BT::SyncActionNode 37 | { 38 | public: 39 | ApproachObject(const std::string& name) : 40 | BT::SyncActionNode(name, {}) 41 | { 42 | } 43 | 44 | // You must override the virtual function tick() 45 | NodeStatus tick() override; 46 | }; 47 | 48 | // Example of custom SyncActionNode (synchronous action) 49 | // with an input port. 50 | class SaySomething : public BT::SyncActionNode 51 | { 52 | public: 53 | SaySomething(const std::string& name, const BT::NodeConfiguration& config) 54 | : BT::SyncActionNode(name, config) 55 | { 56 | } 57 | 58 | // You must override the virtual function tick() 59 | NodeStatus tick() override; 60 | 61 | // It is mandatory to define this static method. 62 | static BT::PortsList providedPorts() 63 | { 64 | return{ BT::InputPort("message") }; 65 | } 66 | }; 67 | 68 | //Same as class SaySomething, but to be registered with SimpleActionNode 69 | NodeStatus SaySomethingSimple(BT::TreeNode& self); 70 | 71 | // Example os Asynchronous node that use StatefulActionNode as base class 72 | class SleepNode : public BT::StatefulActionNode 73 | { 74 | public: 75 | SleepNode(const std::string& name, const BT::NodeConfiguration& config) 76 | : BT::StatefulActionNode(name, config) 77 | {} 78 | 79 | static BT::PortsList providedPorts() 80 | { 81 | // amount of milliseconds that we want to sleep 82 | return{ BT::InputPort("msec") }; 83 | } 84 | 85 | NodeStatus onStart() override 86 | { 87 | int msec = 0; 88 | getInput("msec", msec); 89 | if( msec <= 0 ) 90 | { 91 | // no need to go into the RUNNING state 92 | return NodeStatus::SUCCESS; 93 | } 94 | else { 95 | using namespace std::chrono; 96 | // once the deadline is reached, we will return SUCCESS. 97 | deadline_ = system_clock::now() + milliseconds(msec); 98 | return NodeStatus::RUNNING; 99 | } 100 | } 101 | 102 | /// method invoked by an action in the RUNNING state. 103 | NodeStatus onRunning() override 104 | { 105 | if ( std::chrono::system_clock::now() >= deadline_ ) 106 | { 107 | return NodeStatus::SUCCESS; 108 | } 109 | else { 110 | return NodeStatus::RUNNING; 111 | } 112 | } 113 | 114 | void onHalted() override 115 | { 116 | // nothing to do here... 117 | std::cout << "SleepNode interrupted" << std::endl; 118 | } 119 | 120 | private: 121 | std::chrono::system_clock::time_point deadline_; 122 | }; 123 | 124 | inline void RegisterNodes(BT::BehaviorTreeFactory& factory) 125 | { 126 | static GripperInterface grip_singleton; 127 | 128 | factory.registerSimpleCondition("CheckBattery", std::bind(CheckBattery)); 129 | factory.registerSimpleCondition("CheckTemperature", std::bind(CheckTemperature)); 130 | factory.registerSimpleAction("SayHello", std::bind(SayHello)); 131 | factory.registerSimpleAction("OpenGripper", std::bind(&GripperInterface::open, &grip_singleton)); 132 | factory.registerSimpleAction("CloseGripper", std::bind(&GripperInterface::close, &grip_singleton)); 133 | factory.registerNodeType("ApproachObject"); 134 | factory.registerNodeType("SaySomething"); 135 | } 136 | 137 | } // end namespace 138 | 139 | #endif // SIMPLE_BT_NODES_H -------------------------------------------------------------------------------- /ex05_behavior_trees/ex05_behavior_trees/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | ex05_behavior_trees 7 | 0.0.0 8 | Behavior trees 9 | Alexander Mock 10 | Alexander Mock 11 | BSD 12 | 13 | ament_cmake 14 | 15 | rclcpp 16 | std_msgs 17 | geometry_msgs 18 | ex04_msgs 19 | ex05_bt_plugins 20 | behaviortree_cpp_v3 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | -------------------------------------------------------------------------------- /ex05_behavior_trees/ex05_behavior_trees/src/bt_dummy.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "behaviortree_cpp_v3/bt_factory.h" 3 | #include 4 | 5 | // clang-format off 6 | static const char* xml_text = R"( 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | )"; 21 | 22 | std::string create_path_to_plugin_lib_local_ws( 23 | std::string workspace_path, 24 | std::string package_name, 25 | std::string plugin_name) 26 | { 27 | return workspace_path + "/install/" + package_name + "/lib/lib" + plugin_name + ".so"; 28 | } 29 | 30 | int main(int argc, char** argv) 31 | { 32 | rclcpp::init(argc, argv); 33 | // We use the BehaviorTreeFactory to register our custom nodes 34 | BT::BehaviorTreeFactory factory; 35 | 36 | // load plugin from install directory of your workspace 37 | std::string workspace_path = "/home/amock/ceres_ws"; 38 | std::string package_name = "ex05_bt_plugins"; 39 | std::string plugin_name = "bt_plugin_dummy_nodes"; 40 | 41 | std::string path_to_plugin = create_path_to_plugin_lib_local_ws(workspace_path, package_name, plugin_name); 42 | factory.registerFromPlugin(path_to_plugin); 43 | 44 | auto tree = factory.createTreeFromText(xml_text); 45 | 46 | while(rclcpp::ok()) 47 | { 48 | tree.tickRootWhileRunning(); 49 | } 50 | 51 | return 0; 52 | } 53 | 54 | /* Expected output (in a loop): 55 | * 56 | Loop begin 57 | [ Battery: OK ] 58 | GripperInterface::open 59 | ApproachObject: approach_object 60 | GripperInterface::close 61 | */ 62 | 63 | -------------------------------------------------------------------------------- /ex05_behavior_trees/ex05_behavior_trees/src/bt_dummy_groot.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "behaviortree_cpp_v3/bt_factory.h" 3 | #include 4 | #include 5 | 6 | // clang-format off 7 | static const char* xml_text = R"( 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | )"; 22 | 23 | int main(int argc, char** argv) 24 | { 25 | rclcpp::init(argc, argv); 26 | // We use the BehaviorTreeFactory to register our custom nodes 27 | BT::BehaviorTreeFactory factory; 28 | std::string workspace_path = "/home/amock/ceres_ws"; 29 | factory.registerFromPlugin(workspace_path + "/install/ex05_bt_plugins/lib/libbt_plugin_dummy_nodes.so"); 30 | 31 | 32 | auto tree = factory.createTreeFromText(xml_text); 33 | 34 | // enabling this line we can connect the groot monitor 35 | BT::PublisherZMQ publisher_zmq(tree); 36 | 37 | tree.tickRootWhileRunning(); 38 | 39 | return 0; 40 | } 41 | 42 | /* Expected output: 43 | * 44 | [ Battery: OK ] 45 | GripperInterface::open 46 | ApproachObject: approach_object 47 | GripperInterface::close 48 | */ 49 | 50 | -------------------------------------------------------------------------------- /ex05_behavior_trees/ex05_behavior_trees/src/lib/dummy_nodes.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | 4 | // This function must be implemented in the .cpp file to create 5 | // a plugin that can be loaded at run-time 6 | BT_REGISTER_NODES(factory) 7 | { 8 | DummyNodes::RegisterNodes(factory); 9 | } 10 | 11 | namespace DummyNodes 12 | { 13 | 14 | BT::NodeStatus CheckBattery() 15 | { 16 | std::cout << "[ Battery: OK ]" << std::endl; 17 | return BT::NodeStatus::SUCCESS; 18 | } 19 | 20 | BT::NodeStatus CheckTemperature() 21 | { 22 | std::cout << "[ Temperature: OK ]" << std::endl; 23 | return BT::NodeStatus::SUCCESS; 24 | } 25 | 26 | BT::NodeStatus SayHello() 27 | { 28 | std::cout << "Robot says: Hello World" << std::endl; 29 | return BT::NodeStatus::SUCCESS; 30 | } 31 | 32 | BT::NodeStatus GripperInterface::open() 33 | { 34 | _opened = true; 35 | std::cout << "GripperInterface::open" << std::endl; 36 | return BT::NodeStatus::SUCCESS; 37 | } 38 | 39 | BT::NodeStatus GripperInterface::close() 40 | { 41 | std::cout << "GripperInterface::close" << std::endl; 42 | _opened = false; 43 | return BT::NodeStatus::SUCCESS; 44 | } 45 | 46 | BT::NodeStatus ApproachObject::tick() 47 | { 48 | std::cout << "ApproachObject: " << this->name() << std::endl; 49 | return BT::NodeStatus::SUCCESS; 50 | } 51 | 52 | BT::NodeStatus SaySomething::tick() 53 | { 54 | auto msg = getInput("message"); 55 | if (!msg) 56 | { 57 | throw BT::RuntimeError( "missing required input [message]: ", msg.error() ); 58 | } 59 | 60 | std::cout << "Robot says: " << msg.value() << std::endl; 61 | return BT::NodeStatus::SUCCESS; 62 | } 63 | 64 | BT::NodeStatus SaySomethingSimple(BT::TreeNode &self) 65 | { 66 | auto msg = self.getInput("message"); 67 | if (!msg) 68 | { 69 | throw BT::RuntimeError( "missing required input [message]: ", msg.error() ); 70 | } 71 | 72 | std::cout << "Robot says: " << msg.value() << std::endl; 73 | return BT::NodeStatus::SUCCESS; 74 | } 75 | 76 | } // namespace DummyNodes -------------------------------------------------------------------------------- /ex05_behavior_trees/ex05_bt_plugins/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(ex05_bt_plugins) 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_action REQUIRED) 16 | find_package(geometry_msgs REQUIRED) 17 | find_package(ex04_msgs REQUIRED) 18 | find_package(behaviortree_cpp_v3 REQUIRED) 19 | 20 | # OLD WAY. TODO: 21 | # - Do target_include_directories instead 22 | # - Check if it working with the behavior tree interface 23 | include_directories( 24 | include 25 | ) 26 | 27 | add_library(bt_plugin_dummy_nodes SHARED src/dummy_nodes.cpp) 28 | 29 | ament_target_dependencies(bt_plugin_dummy_nodes 30 | rclcpp 31 | rclcpp_action 32 | geometry_msgs 33 | ex04_msgs 34 | behaviortree_cpp_v3) 35 | target_compile_definitions(bt_plugin_dummy_nodes PRIVATE BT_PLUGIN_EXPORT) 36 | 37 | install(TARGETS 38 | bt_plugin_dummy_nodes 39 | ARCHIVE DESTINATION lib 40 | LIBRARY DESTINATION lib 41 | RUNTIME DESTINATION bin 42 | ) 43 | 44 | install(DIRECTORY include/ 45 | DESTINATION include/ 46 | ) 47 | 48 | ament_export_include_directories( 49 | include 50 | ) 51 | 52 | ament_export_libraries( 53 | bt_plugin_dummy_nodes 54 | ) 55 | 56 | ament_export_dependencies( 57 | rclcpp 58 | rclcpp_action 59 | geometry_msgs 60 | ex04_msgs 61 | behaviortree_cpp_v3 62 | ) 63 | 64 | ament_package() -------------------------------------------------------------------------------- /ex05_behavior_trees/ex05_bt_plugins/ex05_bt_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /ex05_behavior_trees/ex05_bt_plugins/include/ex05_bt_plugins/dummy_nodes.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SIMPLE_BT_NODES_H 2 | #define SIMPLE_BT_NODES_H 3 | 4 | #include "behaviortree_cpp_v3/behavior_tree.h" 5 | #include "behaviortree_cpp_v3/bt_factory.h" 6 | 7 | namespace DummyNodes 8 | { 9 | 10 | using BT::NodeStatus; 11 | 12 | NodeStatus CheckBattery(); 13 | 14 | NodeStatus CheckTemperature(); 15 | NodeStatus SayHello(); 16 | 17 | class GripperInterface 18 | { 19 | public: 20 | GripperInterface() : _opened(true) 21 | { 22 | } 23 | 24 | NodeStatus open(); 25 | 26 | NodeStatus close(); 27 | 28 | private: 29 | bool _opened; 30 | }; 31 | 32 | //-------------------------------------- 33 | 34 | // Example of custom SyncActionNode (synchronous action) 35 | // without ports. 36 | class ApproachObject : public BT::SyncActionNode 37 | { 38 | public: 39 | ApproachObject(const std::string& name) : 40 | BT::SyncActionNode(name, {}) 41 | { 42 | } 43 | 44 | // You must override the virtual function tick() 45 | NodeStatus tick() override; 46 | }; 47 | 48 | // Example of custom SyncActionNode (synchronous action) 49 | // with an input port. 50 | class SaySomething : public BT::SyncActionNode 51 | { 52 | public: 53 | SaySomething(const std::string& name, const BT::NodeConfiguration& config) 54 | : BT::SyncActionNode(name, config) 55 | { 56 | } 57 | 58 | // You must override the virtual function tick() 59 | NodeStatus tick() override; 60 | 61 | // It is mandatory to define this static method. 62 | static BT::PortsList providedPorts() 63 | { 64 | return{ BT::InputPort("message") }; 65 | } 66 | }; 67 | 68 | //Same as class SaySomething, but to be registered with SimpleActionNode 69 | NodeStatus SaySomethingSimple(BT::TreeNode& self); 70 | 71 | // Example os Asynchronous node that use StatefulActionNode as base class 72 | class SleepNode : public BT::StatefulActionNode 73 | { 74 | public: 75 | SleepNode(const std::string& name, const BT::NodeConfiguration& config) 76 | : BT::StatefulActionNode(name, config) 77 | {} 78 | 79 | static BT::PortsList providedPorts() 80 | { 81 | // amount of milliseconds that we want to sleep 82 | return{ BT::InputPort("msec") }; 83 | } 84 | 85 | NodeStatus onStart() override 86 | { 87 | int msec = 0; 88 | getInput("msec", msec); 89 | if( msec <= 0 ) 90 | { 91 | // no need to go into the RUNNING state 92 | return NodeStatus::SUCCESS; 93 | } 94 | else { 95 | using namespace std::chrono; 96 | // once the deadline is reached, we will return SUCCESS. 97 | deadline_ = system_clock::now() + milliseconds(msec); 98 | return NodeStatus::RUNNING; 99 | } 100 | } 101 | 102 | /// method invoked by an action in the RUNNING state. 103 | NodeStatus onRunning() override 104 | { 105 | if ( std::chrono::system_clock::now() >= deadline_ ) 106 | { 107 | return NodeStatus::SUCCESS; 108 | } 109 | else { 110 | return NodeStatus::RUNNING; 111 | } 112 | } 113 | 114 | void onHalted() override 115 | { 116 | // nothing to do here... 117 | std::cout << "SleepNode interrupted" << std::endl; 118 | } 119 | 120 | private: 121 | std::chrono::system_clock::time_point deadline_; 122 | }; 123 | 124 | inline void RegisterNodes(BT::BehaviorTreeFactory& factory) 125 | { 126 | static GripperInterface grip_singleton; 127 | 128 | factory.registerSimpleCondition("CheckBattery", std::bind(CheckBattery)); 129 | factory.registerSimpleCondition("CheckTemperature", std::bind(CheckTemperature)); 130 | factory.registerSimpleAction("SayHello", std::bind(SayHello)); 131 | factory.registerSimpleAction("OpenGripper", std::bind(&GripperInterface::open, &grip_singleton)); 132 | factory.registerSimpleAction("CloseGripper", std::bind(&GripperInterface::close, &grip_singleton)); 133 | factory.registerNodeType("ApproachObject"); 134 | factory.registerNodeType("SaySomething"); 135 | } 136 | 137 | } // end namespace 138 | 139 | #endif // SIMPLE_BT_NODES_H -------------------------------------------------------------------------------- /ex05_behavior_trees/ex05_bt_plugins/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | ex05_bt_plugins 7 | 0.0.0 8 | Behavior trees plugins 9 | Alexander Mock 10 | Alexander Mock 11 | BSD 12 | 13 | ament_cmake 14 | 15 | rclcpp 16 | std_msgs 17 | geometry_msgs 18 | ex04_msgs 19 | 20 | 21 | ament_cmake 22 | 23 | 24 | -------------------------------------------------------------------------------- /ex05_behavior_trees/ex05_bt_plugins/src/dummy_nodes.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | using namespace std::chrono_literals; 7 | 8 | // This function must be implemented in the .cpp file to create 9 | // a plugin that can be loaded at run-time 10 | BT_REGISTER_NODES(factory) 11 | { 12 | DummyNodes::RegisterNodes(factory); 13 | } 14 | 15 | namespace DummyNodes 16 | { 17 | 18 | BT::NodeStatus CheckBattery() 19 | { 20 | std::cout << "Checking battery ..." << std::endl; 21 | std::this_thread::sleep_for(3s); 22 | std::cout << "[ Battery: OK ]" << std::endl; 23 | return BT::NodeStatus::SUCCESS; 24 | } 25 | 26 | BT::NodeStatus CheckTemperature() 27 | { 28 | std::cout << "Checking temperature ..." << std::endl; 29 | std::this_thread::sleep_for(3s); 30 | std::cout << "[ Temperature: OK ]" << std::endl; 31 | return BT::NodeStatus::SUCCESS; 32 | } 33 | 34 | BT::NodeStatus SayHello() 35 | { 36 | std::cout << "Robot says: Hello World" << std::endl; 37 | return BT::NodeStatus::SUCCESS; 38 | } 39 | 40 | BT::NodeStatus GripperInterface::open() 41 | { 42 | std::cout << "Started to open the gripper ..." << std::endl; 43 | std::this_thread::sleep_for(3s); 44 | _opened = true; 45 | std::cout << "Finished." << std::endl; 46 | return BT::NodeStatus::SUCCESS; 47 | } 48 | 49 | BT::NodeStatus GripperInterface::close() 50 | { 51 | std::cout << "Started to close the gripper ..." << std::endl; 52 | std::this_thread::sleep_for(3s); 53 | _opened = false; 54 | std::cout << "Finished." << std::endl; 55 | return BT::NodeStatus::SUCCESS; 56 | } 57 | 58 | BT::NodeStatus ApproachObject::tick() 59 | { 60 | std::cout << "ApproachObject: " << this->name() << " ..." << std::endl; 61 | std::this_thread::sleep_for(5s); 62 | std::cout << "Finished." << std::endl; 63 | return BT::NodeStatus::SUCCESS; 64 | } 65 | 66 | BT::NodeStatus SaySomething::tick() 67 | { 68 | auto msg = getInput("message"); 69 | if (!msg) 70 | { 71 | throw BT::RuntimeError( "missing required input [message]: ", msg.error() ); 72 | } 73 | 74 | std::cout << "Robot says: " << msg.value() << std::endl; 75 | return BT::NodeStatus::SUCCESS; 76 | } 77 | 78 | BT::NodeStatus SaySomethingSimple(BT::TreeNode &self) 79 | { 80 | auto msg = self.getInput("message"); 81 | if (!msg) 82 | { 83 | throw BT::RuntimeError( "missing required input [message]: ", msg.error() ); 84 | } 85 | 86 | std::cout << "Robot says: " << msg.value() << std::endl; 87 | return BT::NodeStatus::SUCCESS; 88 | } 89 | 90 | } // namespace DummyNodes -------------------------------------------------------------------------------- /media/ceres_vid_preview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uos/ros2_tutorial/718bea91b9dd87ac364b35945622fc4fe1c64148/media/ceres_vid_preview.png --------------------------------------------------------------------------------