├── README.md ├── data ├── images │ ├── baboon.png │ └── mariachi.jpg └── rosbags │ └── data ├── robovision_1 ├── CMakeLists.txt ├── README.md ├── package.xml ├── robovision_images │ └── __init__.py ├── scripts │ └── my_subscriber.py └── src │ ├── my_camera_publisher.cpp │ ├── my_publisher.cpp │ └── my_subscriber.cpp ├── robovision_2 ├── CMakeLists.txt ├── README.md ├── images │ ├── baboon_gray.jpg │ ├── baboon_mask_blue.jpg │ ├── baboon_mask_red.jpg │ ├── baboon_number.jpg │ ├── baboon_rotated.jpg │ └── digital_image.jpg ├── package.xml ├── robovision_processing │ └── __init__.py ├── scripts │ └── my_processing.py └── src │ └── my_processing.cpp ├── robovision_3 ├── CMakeLists.txt ├── README.md ├── images │ ├── point_depth.jpg │ ├── rgbd_view.jpg │ ├── scene_box.jpg │ ├── scene_crop.jpg │ ├── scene_full.jpg │ ├── scene_person.jpg │ ├── xy_view.jpg │ ├── xz_view.jpg │ └── yz_view.jpg ├── package.xml ├── robovision_rgbd │ └── __init__.py ├── scripts │ └── rgbd_reader.py └── src │ └── rgbd_reader.cpp ├── robovision_4 ├── CMakeLists.txt ├── README.md ├── package.xml ├── robovision_services │ └── __init__.py ├── scripts │ ├── robovision_client.py │ └── robovision_service.py └── src │ ├── robovision_client.cpp │ └── robovision_service.cpp └── robovision_interfaces ├── CMakeLists.txt ├── msg ├── ObjectCentroid.msg └── ObjectCentroids.msg ├── package.xml └── srv └── GetPointCenter.srv /README.md: -------------------------------------------------------------------------------- 1 | # An Introduction to Robot Vision 2 | 3 | We present a short introduction to Robot Vision. We first present the basic concepts of image publishers and subscribers in ROS2 and then we apply some basic commands to introduce the students to the digital image processing theory; finally, we present some RGBD and point cloud notions and applications. 4 | 5 | For a ROS1 version of this project, please refer to [here](https://github.com/ARTenshi/robovision_ros1). 6 | 7 | # Prerequisites 8 | 9 | Things that you need to install the software and how to install them 10 | 11 | ``` 12 | You should have ROS2 installed. 13 | You should have OpenCV for ROS2 installed. 14 | ``` 15 | # Installation 16 | 17 | ## 0. Requirements 18 | 19 | Install git: 20 | 21 | ``` 22 | sudo apt-get install git 23 | ``` 24 | 25 | ## 1. ROS2 Install: 26 | 27 | ### ROS2 Humble for Ubuntu 22.04: 28 | 29 | Follow the indications here: 30 | 31 | ``` 32 | https://docs.ros.org/en/humble/Installation.html 33 | ``` 34 | 35 | ### `cv_bridge` 36 | 37 | If you already have ROS Humble installed, you can reinstall `cv_bridge` with the following command: 38 | 39 | ``` 40 | sudo apt-get install --reinstall ros-humble-cv-bridge 41 | ``` 42 | 43 | ## 2. Get the introduction to robot vision libraries: 44 | 45 | ### 2.1 Clone this repository 46 | 47 | First, create a workspace: 48 | 49 | ``` 50 | cd ~ 51 | mkdir -p robovision_ros2_ws/src 52 | cd ~/robovision_ros2_ws/src 53 | git clone https://github.com/ARTenshi/robovision_ros2.git 54 | ``` 55 | 56 | ### 2.2 Download additional data 57 | 58 | This project requires additional data files, available [here (Google Drive)](https://bit.ly/3WjWnI2). 59 | 60 | Download those folders into the `~/robovision_ros2_ws/src/robovision_ros2/data/rosbags/` folder. 61 | 62 | ### 2.3 Compile your code 63 | 64 | ``` 65 | cd ~/robovision_ros2_ws 66 | colcon build 67 | ``` 68 | 69 | #### Troubleshooting 70 | 71 | If there are errors with `cv_bridge`, you might need to compile it from source. 72 | 73 | 1. Install Dependencies 74 | ``` 75 | sudo apt-get update 76 | sudo apt-get install -y python3-opencv libopencv-dev ros-humble-cv-bridge-msgs 77 | ``` 78 | 79 | 2. Set Up Workspace 80 | ``` 81 | mkdir -p ~/ros2_ws/src && cd ~/ros2_ws/src 82 | git clone https://github.com/ros-perception/vision_opencv.git 83 | cd vision_opencv && git checkout humble 84 | ``` 85 | 86 | 3. Build and Source 87 | ``` 88 | cd ~/ros2_ws 89 | colcon build --packages-select cv_bridge 90 | source ~/ros2_ws/install/setup.bash 91 | ``` 92 | 93 | (Optional) Add to `.bashrc`: 94 | ``` 95 | echo "source ~/ros2_ws/install/setup.bash" >> ~/.bashrc 96 | ``` 97 | 98 | ### 2.3 Test the code 99 | 100 | Run the following command in a terminal: 101 | 102 | ``` 103 | source ~/robovision_ros2_ws/install/setup.bash 104 | ros2 run robovision_images my_publisher ~/robovision_ros2_ws/src/robovision_ros2/data/images/baboon.png 105 | ``` 106 | 107 | 108 | In a second terminal, run these commands: 109 | 110 | ``` 111 | source ~/robovision_ros2_ws/install/setup.bash 112 | ros2 run robovision_images my_subscriber 113 | ``` 114 | 115 | # Developing 116 | 117 | Basic concepts on ROS2 image publishers and subscribers can be found here: 118 | 119 | > [Lesson 1](https://github.com/ARTenshi/robovision_ros2/tree/main/robovision_1) 120 | 121 | To learn how to process RGB images, follow the indications here: 122 | 123 | > [Lesson 2](https://github.com/ARTenshi/robovision_ros2/tree/main/robovision_2) 124 | 125 | To work with RGBD images, enter here: 126 | 127 | > [Lesson 3](https://github.com/ARTenshi/robovision_ros2/tree/main/robovision_3) 128 | 129 | Finally, how to use services and clients in ROS2 can be seen here: 130 | 131 | > [Lesson 4](https://github.com/ARTenshi/robovision_ros2/tree/main/robovision_4) 132 | 133 | 134 | # Authors 135 | 136 | * **Luis Contreras** - [ARTenshi](https://artenshi.github.io/) 137 | * **Hiroyuki Okada** - [AIBot](http://aibot.jp/) 138 | -------------------------------------------------------------------------------- /data/images/baboon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ARTenshi/robovision_ros2/a825a9236d40c8d98e7793a570fb3cbf49d2238b/data/images/baboon.png -------------------------------------------------------------------------------- /data/images/mariachi.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ARTenshi/robovision_ros2/a825a9236d40c8d98e7793a570fb3cbf49d2238b/data/images/mariachi.jpg -------------------------------------------------------------------------------- /data/rosbags/data: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ARTenshi/robovision_ros2/a825a9236d40c8d98e7793a570fb3cbf49d2238b/data/rosbags/data -------------------------------------------------------------------------------- /robovision_1/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(robovision_images) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(ament_cmake_python REQUIRED) 11 | find_package(rclcpp REQUIRED) 12 | 13 | find_package(OpenCV REQUIRED) 14 | find_package(cv_bridge REQUIRED) 15 | find_package(sensor_msgs REQUIRED) 16 | find_package(std_msgs REQUIRED) 17 | 18 | include_directories(${cv_bridge_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) 19 | 20 | # C++ section 21 | add_executable(my_publisher src/my_publisher.cpp) 22 | ament_target_dependencies(my_publisher 23 | rclcpp 24 | cv_bridge 25 | sensor_msgs 26 | OpenCV 27 | ) 28 | 29 | add_executable(my_camera_publisher src/my_camera_publisher.cpp) 30 | ament_target_dependencies(my_camera_publisher 31 | rclcpp 32 | cv_bridge 33 | sensor_msgs 34 | OpenCV 35 | ) 36 | 37 | add_executable(my_subscriber src/my_subscriber.cpp) 38 | ament_target_dependencies(my_subscriber 39 | rclcpp 40 | cv_bridge 41 | sensor_msgs 42 | OpenCV 43 | ) 44 | 45 | # Python section 46 | ament_python_install_package(${PROJECT_NAME}) 47 | 48 | # Ensure both are included 49 | install(TARGETS 50 | my_publisher 51 | my_camera_publisher 52 | my_subscriber 53 | DESTINATION lib/${PROJECT_NAME} 54 | ) 55 | install(PROGRAMS 56 | scripts/my_subscriber.py 57 | DESTINATION lib/${PROJECT_NAME} 58 | ) 59 | 60 | 61 | ament_package() 62 | -------------------------------------------------------------------------------- /robovision_1/README.md: -------------------------------------------------------------------------------- 1 | # Image Publishers and Subscribers in ROS2 2 | 3 | The goal of this repository is to introduce students to image publishers and subscribers using ROS2 and OpenCV. 4 | 5 | # 0 CMakeList.txt and package.xml 6 | 7 | When we build and install a package in ROS2, we need to give the compiler some information regarding names and libraries. This guide explains how to use the `CMakeLists.txt` and `package.xml` files to configure and build a package in ROS2. 8 | 9 | ## 0.1 `package.xml` 10 | The `package.xml` file contains package metadata and dependency declarations. 11 | 12 | - **Key Sections**: 13 | - ``: Defines the package name (`robovision_images`). 14 | - ``: Specifies the package version. 15 | - ``: Provides the maintainer and contact email. 16 | - ``: Placeholder for the license. 17 | - ``: Tools for building the package, such as `ament_cmake` and `ament_cmake_python`. 18 | - ``: Runtime dependencies like `rclcpp`, `cv_bridge`, `sensor_msgs`, and `std_msgs`. 19 | - ``: Dependencies for testing, such as `ament_lint_auto`. 20 | 21 | ## 0.2 `CMakeLists.txt` 22 | The `CMakeLists.txt` file defines how the package is built. 23 | 24 | - **Key Components**: 25 | - Specifies the minimum CMake version and compiler options. 26 | - Locates dependencies (`rclcpp`, `cv_bridge`, `OpenCV`, etc.). 27 | - Declares include directories for OpenCV and `cv_bridge`. 28 | - Adds executables for C++ nodes like `my_publisher` and `my_camera_publisher` and links their dependencies. 29 | - Installs Python scripts and C++ executables to the appropriate directories. 30 | - Integrates the package with ROS2 using `ament_package()`. 31 | 32 | ## 0.3 Example: `robovision_images` 33 | 34 | ### 0.3.1 `package.xml` 35 | 36 | We declare the name of our project 37 | 38 | ```xml 39 | robovision_images 40 | ``` 41 | 42 | We are using both C++ and Python code in our project, and therefore, we need to declare it: 43 | 44 | ```xml 45 | ament_cmake 46 | ament_cmake_python 47 | 48 | rclcpp 49 | rclpy 50 | ``` 51 | 52 | Additionally, we need to add all the ros-based dependencies we will use in our project 53 | 54 | ```xml 55 | cv_bridge 56 | sensor_msgs 57 | std_msgs 58 | ``` 59 | 60 | ### 0.3.2 `CMakeList.txt` 61 | 62 | First, we declare the name of our project 63 | 64 | ```cmake 65 | project(robovision_images) 66 | ``` 67 | 68 | Similarly, we need to declare that we are using both C++ and Python code 69 | 70 | ```cmake 71 | find_package(ament_cmake REQUIRED) 72 | find_package(ament_cmake_python REQUIRED) 73 | find_package(rclcpp REQUIRED) 74 | ``` 75 | and the dependencies we are using 76 | 77 | ```cmake 78 | find_package(OpenCV REQUIRED) 79 | find_package(cv_bridge REQUIRED) 80 | find_package(sensor_msgs REQUIRED) 81 | find_package(std_msgs REQUIRED) 82 | ``` 83 | 84 | To build a project, we need to specify which files we want to compile and how we are going to refer them to our package. For C++ files, we declare them as in the example here 85 | 86 | ```cmake 87 | add_executable(my_publisher src/my_publisher.cpp) 88 | ament_target_dependencies(my_publisher 89 | rclcpp 90 | cv_bridge 91 | sensor_msgs 92 | OpenCV 93 | ) 94 | ``` 95 | 96 | and for Python, we use 97 | 98 | ```cmake 99 | ament_python_install_package(${PROJECT_NAME}) 100 | ``` 101 | 102 | Finally, we need to install them in our install folder to be able to source them later 103 | 104 | ```cmake 105 | install(TARGETS 106 | my_publisher 107 | DESTINATION lib/${PROJECT_NAME} 108 | ) 109 | install(PROGRAMS 110 | scripts/my_subscriber.py 111 | DESTINATION lib/${PROJECT_NAME} 112 | ) 113 | ``` 114 | 115 | This is a brief introduction to how to use these files to configure your project. Please inspect those files in each section! 116 | 117 | # 1. ROS Publishers and Subcribers 118 | 119 | We assume the students have a notion of these subjects. If it is not the case, they can start here, for C++: 120 | 121 | > https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Publisher-And-Subscriber.html 122 | 123 | and here, for Python: 124 | 125 | > https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Py-Publisher-And-Subscriber.html 126 | 127 | ROS2 is designed for object-oriented programming (OOP), and therefore we will provide solutions based on such paradigm (i.e. Class-based templates). 128 | 129 | The basic **C++** structure is as follows: 130 | 131 | ```cpp 132 | #include "rclcpp/rclcpp.hpp" 133 | 134 | class MyCustomNode : public rclcpp::Node // CHANGE CLASS NAME 135 | { 136 | public: 137 | MyCustomNode() : Node("node_name") // CHANGE CLASS AND NODE NAME 138 | { 139 | } 140 | 141 | private: 142 | }; 143 | 144 | int main(int argc, char **argv) 145 | { 146 | rclcpp::init(argc, argv); 147 | auto node = std::make_shared(); // CHANGE CLASS NAME 148 | rclcpp::spin(node); 149 | rclcpp::shutdown(); 150 | return 0; 151 | } 152 | ``` 153 | 154 | While the basic **Python** structure is: 155 | 156 | ```python 157 | #!/usr/bin/env python3 158 | import rclpy 159 | from rclpy.node import Node 160 | 161 | 162 | class MyCustomNode(Node): # CHANGE CLASS NAME 163 | def __init__(self): 164 | super().__init__("node_name") # CHANGE NODE NAME 165 | 166 | 167 | def main(args=None): 168 | rclpy.init(args=args) 169 | node = MyCustomNode() # CHANGE CLASS NAME 170 | rclpy.spin(node) 171 | rclpy.shutdown() 172 | 173 | 174 | if __name__ == "__main__": 175 | main() 176 | ``` 177 | 178 | 179 | # 1.1 Static image publisher 180 | 181 | Have a look at the `my_publisher.cpp` file. This file presents the basic structure to construct a **publisher** in ROS2. 182 | 183 | ## 1.1.1 Create a publisher 184 | 185 | The `main` section remains the same, and we use it to call our class; however, we have modified it to get some arguments. Within our `ImagePublisherNode` class, first, we need to create a node object and give it a unique name: 186 | 187 | ```cpp 188 | ImagePublisherNode(const std::string & image_path) : Node("image_publisher") 189 | ``` 190 | 191 | Then, we create a ROS2 publisher and give it a unique name to be shown as a ROS2 topic, so anyone else can access it without confusion. The structure is simple: create_publisher("topic/name", 10). In this case, the type is `sensor_msgs::msg::Image` and the name of our output topic is `camera/image`, as follows: 192 | 193 | ```cpp 194 | image_publisher_ = this->create_publisher("camera/image", 10); 195 | 196 | ``` 197 | 198 | Now, let's gather some data. We will open an image using OpenCV and then publish it in our topic. First, we read the image: 199 | 200 | ```cpp 201 | cv::Mat cv_image = cv::imread(image_path, cv::IMREAD_COLOR); 202 | if (cv_image.empty()) { 203 | RCLCPP_ERROR(this->get_logger(), "Failed to load image from path: %s", image_path.c_str()); 204 | return; 205 | } 206 | ``` 207 | 208 | and we convert it to a **ROS message**, the type of data that can be sent through the ROS framework: 209 | 210 | ```cpp 211 | image_msg_ = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", cv_image).toImageMsg(); 212 | ``` 213 | 214 | Now we can publish this message. 215 | 216 | We can do it once (`image_publisher_->publish(*image_msg_)`), but it means that the information will be only available for a fraction of time; if you don't access it at that very moment, you won't be able to use it anymore. So, let's publish it all the time! We first decide on a frame rate, which is the number of **frames per second** (fps). In general, we consider **real-time** something around 30 fps. We use a `while` loop to publish our image at 30 Hz (i.e. 30 fps). In ROS2, we need to create a **tiner**, that will iterate at a given frequency once the node starts to spin in the `main` section: 217 | 218 | ```cpp 219 | image_timer_ = this->create_wall_timer(std::chrono::milliseconds(30), 220 | std::bind(&ImagePublisherNode::publish_image, this)); 221 | ``` 222 | 223 | This **timer** will call a function (`ImagePublisherNode::publish_image`) where we can publish our image: 224 | 225 | ```cpp 226 | void publish_image() 227 | { 228 | image_publisher_->publish(*image_msg_); 229 | } 230 | ``` 231 | 232 | Finally, notice that we need to define our variables; in particular, in C++, our ROS2 variables are all shared pointers and we need to define them as such: 233 | 234 | ```cpp 235 | sensor_msgs::msg::Image::SharedPtr image_msg_; 236 | 237 | rclcpp::Publisher::SharedPtr image_publisher_; 238 | rclcpp::TimerBase::SharedPtr image_timer_; 239 | ``` 240 | 241 | And that's it, we have our first ROS2 publisher. 242 | 243 | ## 1.1.2 Test your code 244 | 245 | Run the following command in a terminal: 246 | 247 | ```bash 248 | ros2 topic list 249 | ``` 250 | 251 | You should see something like: 252 | 253 | ```bash 254 | /parameter_events 255 | /rosout 256 | ``` 257 | 258 | Now, in the same terminal, run the following commands: 259 | 260 | ```bash 261 | source ~/robovision_ros2_ws/install/setup.bash 262 | ros2 run robovision_images my_publisher ~/robovision_ros2_ws/src/robovision_ros2/data/images/baboon.png 263 | ``` 264 | 265 | Then, in a new terminal, run this command again: 266 | 267 | ```bash 268 | ros2 topic list 269 | ``` 270 | 271 | Do you remember this line `image_publisher_ = this->create_publisher("camera/image", 10);`? Well, now we can see our topic `/camera/image`! Furthermore, we can get the details of it. If we enter the command: 272 | 273 | ```bash 274 | ros2 topic info /camera/image 275 | ``` 276 | 277 | we can see: 278 | 279 | ```bash 280 | Type: sensor_msgs/msg/Image 281 | Publisher count: 1 282 | Subscription count: 0 283 | 284 | ``` 285 | 286 | Finally, enter: 287 | 288 | ```bash 289 | ros2 node list 290 | ``` 291 | 292 | and 293 | 294 | ```bash 295 | ros2 node info /image_publisher 296 | ``` 297 | 298 | we see: 299 | 300 | ```bash 301 | image_publisher 302 | Publishers: 303 | /camera/image: sensor_msgs/msg/Image 304 | ``` 305 | 306 | So, we can see that our topic is a *sensor_msgs/msg/Image* data and that the *Publisher* corresponds to the name we gave it when we defined the node class a few lines above `ImagePublisherNode(const std::string & image_path) : Node("image_publisher")`. However, we don't have any *Subscriber* yet. Let's solve it in Section 1.3. 307 | 308 | ## 1.1.3 Homework 1.1 309 | 310 | * Add a new publisher in your code that publishes a scaled version by half of the original image. 311 | 312 | To give you an idea of how ROS2 works, we will help you to solve this task this time, but you are expected to solve it all by yourself. 313 | 314 | Do you remember our ROS2 publisher? We need one publisher per topic, so let's add a new one (don't forget to give a different and unique name to each topic, in this case, we named it `camera/scaled_image`): 315 | 316 | ```cpp 317 | scaled_image_publisher_ = this->create_publisher("camera/scaled_image", 10); 318 | ``` 319 | 320 | Don't forget to define it: 321 | 322 | ```cpp 323 | rclcpp::Publisher::SharedPtr scaled_image_publisher_; 324 | ``` 325 | 326 | After a short search on the internet, we found that, to scale an image in OpenCV, we can use the following command: 327 | 328 | ```cpp 329 | cv::Mat scaled_image; 330 | cv::resize(cv_image, scaled_image, cv::Size(), 0.5, 0.5, CV_INTER_AREA); 331 | ``` 332 | 333 | Now, we need to create a new image message: 334 | 335 | ```cpp 336 | scaled_image_msg_ = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", scaled_image).toImageMsg(); 337 | ``` 338 | 339 | and publish our scaled image: 340 | 341 | ```cpp 342 | scaled_image_publisher_->publish(*scaled_image_msg_); 343 | ``` 344 | 345 | Your code should look something like: 346 | 347 | ```cpp 348 | #include "rclcpp/rclcpp.hpp" 349 | 350 | #include 351 | #include 352 | #include 353 | 354 | class ImagePublisherNode : public rclcpp::Node 355 | { 356 | public: 357 | ImagePublisherNode(const std::string & image_path) : Node("image_publisher") 358 | { 359 | image_publisher_ = this->create_publisher("camera/image", 10); 360 | scaled_image_publisher_ = this->create_publisher("camera/scaled_image", 10); 361 | 362 | // Load the image from file 363 | cv::Mat cv_image = cv::imread(image_path, cv::IMREAD_COLOR); 364 | if (cv_image.empty()) { 365 | RCLCPP_ERROR(this->get_logger(), "Failed to load image from path: %s", image_path.c_str()); 366 | return; 367 | } 368 | 369 | //Resize your image 370 | cv::Mat scaled_image; 371 | cv::resize(cv_image, scaled_image, cv::Size(), 0.5, 0.5, CV_INTER_AREA); 372 | 373 | //Convert the output data into a ROS message format 374 | image_msg_ = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", cv_image).toImageMsg(); 375 | scaled_image_msg_ = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", scaled_image).toImageMsg(); 376 | 377 | image_timer_ = this->create_wall_timer(std::chrono::milliseconds(30), 378 | std::bind(&ImagePublisherNode::publish_image, this)); 379 | 380 | RCLCPP_INFO(this->get_logger(), "Starting image_publisher application in cpp..."); 381 | } 382 | 383 | private: 384 | 385 | sensor_msgs::msg::Image::SharedPtr image_msg_, scaled_image_msg_; 386 | 387 | rclcpp::Publisher::SharedPtr image_publisher_; 388 | rclcpp::Publisher::SharedPtr scaled_image_publisher_; 389 | rclcpp::TimerBase::SharedPtr image_timer_; 390 | 391 | void publish_image() 392 | { 393 | image_publisher_->publish(*image_msg_); 394 | scaled_image_publisher_->publish(*scaled_image_msg_); 395 | } 396 | }; 397 | 398 | int main(int argc, char **argv) 399 | { 400 | rclcpp::init(argc, argv); 401 | 402 | if (argc != 2) { 403 | RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Usage: my_publisher "); 404 | rclcpp::shutdown(); 405 | return 1; 406 | } 407 | 408 | std::string image_path = argv[1]; 409 | 410 | auto node = std::make_shared(image_path); 411 | rclcpp::spin(node); 412 | 413 | rclcpp::shutdown(); 414 | return 0; 415 | } 416 | ``` 417 | 418 | Now let's test it! 419 | 420 | First, we need to compile our code, in a new terminal run: 421 | 422 | ```bash 423 | cd ~/robovision_ros2_ws 424 | colcon build 425 | ``` 426 | 427 | Now, run the following command: 428 | 429 | ```bash 430 | source ~/robovision_ros2_ws/install/setup.bash 431 | ros2 run robovision_images my_publisher ~/robovision_ros2_ws/src/robovision_ros2/data/images/baboon.png 432 | ``` 433 | 434 | Finally, in a new terminal, run this command: 435 | 436 | ```bash 437 | ros2 topic list 438 | ``` 439 | 440 | and 441 | 442 | ```bash 443 | ros2 node info /image_publisher 444 | ``` 445 | 446 | What can you see? Please, explain. 447 | 448 | 449 | # 1.2 Camera publisher 450 | 451 | Have a look at the `my_camera_publisher.cpp` file. This file presents the basic structure to open a camera and create a **publisher** in ROS2. Can you note the similarities and differences between static image and camera publishers? 452 | 453 | ## 1.2.1 Create a publisher 454 | 455 | First, we started our node as before: 456 | 457 | ```cpp 458 | image_publisher_ = this->create_publisher("camera/image", 10); 459 | ``` 460 | 461 | Please note that we use the same topic name for the static image and the camera publishers, so you can only use one of them at a time. 462 | 463 | Let's open our camera! Every camera connected to your computer has an ID number, starting from 0. If you have a laptop with an extra USB camera attached to it, the laptop's camera will have ID 0 and the USC camera ID 1, and so on. In any case, provided you have at least one camera connected to your computer, there will be a device with ID 0 and, therefore, the default value is zero. We may have a hardcoded ID value, or receive it as a dynamic argument when running our node; the latter case is preferred so, let's do it. 464 | 465 | Now, remember how we enter the image path in our previous code, in the `main` function: 466 | 467 | ```cpp 468 | if (argc != 2) { 469 | RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Usage: my_publisher "); 470 | rclcpp::shutdown(); 471 | return 1; 472 | } 473 | 474 | std::string image_path = argv[1]; 475 | ``` 476 | 477 | However, ROS has a built-in option to enter dynamic arguments, and we will use it from now on -- this will be useful when creating launch files. To do this, in our class definition, for a camera_index variable, we declare: 478 | 479 | ```cpp 480 | // Declare and get the camera index parameter (default is 0) 481 | this->declare_parameter("camera_index", 0); 482 | int camera_index = this->get_parameter("camera_index").as_int(); 483 | ``` 484 | 485 | Then, the following lines should open a camera with ID camera_index: 486 | 487 | ```cpp 488 | // Open the camera 489 | capture_.open(camera_index); 490 | 491 | // Check if the camera opened successfully 492 | if (!capture_.isOpened()) 493 | { 494 | RCLCPP_ERROR(this->get_logger(), "Failed to open camera"); 495 | throw std::runtime_error("Camera not available"); 496 | } 497 | ``` 498 | 499 | Now, we should notice that, unlike static images, video sequences change in time and therefore we should update our frame at a desired frame rate (depending on your device specifications). Therefore, we need to get a new frame in our timer callback function (`capture_ >> cv_image;`) and publish it. In general, a camera takes some time to start after you call it, so we need to wait until our program starts receiving a video stream (we use the `!capture_.isOpened()` to do that). Then, our code to read and publish camera images is: 500 | 501 | ```cpp 502 | void publish_image() 503 | { 504 | cv::Mat cv_image; 505 | capture_ >> cv_image; // Capture an image frame 506 | 507 | if (cv_image.empty()) 508 | { 509 | RCLCPP_WARN(this->get_logger(), "Empty frame captured"); 510 | retu0rn; 511 | } 512 | 513 | //Convert the output data into a ROS message format 514 | sensor_msgs::msg::Image::SharedPtr image_msg_; 515 | image_msg_ = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", cv_image).toImageMsg(); 516 | image_publisher_->publish(*image_msg_); 517 | } 518 | ``` 519 | 520 | ## 1.2.2 Test your code 521 | 522 | First, we need to compile our code, in a new terminal run: 523 | 524 | ```bash 525 | cd ~/robovision_ros2_ws 526 | colcon build 527 | ``` 528 | 529 | Now, run the next command: 530 | 531 | ```bash 532 | ros2 topic list 533 | ``` 534 | 535 | You should see something like this: 536 | 537 | ```bash 538 | /parameter_events 539 | /rosout 540 | ``` 541 | 542 | Now, in the same terminal, run the following commands: 543 | 544 | ```bash 545 | source ~/robovision_ros2_ws/install/setup.bash 546 | ros2 run robovision_images my_camera_publisher 547 | ``` 548 | 549 | Then, again, in a new terminal, run this command: 550 | 551 | ```bash 552 | ros2 topic list 553 | ``` 554 | 555 | Can you explain the output? How do you get extra information from your topic? 556 | 557 | If you want to test more than one camera, you can specify using the `--ros-args` and, for a parameter declaration, `-p param_name:=param_value`; in this case, we set the `camera_index` variable with your camera index (as an example, we declare it as 0): 558 | 559 | ```bash 560 | ros2 run robovision_images my_camera_publisher --ros-args -p camera_index:=0 561 | ``` 562 | 563 | ## 1.2.3 Homework 1.2 564 | 565 | * Add a new publisher in your code that publishes a scaled version by half of the original video frame. 566 | 567 | **Hint** You should start your new publisher outside the timer function `publish_image()` but process the scaled image inside it. 568 | 569 | 570 | # 1.3 Image subscriber 571 | 572 | Now that we have a stream of images being published in ROS, let's do something with them. Have a look at the `my_subscriber.cpp` and `my_subscriber.py` files. The important part here is the **callback function**. Again, review your concepts on ROS publishers and subscribers using the links provided before. 573 | 574 | ## 1.3.1 Create a subscriber 575 | 576 | ### C++ 577 | 578 | As with any node in ROS, we need to start it and give it a unique name: 579 | 580 | ```cpp 581 | ImageSubscriberNode() : Node("image_subscriber") 582 | ``` 583 | 584 | Now, again, we need to create a ROS subscriber. The basic structure is: `create_subscription("topic/name", queue_size, callback_function)`; the `msg::Type` is of the same type as the topic we want to get. Also, we tell ROS which function will be called every time a new image arrives. In our example, our `camera/image` topic is of type `sensor_msgs::msg::Image` and we call a `callback_image`, we declare it with a std::bind (don't worry about it, just use this structure every time you subscribe to a topic!). Then we have: 585 | 586 | ```cpp 587 | image_subscriber_ = this->create_subscription( 588 | "camera/image", 10, std::bind(&ImageSubscriberNode::callback_image, this, std::placeholders::_1)); 589 | ``` 590 | 591 | That was easy... but now we need to create the callback function. The basic structure consists of a function's name and, as a parameter, the variable with the data type that will be entered. In this case, our `msg` variable is of type `sensor_msgs::msg::Image::SharedPtr`: 592 | 593 | ```cpp 594 | void callback_image(const sensor_msgs::msg::Image::SharedPtr msg) 595 | ``` 596 | 597 | Inside this function, we can process our data. However, we prefer to use our subscribers callback functions to update our variables and process them in a custom function (e.g. our timer function); this is because, in compĺex programs, we might be required to process the information from several topics at a time. 598 | 599 | Therefore, first, let's focus on our callback function. We first need to transform our `sensor_msgs::msg::Image::SharedPtr` data in the `msg` variable to `cv::Mat`: 600 | 601 | ```cpp 602 | void callback_image(const sensor_msgs::msg::Image::SharedPtr msg) 603 | { 604 | image_ = cv_bridge::toCvCopy(msg, "bgr8")->image; 605 | is_image_ = true; 606 | } 607 | ``` 608 | 609 | And then, we can use it in our timer function `image_processing()`; in this case, we will only display it with OpenCV (don't forget the `cv::waitKey(1);` function to tell OpenCV to stop and show your images): 610 | 611 | ```cpp 612 | void image_processing() 613 | { 614 | if (is_image_){ 615 | cv::imshow("view", image_); 616 | cv::waitKey(1); 617 | } 618 | } 619 | ``` 620 | 621 | ### Python 622 | 623 | Similarly, in Python we first start our node and name it: 624 | 625 | ```python 626 | super().__init__("image_subscriber") 627 | ``` 628 | 629 | Then, we have to create a subscriber to tell ROS which function we will use when a new message comes as follows: 630 | 631 | ```python 632 | self.subscriber_ = self.create_subscription( 633 | Image, "camera/image", self.callback_camera_image, 10) 634 | ``` 635 | 636 | As in the C++ example, we can process our new data inside the callback function. However, we will use the callback functions to update our variables and process them later in a custom function (e.g. our timer function). So, in Python, we declare our callback function only with the name of our message but we let the data type as an implicit value: 637 | 638 | ```python 639 | def callback_camera_image(self, msg) 640 | ``` 641 | 642 | We then transform our ROS message to an OpenCV array: 643 | 644 | ```python 645 | #Transform the new message to an OpenCV matrix 646 | bridge_rgb=CvBridge() 647 | self.img = bridge_rgb.imgmsg_to_cv2(msg,msg.encoding).copy() 648 | ``` 649 | 650 | and we let know Python that we have received our first message: 651 | 652 | ```python 653 | self.is_img = True 654 | ``` 655 | 656 | Finally, we declare our timer using a basic structure `create_timer(time_in_seconds, callback_function)` as: 657 | 658 | ```python 659 | self.processing_timer_ = self.create_timer(0.030, self.image_processing) #update image each 30 miliseconds 660 | ``` 661 | 662 | We, then, can process our data in the callback function; in our example, we are going to display our image: 663 | 664 | ```python 665 | def image_processing(self): 666 | if self.is_img: 667 | #Show your images 668 | if(self.display): 669 | cv2.imshow("view", self.img) 670 | cv2.waitKey(1) 671 | ``` 672 | 673 | As you can see, our object-oriented programming (OOP) classes are similar in C++ and Python; however, we prefer C++ for performance but we also use Python for simplicity whenever high performance is not required. 674 | 675 | ## 1.3.2 Test your code 676 | 677 | Run the following command in a terminal to compile your code: 678 | 679 | ```bash 680 | cd ~/robovision_ros2_ws 681 | colcon build 682 | ``` 683 | 684 | Then, run the next command: 685 | 686 | ```bash 687 | source ~/robovision_ros2_ws/install/setup.bash 688 | ros2 run robovision_images my_publisher ~/robovision_ros2_ws/src/robovision_ros2/data/images/baboon.png 689 | ``` 690 | 691 | Now, in a different terminal, run the following commands: 692 | 693 | ```bash 694 | source ~/robovision_ros2_ws/install/setup.bash 695 | ros2 run robovision_images my_subscriber 696 | ``` 697 | 698 | What did happen? 699 | 700 | In a new terminal run this command: 701 | 702 | ```bash 703 | ros2 node list 704 | ``` 705 | 706 | ```bash 707 | ros2 node info /image_subscriber 708 | ``` 709 | 710 | Can you get what is happening? 711 | 712 | ```bash 713 | /image_subscriber 714 | Subscribers: 715 | /camera/image: sensor_msgs/msg/Image 716 | ``` 717 | 718 | The `ros2 node info` let us know that our *Image* topic `/camera/image` is being accessed by the node with name `/image_subscriber`. Remember that we named our image publisher as `create_publisher("camera/image", 10)` and that we subscribed to the `/camera/image` topic using the declaration `create_subscription("camera/image", 10, std::bind(&ImageSubscriberNode::callback_image, this, std::placeholders::_1))`. 719 | 720 | ## 1.3.3 Homework 1.3 721 | 722 | * Create a second subscriber to subscribe to our second topic, the scaled input image, in the `/camera/scaled_image` topic, and display it. 723 | 724 | **Hint** In C++, you need to create a second subscriber and a corresponding callback to the selected topic: 725 | 726 | ```cpp 727 | scaled_image_subscriber_ = create_subscription( 728 | "camera/scaled_image", 10, std::bind(&ImageSubscriberNode::callback_scaled_image, this, std::placeholders::_1)); 729 | ``` 730 | 731 | and declare your new callback function *callback_scaled_image*: 732 | 733 | ```cpp 734 | void callback_scaled_image(const sensor_msgs::msg::Image::SharedPtr msg) 735 | ``` 736 | 737 | Similarly, in Python, you need to subscribe to the new topic: 738 | 739 | ```python 740 | self.scaled_image_subscriber_ = self.create_subscription( 741 | Image, "camera/scaled_image", self.callback_scaled_image, 10) 742 | ``` 743 | 744 | and define your new callback function *callback_scaled_image*: 745 | 746 | ```python 747 | def callback_scaled_image(self, msg) 748 | ``` 749 | 750 | ## Authors 751 | 752 | * **Luis Contreras** - [ARTenshi](https://artenshi.github.io/) 753 | * **Hiroyuki Okada** - [AIBot](http://aibot.jp/) 754 | 755 | -------------------------------------------------------------------------------- /robovision_1/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | robovision_images 5 | 0.0.0 6 | TODO: Package description 7 | roboworks 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | ament_cmake_python 12 | 13 | rclcpp 14 | rclpy 15 | 16 | cv_bridge 17 | sensor_msgs 18 | std_msgs 19 | 20 | ament_lint_auto 21 | ament_lint_common 22 | 23 | 24 | ament_cmake 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /robovision_1/robovision_images/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ARTenshi/robovision_ros2/a825a9236d40c8d98e7793a570fb3cbf49d2238b/robovision_1/robovision_images/__init__.py -------------------------------------------------------------------------------- /robovision_1/scripts/my_subscriber.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import rclpy 3 | from rclpy.node import Node 4 | 5 | from sensor_msgs.msg import Image 6 | import cv2 7 | from cv_bridge import CvBridge 8 | import traceback 9 | 10 | 11 | class ImageSubscriberNode(Node): 12 | def __init__(self): 13 | super().__init__("image_subscriber") 14 | 15 | self.img = [] 16 | self.is_img = False 17 | self.display = True 18 | 19 | #Subscribers 20 | self.subscriber_ = self.create_subscription( 21 | Image, "camera/image", self.callback_camera_image, 10) 22 | 23 | #Processing 24 | self.processing_timer_ = self.create_timer(0.030, self.image_processing) #update image each 30 miliseconds 25 | 26 | self.get_logger().info("Starting image_subscriber application in python...") 27 | 28 | def callback_camera_image(self, msg): 29 | try: 30 | #Transform the new message to an OpenCV matrix 31 | bridge_rgb=CvBridge() 32 | self.img = bridge_rgb.imgmsg_to_cv2(msg,msg.encoding).copy() 33 | self.is_img = True 34 | 35 | except: 36 | self.get_logger().error(traceback.format_exc()) 37 | 38 | def image_processing(self): 39 | if self.is_img: 40 | #Show your images 41 | if(self.display): 42 | cv2.imshow("view", self.img) 43 | cv2.waitKey(1) 44 | 45 | 46 | def main(args=None): 47 | rclpy.init(args=args) 48 | node = ImageSubscriberNode() 49 | 50 | try: 51 | rclpy.spin(node) 52 | except KeyboardInterrupt: 53 | pass 54 | finally: 55 | node.destroy_node() 56 | rclpy.shutdown() 57 | 58 | 59 | if __name__ == "__main__": 60 | main() 61 | -------------------------------------------------------------------------------- /robovision_1/src/my_camera_publisher.cpp: -------------------------------------------------------------------------------- 1 | #include "rclcpp/rclcpp.hpp" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class CameraPublisherNode : public rclcpp::Node 8 | { 9 | public: 10 | CameraPublisherNode() : 11 | Node("image_publisher"), capture_(-1) // Default invalid camera index 12 | { 13 | // Declare and get the camera index parameter (default is 0) 14 | this->declare_parameter("camera_index", 0); 15 | int camera_index = this->get_parameter("camera_index").as_int(); 16 | 17 | // Open the camera 18 | capture_.open(camera_index); 19 | 20 | // Check if the camera opened successfully 21 | if (!capture_.isOpened()) 22 | { 23 | RCLCPP_ERROR(this->get_logger(), "Failed to open camera"); 24 | throw std::runtime_error("Camera not available"); 25 | } 26 | 27 | image_publisher_ = this->create_publisher("camera/image", 10); 28 | 29 | image_timer_ = this->create_wall_timer(std::chrono::milliseconds(30), 30 | std::bind(&CameraPublisherNode::publish_image, this)); 31 | 32 | RCLCPP_INFO(this->get_logger(), "Starting image_publisher application in cpp..."); 33 | } 34 | 35 | private: 36 | 37 | cv::VideoCapture capture_; 38 | 39 | rclcpp::Publisher::SharedPtr image_publisher_; 40 | rclcpp::TimerBase::SharedPtr image_timer_; 41 | 42 | void publish_image() 43 | { 44 | cv::Mat cv_image; 45 | capture_ >> cv_image; // Capture an image frame 46 | 47 | if (cv_image.empty()) 48 | { 49 | RCLCPP_WARN(this->get_logger(), "Empty frame captured"); 50 | return; 51 | } 52 | 53 | //Convert the output data into a ROS message format 54 | sensor_msgs::msg::Image::SharedPtr image_msg_; 55 | image_msg_ = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", cv_image).toImageMsg(); 56 | image_publisher_->publish(*image_msg_); 57 | } 58 | }; 59 | 60 | int main(int argc, char **argv) 61 | { 62 | rclcpp::init(argc, argv); 63 | 64 | auto node = std::make_shared(); 65 | rclcpp::spin(node); 66 | 67 | rclcpp::shutdown(); 68 | return 0; 69 | } 70 | -------------------------------------------------------------------------------- /robovision_1/src/my_publisher.cpp: -------------------------------------------------------------------------------- 1 | #include "rclcpp/rclcpp.hpp" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class ImagePublisherNode : public rclcpp::Node 8 | { 9 | public: 10 | ImagePublisherNode(const std::string & image_path) : Node("image_publisher") 11 | { 12 | image_publisher_ = this->create_publisher("camera/image", 10); 13 | 14 | // Load the image from file 15 | cv::Mat cv_image = cv::imread(image_path, cv::IMREAD_COLOR); 16 | if (cv_image.empty()) { 17 | RCLCPP_ERROR(this->get_logger(), "Failed to load image from path: %s", image_path.c_str()); 18 | return; 19 | } 20 | 21 | //Convert the output data into a ROS message format 22 | image_msg_ = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", cv_image).toImageMsg(); 23 | 24 | image_timer_ = this->create_wall_timer(std::chrono::milliseconds(30), 25 | std::bind(&ImagePublisherNode::publish_image, this)); 26 | 27 | RCLCPP_INFO(this->get_logger(), "Starting image_publisher application in cpp..."); 28 | } 29 | 30 | private: 31 | 32 | sensor_msgs::msg::Image::SharedPtr image_msg_; 33 | 34 | rclcpp::Publisher::SharedPtr image_publisher_; 35 | rclcpp::TimerBase::SharedPtr image_timer_; 36 | 37 | void publish_image() 38 | { 39 | image_publisher_->publish(*image_msg_); 40 | } 41 | }; 42 | 43 | int main(int argc, char **argv) 44 | { 45 | rclcpp::init(argc, argv); 46 | 47 | if (argc != 2) { 48 | RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Usage: my_publisher "); 49 | rclcpp::shutdown(); 50 | return 1; 51 | } 52 | 53 | std::string image_path = argv[1]; 54 | 55 | auto node = std::make_shared(image_path); 56 | rclcpp::spin(node); 57 | 58 | rclcpp::shutdown(); 59 | return 0; 60 | } 61 | -------------------------------------------------------------------------------- /robovision_1/src/my_subscriber.cpp: -------------------------------------------------------------------------------- 1 | #include "rclcpp/rclcpp.hpp" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | class ImageSubscriberNode : public rclcpp::Node 9 | { 10 | public: 11 | ImageSubscriberNode() : Node("image_subscriber"), is_image_(false) 12 | { 13 | //Subscribers 14 | image_subscriber_ = this->create_subscription( 15 | "camera/image", 10, std::bind(&ImageSubscriberNode::callback_image, this, std::placeholders::_1)); 16 | 17 | //Processing 18 | image_processing_timer_ = this->create_wall_timer( 19 | std::chrono::milliseconds(30), 20 | std::bind(&ImageSubscriberNode::image_processing, this)); 21 | 22 | RCLCPP_INFO(this->get_logger(), "Starting image_subscriber application in cpp..."); 23 | } 24 | 25 | private: 26 | 27 | bool is_image_; 28 | cv::Mat image_; 29 | sensor_msgs::msg::Image::SharedPtr image_msg_; 30 | 31 | rclcpp::Subscription::SharedPtr image_subscriber_; 32 | rclcpp::TimerBase::SharedPtr image_processing_timer_; 33 | 34 | void callback_image(const sensor_msgs::msg::Image::SharedPtr msg) 35 | { 36 | image_ = cv_bridge::toCvCopy(msg, "bgr8")->image; 37 | is_image_ = true; 38 | } 39 | 40 | void image_processing() 41 | { 42 | if (is_image_){ 43 | cv::imshow("view", image_); 44 | cv::waitKey(1); 45 | } 46 | } 47 | }; 48 | 49 | int main(int argc, char **argv) 50 | { 51 | rclcpp::init(argc, argv); 52 | auto node = std::make_shared(); 53 | rclcpp::spin(node); 54 | rclcpp::shutdown(); 55 | return 0; 56 | } 57 | -------------------------------------------------------------------------------- /robovision_2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(robovision_processing) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(ament_cmake_python REQUIRED) 11 | find_package(rclcpp REQUIRED) 12 | 13 | find_package(OpenCV REQUIRED) 14 | find_package(cv_bridge REQUIRED) 15 | find_package(sensor_msgs REQUIRED) 16 | find_package(std_msgs REQUIRED) 17 | 18 | include_directories(${cv_bridge_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) 19 | 20 | # C++ section 21 | add_executable(my_processing src/my_processing.cpp) 22 | ament_target_dependencies(my_processing 23 | rclcpp 24 | cv_bridge 25 | sensor_msgs 26 | OpenCV 27 | ) 28 | 29 | # Python section 30 | ament_python_install_package(${PROJECT_NAME}) 31 | 32 | # Ensure both are included 33 | install(TARGETS 34 | my_processing 35 | DESTINATION lib/${PROJECT_NAME} 36 | ) 37 | install(PROGRAMS 38 | scripts/my_processing.py 39 | DESTINATION lib/${PROJECT_NAME} 40 | ) 41 | 42 | 43 | ament_package() 44 | -------------------------------------------------------------------------------- /robovision_2/README.md: -------------------------------------------------------------------------------- 1 | # RGB Image Processing with OpenCV and ROS 2 | 3 | The goal of this repository is to introduce students to RGB image processing using OpenCV and ROS. 4 | 5 | 6 | # 1. Getting to know your image 7 | 8 | Before starting to manipulate images, we should understand what they are and how we can access their data. In this work, we understand the images as a 2D array, or matrix, where each element (also known as **pixel**) in the array has a color value. We use three color channels per element in the array: Red, Green, and Blue. The origin of this image matrix is at the top-left corner and column values increase positively from left to right while row values increase positively from top to bottom, as can be seen in the image below: 9 | 10 |

11 | 12 |

13 | 14 | Each color channel has an integer value between 0 and 255. For example, a value of RGB = [255, 0, 0] represents the red color because the value red = 255 is the maximum possible while the green = 0 and blue = 0 values represent the absence of those color. Similarly, RGB = [0, 255, 0] represents the green color and RGB = [0, 0, 255] represents the blue color. The combination of these three channels with different intensities gives us our perception of true color (for example, if you combine different values of red and green you will obtain a range of tonalities of yellow, e.g. RGB = [128, 128, 0]). 15 | 16 | 17 | # 2. Basic operations 18 | 19 | From our previous tutorial, we learnt how to subscribe to a ROS Image topic in C++: 20 | 21 | ```cpp 22 | image_subscriber_ = this->create_subscription( 23 | "camera/image", 10, std::bind(&ImageProcessingNode::callback_image, this, std::placeholders::_1)); 24 | ``` 25 | 26 | and to transform our ROS Image message to an OpenCV matrix array: 27 | 28 | ```cpp 29 | image_ = cv_bridge::toCvCopy(msg, "bgr8")->image; 30 | ``` 31 | 32 | In Python, to create an Image subscribe: 33 | 34 | ```python 35 | self.subscriber_ = self.create_subscription( 36 | Image, "camera/image", self.callback_camera_image, 10) 37 | ``` 38 | 39 | and to convert it to a matrix array: 40 | 41 | ```python 42 | bridge_rgb=CvBridge() 43 | self.img = bridge_rgb.imgmsg_to_cv2(msg,msg.encoding).copy() 44 | ``` 45 | 46 | ## 2.1 Image dimensions 47 | 48 | ### C++ 49 | 50 | Have a look at the `my_processing.cpp` file. Let's inspect our image. First, let's see the dimensions of our image (we add a `counter_` index so we print get these values only in the first iteration): 51 | 52 | ```cpp 53 | if (counter_ == 0) 54 | std::cout << "size: rows: " << image_.rows << 55 | ", cols: " << image_.cols << 56 | ", depth: " << image_.channels() << std::endl; 57 | ``` 58 | 59 | Let's test our code. First, we need to compile our code: 60 | 61 | ```bash 62 | cd ~/robovision_ros2_ws 63 | colcon build 64 | ``` 65 | 66 | Now, as in our previous examples, run: 67 | 68 | ```bash 69 | source ~/robovision_ros2_ws/install/setup.bash 70 | ros2 run robovision_images my_publisher ~/robovision_ros2_ws/src/robovision_ros2/data/images/baboon.png 71 | ``` 72 | 73 | Finally, in a new terminal, run this command: 74 | 75 | ```bash 76 | source ~/robovision_ros2_ws/install/setup.bash 77 | ros2 run robovision_processing my_processing 78 | ``` 79 | 80 | where we should be able to see information regarding our image's size. 81 | 82 | ### Python 83 | 84 | Have a look at the `my_processing.py` file. Similarly, we first determine the dimensions of our image. In Python, we use the `shape` operator in our matrices, it returns three values (the number of rows, columns and channels) for color images and two values (rows and columns) for grayscale images. So you can use the length of this vector to determine if your image is a multi- or single-channel array. 85 | 86 | ```python 87 | if (self.counter == 0): 88 | (rows,cols,channels) = self.img.shape #Check the size of your image 89 | print ('size: rows: {}, cols: {}, channels: {}'.format(rows, cols, channels)) 90 | ``` 91 | 92 | Now, let's try our code. In a pure Python project, we can build our code using the `--symlink-install` flag once and test modifications of our file without the need to build it again; however, in this project, we mix C++ and Python code, so we need to build it every time we make any modification. So, run the following command: 93 | 94 | ```bash 95 | cd ~/robovision_ros2_ws 96 | colcon build 97 | ``` 98 | 99 | Teen, we run: 100 | 101 | ```bash 102 | source ~/robovision_ros2_ws/install/setup.bash 103 | ros2 run robovision_images my_publisher ~/robovision_ros2_ws/src/robovision_ros2/data/images/baboon.png 104 | ``` 105 | 106 | Finally, in a new terminal: 107 | 108 | ```bash 109 | source ~/robovision_ros2_ws/install/setup.bash 110 | ros2 run robovision_processing my_processing.py 111 | ``` 112 | 113 | where we should be able to see information regarding to our image. 114 | 115 | ### Homework 2.1 116 | 117 | * What is the size (columns, rows, and channels) of our image? 118 | 119 | ## 2.2 Image manipulation 120 | 121 | To understand how to manipulate our images, let's first make some easy edits. In this case, we will add an increasing id number to our image. Remember that the top-left corner is our image origin (0,0), and that the columns increase from left to right and the rows from top to bottom. 122 | 123 | ### C++ 124 | 125 | To add a text into our image `img`, we use the OpenCV function `cv::putText`. We have an integer `counter` variable that we increase each time we receive and process a new image, and we transform it into a text sequence using the `std::to_string(counter)` function. 126 | 127 | To understand how an image is coded in OpenCV, play around with the `cv::Point` and `CV_RGB` values. The cv::Point(col_id, row_id) marks the origin of our text (i.e. the bottom-left corner of our text box); use different values of col_id and row_id and see what happens. 128 | 129 | On the other hand, the CV_RGB(red, green, blue) parameter determines our text's color. Remember that a single color channel ranges from 0 to 255, so try different red, green, and blue combinations and see all the colors you can create! 130 | 131 | ```cpp 132 | cv::putText(image_, 133 | std::to_string(counter_), 134 | cv::Point(25, 25), //change these values cv::Point(col_id, row_id) 135 | cv::FONT_HERSHEY_DUPLEX, 136 | 1.0, 137 | CV_RGB(255, 0, 0), //change these values CV_RGB(red, green, blue) 138 | 2); 139 | ``` 140 | 141 | Compile and test your code as in the previous section. 142 | 143 | ### Python 144 | 145 | Likewise, we use the `cv2.putText` function to add a text string into our `img` array. The two dimensional vector (col_id, row_id) marks the origin (bottom-left) of our text box. The three dimensional vector indicates our text's color. However, in Python **the order of our color channels is (blue, green, red)**. Be aware of this difference when you work with C++ and Python at the same time. Now, give different values to the color vector and see how to behave. 146 | 147 | ```python 148 | cv2.putText(self.img, 149 | str(self.counter), 150 | (25,25), 151 | cv2.FONT_HERSHEY_SIMPLEX, 152 | 1, 153 | (255, 0, 0), 154 | 2, 155 | cv2.LINE_AA) 156 | ``` 157 | 158 | Again, try your code as in the previous section. 159 | 160 | You should see something like 161 | 162 |

163 | 164 |

165 | 166 | ### Homework 2.2 167 | 168 | * Provide the output image for five different text positions and color combinations. 169 | 170 | ## 2.3 Image transformations 171 | 172 | ## 2.3.1 BGR to Grayscale 173 | 174 | Now that we have an idea of the composition of an image and an understanding of our code, let's add some image transformations. We will show you how to apply one of the built-in functions in OpenCV, so you can explore all the available functions depending on your task at hand. 175 | 176 | Let's change our image from color to grayscale. We can understand a grayscale as a color image where the `Gray` value is a combination of the different color channels as follows: 177 | 178 | ![equation](https://latex.codecogs.com/svg.latex?Gray=((0.3*R)+(0.59*G)+(0.11*B))) 179 | 180 | and it has the same value in its three channels, i.e. RGB = [Gray, Gray, Gray]. Therefore, to reduce the memory used, many systems only use a one-channel matrix with a single Gray value per pixel: RGB = [Gray]. 181 | 182 | ### C++ 183 | 184 | Please complete the provided code with the following instructions as appropriate. We first create a new matrix `image_gray` and apply the OpenCV `cvtColor` function to our original `image_` image: 185 | 186 | ```cpp 187 | cv::Mat image_gray; 188 | cv::cvtColor(image_, image_gray, CV_BGR2GRAY); 189 | ``` 190 | 191 | Let's inspect our image: 192 | 193 | ```cpp 194 | if (counter_ == 0) 195 | std::cout << "image_gray size: rows: " << image_gray.rows << 196 | ", cols: " << image_gray.cols << 197 | ", depth: " << image_gray.channels() << std::endl; 198 | ``` 199 | 200 | and display it in a new window: 201 | 202 | ```cpp 203 | cv::imshow("gray", image_gray); 204 | ``` 205 | Compile and test your code as in the previous sections. 206 | 207 | ### Python 208 | 209 | Similarly, we apply our `cvtColor` function to our original `img` and store it in a new `img_gray` array. 210 | 211 | ```python 212 | img_gray = cv2.cvtColor(self.img, cv2.COLOR_BGR2GRAY) 213 | ``` 214 | 215 | Then, we inspect our image. Remember that our `shape` operator returns three values for color images and two values for grayscale images and that you can use the length of this vector to determine if your image is a multi- or single-channel array. 216 | 217 | ```python 218 | if (self.counter == 0): 219 | (rows,cols) = img_gray.shape 220 | print ('img_gray size: rows: {}, cols: {}'.format(rows, cols)) 221 | print ('length (img_gray): {}'.format(len(img_gray.shape))) 222 | ``` 223 | 224 | and show it: 225 | 226 | ```python 227 | cv2.imshow("gray", img_gray) 228 | ``` 229 | 230 | Now, save your files and test them as before. What can you see? Did you notice how a grayscale image uses only a single channel to represent all the information instead of three channels with the same value? 231 | 232 | Again, try your code as in the previous section. 233 | 234 | The expected output is 235 | 236 |

237 | 238 |

239 | 240 | ### Homework 2.3.1 241 | 242 | * What is the size (columns, rows, and channels) of our grayscale image? Show the resulting image. 243 | 244 | * What is the scope of your grayscale image variables? Do you need to make them global? Why? 245 | 246 | ## 2.3.2 Color Thresholding 247 | 248 | Now we will create a very basic color segmentator. For simplicity, we will use the BGR color space, but it is more common to use the HSV color space; we let the interested reader to find what are the extra steps to use an HSV image for color segmentation. 249 | 250 | An easy way to create a color-based segmentator is to find all the pixels that are around our target color. For example, if we want to segment a high-intensity red color with a red channel value around `my_red=[225]`, we need to find all the pixels in the range (my_red-delta , my_red+delta) where `delta` is a value that we define; it is important to note that, in this example, the other channels range from 0 to 255. 251 | 252 | Therefore, we have two steps to create a segmented image. First, we find the pixels inside our desired range and mark them as ONE and all the pixels outside the range as ZERO, we call this array with ZEROs and ONEs a **mask**. Then, we create an output image where all the elements in the MASK have a color value from the input image and all other pixels are zero (or black). 253 | 254 | ### C++ 255 | 256 | First, we create our mask image (don't forget to declare it!) and fill it with zeros and ones, depending on whether the pixel is inside or outside our color range (**remember the order in our array: [Blue,Green,Red]**), respectively: 257 | 258 | ```cpp 259 | cv::Mat mask; 260 | cv::inRange(image_, cv::Scalar(0,0,220), cv::Scalar(240,255,240), mask); 261 | ``` 262 | In the example, our target color is `red=255`, and `delta=20`, the blue and green channels vary from 0 to 255. Then, we copy only those pixels that met our range condition. We use the `image_.copyTo(output,mask)` to copy to our *output* image only those pixels with a ONE in our *mask*; we declare our output as `image_filtered_` and then: 263 | 264 | ```cpp 265 | cv::Mat image_filtered; 266 | image_.copyTo(image_filtered, mask); 267 | ``` 268 | 269 | Finally, don't forget to show your images 270 | 271 | ```cpp 272 | cv::imshow("mask", mask); 273 | cv::imshow("image_filtered", image_filtered); 274 | ``` 275 | 276 | Compile and test your code as in the previous sections. 277 | 278 | A segmented image in the red channel should look like 279 | 280 |

281 | 282 |

283 | 284 | ### Python 285 | 286 | Here, we define the valid color range we want to use (**remember the order in our array: [Blue,Green,Red]**). Now, we will select a blue color, so `blue=220` and `delta=10`, the red and green channels vary from 0 to 255, as follow: 287 | 288 | ```python 289 | lower_val = np.array([200,0,0]) 290 | upper_val = np.array([240,255,255]) 291 | ``` 292 | 293 | Then, we create our mask with ONE value whenever a pixel is in that color range, we use the `cv2.inRange` function: 294 | 295 | ```python 296 | mask = cv2.inRange(self.img, lower_val, upper_val) 297 | ``` 298 | 299 | Finally, we copy those pixels from our original to our segmented image as follows: 300 | 301 | ```python 302 | image_filtered = cv2.bitwise_and(self.img,self.img, mask= mask) 303 | ``` 304 | 305 | Now, we show our results: 306 | 307 | ```python 308 | cv2.imshow("mask", mask) 309 | cv2.imshow("image_filtered", image_filtered) 310 | ``` 311 | 312 | Again, try your code as in the previous section. 313 | 314 | 315 | A segmented image in the blue channel should look like 316 | 317 |

318 | 319 |

320 | 321 | ### Homework 2.3.2 322 | 323 | * Create five segmented images using different color ranges. From the previous unit, do you remember how to start an image publisher using your camera? Try using it and place objects with different colors in front of your camera and see what happens! 324 | 325 | 326 | # 3. Per-element operations 327 | 328 | Although OpenCV comes with a variety of functions, we will always need to access the elements of our matrix for a number of reasons (from simple inspection to the implementation of our own algorithms). Here, we will provide a simple way to access all the elements in your array. 329 | 330 | ## 3.1 Single element access 331 | 332 | Let's first inspect one pixel value of our array at a given (row_id, col_id) position. **Remember that our array starts in `(0,0)` and therefore the last element in our image `image_` (at the bottom-right corner) is `(image_.rows - 1, image_.cols - 1)`**. 333 | 334 | ### C++ 335 | 336 | We use the `image_.at(row_id,col_id)` attribute in our `image_` matrix to get a color element in the [Blue,Green,Red] order, and `image_.at(row_id,col_id)` to get a grayscale value -- if you don't know what is a `uchar` data type, please review that concept; in short, it is an integer that goes from 0 to 255. **Notice the id input order in our function, the first index ALWAY corresponds to the rows and the second index to the columns**; be careful with the elements' order when you use OpenCV functions, as an example, remember that in our `cv::putText` function the `cv::Point` element has a (col_id, row_id) order while the `at<>` attribute of our image has a (row_id,col_id) order. 337 | 338 | In this case, we will inspect the middle point in our array as follows: 339 | 340 | ```cpp 341 | if (counter == 0) 342 | { 343 | int row_id, col_id; 344 | row_id = image_.rows/2; 345 | col_id = image_.cols/2; 346 | 347 | std::cout << "pixel value in img at row=" << row_id << 348 | ", col=" << col_id << 349 | " is: " << image_.at(row_id,col_id) << std::endl; 350 | std::cout << "pixel value in img_gray at row=" << row_id << 351 | ", col=" << col_id << 352 | " is: " << (int)image_gray.at(row_id,col_id) << std::endl; 353 | } 354 | ``` 355 | 356 | Compile and test your code as in the previous sections. 357 | 358 | **Important: You should notice that, for color images, the output of the `img.at(row_id,col_id)` operator is a 3D vector with the [Blue,Green,Red] information, in that order.** 359 | 360 | ### Python 361 | 362 | Here, we access the elements in our `img` array as in any other Python array: `img[row_id,col_id]` to get a color pixel in [Blue,Green,Red] order or a grayvalue scalar. **We ALWAYS indicate the rows first and then the columns**; again, be careful with the input order when you use different OpenCV functions in Python. Then, we get the values of our middle pixel element as: 363 | 364 | ```python 365 | if (counter == 0): 366 | (rows,cols,channels) = self.img.shape 367 | row_id = int(rows/2) 368 | col_id = int(cols/2) 369 | 370 | print ('pixel value in img at row: {} and col: {} is {}'.format(row_id, col_id, self.img[row_id,col_id])) 371 | print ('pixel value in img_gray at row: {} and col: {} is {}'.format(row_id, col_id, img_gray[row_id,col_id])) 372 | ``` 373 | 374 | Again, try your code as in the previous section. 375 | 376 | **IMPORTANT: You should notice that, for color images, the output of the `img[row_id,col_id]` operator is a 3D vector with the [Blue,Green,Red] information, in that order.** 377 | 378 | ### Homework 3.1 379 | 380 | * Provide the values of ten different pixels in your image. 381 | 382 | Do you remember the equation to obtain the Gray value from a combination of the different color channels? Does it apply to your image? 383 | 384 | 385 | ## 3.2 Multi-element access 386 | 387 | Now that we know how to access an element in our image and the kind of information in it (color or grayscale), let's access all our pixels consistently. Remember that the Gray value is a combination of the three different color channels in an image. A common function that approximates the gray value is as follows: 388 | 389 | ![equation](https://latex.codecogs.com/svg.latex?Gray=((0.3*R)+(0.59*G)+(0.11*B))) 390 | 391 | so let's apply that equation to all and every pixel in our image. To do so, we need to run a *nested for loop* where one index goes from the first to the last column and the other index goes from the first row to the last. Remember that, in general, **the array indices start in zero and, therefore, they should end at (width-1) and (height-1)**. 392 | 393 | ### C++ 394 | 395 | First, we need to create a single channel matrix to store our new image, so it should have the same dimensions as our input and each value should be `uchar`: 396 | 397 | ```cpp 398 | cv::Mat image_gray_2 = cv::Mat::zeros(image_.rows,image_.cols, CV_8UC1); 399 | ``` 400 | 401 | Then, we create our indices to access all the elements in our image. Please, remember which index corresponds to the rows and what to the columns! In our case, the `i` variable corresponds to the rows and the `j` index to the columns: 402 | 403 | ```cpp 404 | for(int i=0; i(row_id,col_id)` operator; remember that this operator's output is a [Blue,Green,Red] vector so, we have: 408 | 409 | ```cpp 410 | int gray_val = 0.11*image_.at(i,j)[0] + 0.59*image_.at(i,j)[1] + 0.3*image_.at(i,j)[2]; 411 | ``` 412 | 413 | We store the `int` grayscale value in our `uchar` new `uchar` matrix at the (i,j) position: 414 | 415 | ```cpp 416 | image_gray_2.at(i,j) = (unsigned char)gray_val; 417 | ``` 418 | 419 | Your code should look something like: 420 | 421 | ```cpp 422 | cv::Mat image_gray_2 = cv::Mat::zeros(image_.rows,image_.cols, CV_8UC1); 423 | for(int i=0; i(i,j)[0] + 0.59*image_.at(i,j)[1] + 0.3*image_.at(i,j)[2]; 427 | image_gray_2.at(i,j) = (unsigned char)gray_val; 428 | } 429 | ``` 430 | 431 | Finally, don't forget to display your image (each new window should have its unique name): 432 | 433 | ```cpp 434 | cv::imshow("gray_2", image_gray_2); 435 | ``` 436 | 437 | Compile and test your code as in the previous sections. 438 | 439 | ### Python 440 | 441 | Similarly, we start by defining our new `uint8` matrix (the `uint8` data type in Python is similar to the `uchar` data type in C++): 442 | 443 | ```python 444 | (rows,cols,channels) = img.shape 445 | img_gray_2 = np.zeros( (rows,cols,1), np.uint8 ) 446 | ``` 447 | 448 | Then, we create our indices (please refer to the manual to know how the `range(int)` function works; basically, it creates a sequence of numbers from 0 to int-1), remember which index corresponds to the rows and the columns: 449 | 450 | ```python 451 | for i in range(rows): 452 | for j in range(cols): 453 | ``` 454 | 455 | Then, access to the information for pixel (i,j), combine it to create a grayscale value, and store it in our new grayscale image: 456 | 457 | ```python 458 | for i in range(rows): 459 | for j in range(cols): 460 | ``` 461 | 462 | Your code should look like: 463 | 464 | ```python 465 | (rows,cols,channels) = self.img.shape 466 | img_gray_2 = np.zeros( (rows,cols,1), np.uint8 ) 467 | 468 | for i in range(rows): 469 | for j in range(cols): 470 | p = self.img[i,j] 471 | img_gray_2[i,j] = int( int(0.11*p[0]) + 0.59*int(p[1]) + int(0.3*p[2]) ) 472 | ``` 473 | 474 | Finally, don't forget to show your new image: 475 | 476 | ```python 477 | cv2.imshow("gray_2", img_gray_2) 478 | ``` 479 | 480 | Again, try your code as in the previous section. 481 | 482 | **IMPORTANT: Can you notice the difference in velocity!? Although C++ seems more cumbersome than Python, the execution speed is much faster. A common practice is to use C++ for the core operations in our applications and Python for the high-level processes.** 483 | 484 | ### Homework 3.2 485 | 486 | * Create a loop to access all the elements in your grayscale image and rotate it 180 degrees. The result should be as in the image below. 487 | 488 |

489 | 490 |

491 | 492 | **Hint** You should create a new matrix before starting your loop. Also, observe the indices in your original and your new image. what is their relationship? 493 | 494 | ## Authors 495 | 496 | * **Luis Contreras** - [ARTenshi](https://artenshi.github.io/) 497 | * **Hiroyuki Okada** - [AIBot](http://aibot.jp/) 498 | 499 | -------------------------------------------------------------------------------- /robovision_2/images/baboon_gray.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ARTenshi/robovision_ros2/a825a9236d40c8d98e7793a570fb3cbf49d2238b/robovision_2/images/baboon_gray.jpg -------------------------------------------------------------------------------- /robovision_2/images/baboon_mask_blue.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ARTenshi/robovision_ros2/a825a9236d40c8d98e7793a570fb3cbf49d2238b/robovision_2/images/baboon_mask_blue.jpg -------------------------------------------------------------------------------- /robovision_2/images/baboon_mask_red.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ARTenshi/robovision_ros2/a825a9236d40c8d98e7793a570fb3cbf49d2238b/robovision_2/images/baboon_mask_red.jpg -------------------------------------------------------------------------------- /robovision_2/images/baboon_number.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ARTenshi/robovision_ros2/a825a9236d40c8d98e7793a570fb3cbf49d2238b/robovision_2/images/baboon_number.jpg -------------------------------------------------------------------------------- /robovision_2/images/baboon_rotated.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ARTenshi/robovision_ros2/a825a9236d40c8d98e7793a570fb3cbf49d2238b/robovision_2/images/baboon_rotated.jpg -------------------------------------------------------------------------------- /robovision_2/images/digital_image.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ARTenshi/robovision_ros2/a825a9236d40c8d98e7793a570fb3cbf49d2238b/robovision_2/images/digital_image.jpg -------------------------------------------------------------------------------- /robovision_2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | robovision_processing 5 | 0.0.0 6 | TODO: Package description 7 | roboworks 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | ament_cmake_python 12 | 13 | rclcpp 14 | rclpy 15 | 16 | cv_bridge 17 | sensor_msgs 18 | std_msgs 19 | 20 | ament_lint_auto 21 | ament_lint_common 22 | 23 | 24 | ament_cmake 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /robovision_2/robovision_processing/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ARTenshi/robovision_ros2/a825a9236d40c8d98e7793a570fb3cbf49d2238b/robovision_2/robovision_processing/__init__.py -------------------------------------------------------------------------------- /robovision_2/scripts/my_processing.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import rclpy 3 | from rclpy.node import Node 4 | 5 | from sensor_msgs.msg import Image 6 | import cv2 7 | from cv_bridge import CvBridge 8 | import traceback 9 | 10 | 11 | class ImageProcessingNode(Node): 12 | def __init__(self): 13 | super().__init__("image_processing") 14 | 15 | self.img = [] 16 | self.is_img = False 17 | self.display = True 18 | 19 | self.counter = 0 20 | 21 | #Subscribers 22 | self.subscriber_ = self.create_subscription( 23 | Image, "camera/image", self.callback_camera_image, 10) 24 | 25 | #Processing 26 | self.processing_timer_ = self.create_timer(0.030, self.image_processing) #update image each 30 miliseconds 27 | 28 | self.get_logger().info("Starting image_processing application in python...") 29 | 30 | def callback_camera_image(self, msg): 31 | try: 32 | #Transform the new message to an OpenCV matrix 33 | bridge_rgb=CvBridge() 34 | self.img = bridge_rgb.imgmsg_to_cv2(msg,msg.encoding).copy() 35 | self.is_img = True 36 | 37 | except: 38 | self.get_logger().error(traceback.format_exc()) 39 | 40 | def image_processing(self): 41 | if self.is_img: 42 | 43 | if (self.counter == 0): 44 | (rows,cols,channels) = self.img.shape #Check the size of your image 45 | print ('size: rows: {}, cols: {}, channels: {}'.format(rows, cols, channels)) 46 | 47 | self.counter += 1 48 | #Show your images 49 | if(self.display): 50 | cv2.imshow("view", self.img) 51 | cv2.waitKey(1) 52 | 53 | 54 | def main(args=None): 55 | rclpy.init(args=args) 56 | node = ImageProcessingNode() 57 | 58 | try: 59 | rclpy.spin(node) 60 | except KeyboardInterrupt: 61 | pass 62 | finally: 63 | node.destroy_node() 64 | rclpy.shutdown() 65 | 66 | 67 | if __name__ == "__main__": 68 | main() 69 | -------------------------------------------------------------------------------- /robovision_2/src/my_processing.cpp: -------------------------------------------------------------------------------- 1 | #include "rclcpp/rclcpp.hpp" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | class ImageProcessingNode : public rclcpp::Node 9 | { 10 | public: 11 | ImageProcessingNode() : Node("image_processing"), is_image_(false) 12 | { 13 | counter_ = 0; 14 | 15 | //Subscribers 16 | image_subscriber_ = this->create_subscription( 17 | "camera/image", 10, std::bind(&ImageProcessingNode::callback_image, this, std::placeholders::_1)); 18 | 19 | //Processing 20 | image_processing_timer_ = this->create_wall_timer( 21 | std::chrono::milliseconds(30), 22 | std::bind(&ImageProcessingNode::image_processing, this)); 23 | 24 | RCLCPP_INFO(this->get_logger(), "Starting image_processing application in cpp..."); 25 | } 26 | 27 | private: 28 | 29 | bool is_image_; 30 | int counter_; 31 | cv::Mat image_; 32 | sensor_msgs::msg::Image::SharedPtr image_msg_; 33 | 34 | rclcpp::Subscription::SharedPtr image_subscriber_; 35 | rclcpp::TimerBase::SharedPtr image_processing_timer_; 36 | 37 | void callback_image(const sensor_msgs::msg::Image::SharedPtr msg) 38 | { 39 | image_ = cv_bridge::toCvCopy(msg, "bgr8")->image; 40 | is_image_ = true; 41 | } 42 | 43 | void image_processing() 44 | { 45 | if (is_image_){ 46 | if (counter_ == 0) 47 | std::cout << "size: rows: " << image_.rows << 48 | ", cols: " << image_.cols << 49 | ", depth: " << image_.channels() << std::endl; 50 | 51 | cv::imshow("view", image_); 52 | cv::waitKey(1); 53 | 54 | counter_++; 55 | } 56 | } 57 | }; 58 | 59 | int main(int argc, char **argv) 60 | { 61 | rclcpp::init(argc, argv); 62 | auto node = std::make_shared(); 63 | rclcpp::spin(node); 64 | rclcpp::shutdown(); 65 | return 0; 66 | } 67 | -------------------------------------------------------------------------------- /robovision_3/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(robovision_rgbd) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(ament_cmake_python REQUIRED) 11 | find_package(rclcpp REQUIRED) 12 | 13 | find_package(OpenCV REQUIRED) 14 | find_package(cv_bridge REQUIRED) 15 | 16 | find_package(std_msgs REQUIRED) 17 | find_package(sensor_msgs REQUIRED) 18 | find_package(geometry_msgs REQUIRED) 19 | 20 | find_package(tf2 REQUIRED) 21 | find_package(tf2_geometry_msgs REQUIRED) 22 | 23 | include_directories(${cv_bridge_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) 24 | 25 | # C++ section 26 | add_executable(rgbd_reader src/rgbd_reader.cpp) 27 | ament_target_dependencies(rgbd_reader 28 | rclcpp 29 | cv_bridge 30 | sensor_msgs 31 | geometry_msgs 32 | OpenCV 33 | tf2 34 | tf2_geometry_msgs 35 | ) 36 | 37 | # Python section 38 | ament_python_install_package(${PROJECT_NAME}) 39 | 40 | # Ensure both are included 41 | install(TARGETS 42 | rgbd_reader 43 | DESTINATION lib/${PROJECT_NAME} 44 | ) 45 | install(PROGRAMS 46 | scripts/rgbd_reader.py 47 | DESTINATION lib/${PROJECT_NAME} 48 | ) 49 | 50 | 51 | ament_package() 52 | -------------------------------------------------------------------------------- /robovision_3/README.md: -------------------------------------------------------------------------------- 1 | # Point Cloud processing with ROS 2 | 3 | We present an introduction to Point Cloud data in ROS and propose a simple task where the students should track a person moving in front of an RGBD camera mounted on a mobile robot. 4 | 5 | # 0. Get the additional data 6 | 7 | ## 0.1 Rosbag data 8 | 9 | Be sure that you have downloaded our additional data available [here (Google Drive)](https://bit.ly/3WjWnI2). 10 | 11 | Those folders should be in the `~/robovision_ros2_ws/src/robovision_ros2/data/rosbags/` folder. 12 | 13 | To be sure, let's test it. In one terminal run: 14 | 15 | ```bash 16 | ros2 bag play ~/robovision_ros2_ws/src/robovision_ros2/data/rosbags/person_static --loop 17 | ``` 18 | 19 | In a second terminal, run this command: 20 | 21 | ```bash 22 | ros2 topic list 23 | ``` 24 | 25 | You should be able to see several topics! 26 | 27 | # 1. Getting to know your RGBD image 28 | 29 | In this lesson, we will apply all that we have learnt in the past two units. First, let's inspect our code in the `rgbd_reader.py` file. 30 | 31 | In the main function we can see that, as we have done before, we first initialise our node 32 | 33 | ```python 34 | super().__init__("point_cloud_centroid") 35 | ``` 36 | 37 | and then, we subscribe to the topics we are going to use: 38 | 39 | ```python 40 | self.rgb_subscriber_ = self.create_subscription( 41 | Image, "/camera/rgb/image_rect_color", self.callback_rgb_rect, 10) 42 | self.depth_subscriber_ = self.create_subscription( 43 | Image, "/camera/depth_registered/image", self.callback_depth_rect, 10) 44 | self.point_cloud_subscriber_ = self.create_subscription( 45 | PointCloud2, "/camera/depth_registered/points", self.callback_point_cloud, 10) 46 | ``` 47 | 48 | Here, you can notice the relevance of having our subscribers' callback functions to update our variables at different rates and a different timer function to process that information. In this case, we subscribe to three topics: an **RGB** and **Depth** images and a **Point Cloud** of 3D points, all of them come from the same RGBD sensor 49 | 50 |

51 | 52 |

53 | 54 | The RGB image is a color image with three channels (Red, Green, and Blue), and the Depth image corresponds to the metric distance of each pixel of the objects in the image to an orthogonal plane that passes through the center of the camera; you can visualise it as the horizontal distance of a given point to the camera seen from above, regardless of the height, as in the Figure 55 | 56 |

57 | 58 |

59 | 60 | We have used an `Image` topic for RGB images before. The Depth image is a matrix of floats corresponding to the metric distance in milimeters. Therefore, in the callback function `callback_depth_rect` we read it as 61 | 62 | ```python 63 | self.depth_=bridge_depth.imgmsg_to_cv2(msg,"32FC1").copy() 64 | self.depth_mat_ = np.array(self.depth_, dtype=np.float32) 65 | ``` 66 | As the values range from 400 (40 centimeters) to 10000 (10 meters), we normalize it to valid image values and save it in an image array to be able to display it 67 | 68 | ```python 69 | v2.normalize(self.depth_mat_, self.depth_, 0, 1, cv2.NORM_MINMAX) 70 | ``` 71 | 72 | **Important:** the Depth information comes from a structured infrared light sensor and therefore very reflective or transparent surfaces tend to distort the depth information; those points appear as black (zero values) pixels in our depth image. The minimum distance our RGBD sensor is able to read is 40 cm, anything closer to that will be a zero value, as well. 73 | 74 | Furthermore, from the RGB and Depth images, for every pixel in the image, we can obtain the metric XYZ position in the space -- we will not go further on this because, luckily, we see that ROS has already calculated it and the `/camera/depth_registered/points` of type `PointCloud2` provides this information. If you type 75 | 76 | ```bash 77 | ros2 interface show sensor_msgs/msg/PointCloud2 78 | ``` 79 | 80 | in a terminal, you can see the composition of this type of message. 81 | 82 | The point cloud message is a list of tuples (x, y, z, ...) in milimeters of the type `PointCloud2`. Therefore, in the callback function `callback_point_cloud` we read it as 83 | 84 | ```python 85 | self.point_cloud_ = np.array(list(pc2.read_points(msg, field_names=None, skip_nans=False))) 86 | ``` 87 | 88 | This reads the point cloud as a list, so we reshape it as a matrix form aligned to our RGB image 89 | 90 | ```python 91 | if msg.height > 1: 92 | self.point_cloud_ = self.point_cloud_.reshape((msg.height, msg.width, -1)) 93 | 94 | rows, cols, _ = self.point_cloud_.shape 95 | print ('new message has arrived; point cloud size: rows: {}, cols: {}'.format(rows, cols)) 96 | ``` 97 | 98 | Back to our code, we see that we have a ROS publisher where we want to publish the 3D position of an object in front of our camera; remember that a topic should have a unique name -- in this case, we called it `/object_centroid` and is of the type `Pose`: 99 | 100 | ```python 101 | self.centroid_publisher_ = self.create_publisher( 102 | Pose, "/object_centroid", 10) 103 | ``` 104 | 105 | A Pose message is a geometry_msgs type that consists of a 3D position in meters and a 4D orientation in quaternion form of every point in the space with respect to the center of the camera: 106 | 107 | - object_centroid.position.x 108 | - object_centroid.position.y 109 | - object_centroid.position.z 110 | - object_centroid.orientation.x = quaternion[0] 111 | - object_centroid.orientation.y = quaternion[1] 112 | - object_centroid.orientation.z = quaternion[2] 113 | - object_centroid.orientation.w = quaternion[3] 114 | 115 | In the PointCloud2 message, the axes are as follows: 116 | 117 | - x: positive from the center of the camera to the right 118 | - y: positive from the center of the camera to the bottom 119 | - z: positive from the center of the camera to the front 120 | 121 | With the origin at the center of the camera, the XY axes (front view) and the `ROLL` angle of a point `p` on the plane are: 122 | 123 |

124 | 125 |

126 | 127 | the YZ axes (side view) and the `PITCH` angle of a point `p` on the plane are 128 | 129 |

130 | 131 |

132 | 133 | and the XZ axes (top view) and the `YAW` angle of a point `p` on the plane are: 134 | 135 |

136 | 137 |

138 | 139 | ### Homework 1.1 140 | 141 | Please, inspect the `rgbd_reader.cpp` implementation. The structure is very similar in C++ and Python, so you can use any of them, depending on your requirements. 142 | 143 | 144 | # 2. Point Cloud's manipulation 145 | 146 | ## 2.1 Single element access 147 | 148 | Now, we will process this information in a timer function `point_cloud_processing`, in Python 149 | 150 | ```python 151 | self.processing_timer_ = self.create_timer(0.030, self.point_cloud_processing) 152 | ``` 153 | 154 | and in C++ 155 | 156 | ```cpp 157 | processing_timer_ = this->create_wall_timer( 158 | std::chrono::milliseconds(30), 159 | std::bind(&PointCloudCentroidNode::point_cloud_processing, this)); 160 | ``` 161 | 162 | We access a single element in our array just as any array in Python `self.point_cloud_[row_id, col_id, 0]`. To access a single dimension X, Y, or Z, we can indicate it directly `self.point_cloud_[row_id, col_id, 0][XYZ]`, where XYZ=0 for the dimension X, XYZ=1 for the dimension Y, and XYZ=2 for the dimension Z. In our example, to access the 3D information in the central point of our image we enter 163 | 164 | ```python 165 | rows, cols, _= self.point_cloud_.shape 166 | row_id = int(rows/2) 167 | col_id = int(cols/2) 168 | 169 | p = [float(self.point_cloud_[row_id, col_id, 0][0]), 170 | float(self.point_cloud_[row_id, col_id, 0][1]), 171 | float(self.point_cloud_[row_id, col_id, 0][2])] 172 | ``` 173 | 174 | In C++, we access an element in out matrix as `point_cloud_.at(row_id, col_id)`, and each component as `point_cloud_.at(row_id, col_id)[XYZ]`, where XYZ=0 for the dimension X, XYZ=1 for the dimension Y, and XYZ=2 for the dimension Z 175 | 176 | ```cpp 177 | int rows = point_cloud_.rows; 178 | int cols = point_cloud_.cols; 179 | int row_id = rows / 2; 180 | int col_id = cols / 2; 181 | 182 | cv::Vec4f point = point_cloud_.at(row_id, col_id); 183 | ``` 184 | 185 | Finally, we store it in our global variable to be published later in the program's main loop by our ROS publisher. In Python 186 | 187 | ```python 188 | self.centroid_=Pose() 189 | 190 | self.centroid_.position.x = p[0] 191 | self.centroid_.position.y = p[1] 192 | self.centroid_.position.z = p[2] 193 | 194 | self.centroid_.orientation.x=0.0 195 | self.centroid_.orientation.y=0.0 196 | self.centroid_.orientation.z=0.0 197 | self.centroid_.orientation.w=1.0 198 | 199 | #Publish centroid pose 200 | self.centroid_publisher_.publish(self.centroid_) 201 | ``` 202 | 203 | and in C++ 204 | 205 | ```cpp 206 | geometry_msgs::msg::Pose centroid; 207 | 208 | centroid.position.x = static_cast(point[0]); // x 209 | centroid.position.y = static_cast(point[1]); // y 210 | centroid.position.z = static_cast(point[2]); // z 211 | 212 | centroid.orientation.x = 0.0; 213 | centroid.orientation.y = 0.0; 214 | centroid.orientation.z = 0.0; 215 | centroid.orientation.w = 1.0; 216 | 217 | // Publish the centroid pose 218 | centroid_publisher_->publish(centroid); 219 | ``` 220 | 221 | ## 2.2 Run your code 222 | 223 | Now, let's try our code. So, run the following commands: 224 | 225 | ```bash 226 | cd ~/robovision_ros2_ws 227 | colcon build 228 | ``` 229 | 230 | If you don't have an RGBD camera, don't worry, we provide you with a ROS bag with some data collected using an XTion Pro mounted at the top of a Turtlebot 2, at a height 1.0 meter from the floor. In a different terminal, run: 231 | 232 | ```bash 233 | ros2 bag play ~/robovision_ros2_ws/src/robovision_ros2/data/rosbags/person_static --loop 234 | ``` 235 | 236 | Then, in a different terminal enter 237 | 238 | ```bash 239 | ros2 topic list 240 | ``` 241 | 242 | Can you see all the different topics you can work with!? 243 | 244 | Now enter, for a Python implementation 245 | 246 | ```bash 247 | source ~/robovision_ros2_ws/install/setup.bash 248 | ros2 run robovision_rgbd rgbd_reader.py 249 | ``` 250 | 251 | or, for C++ implementation 252 | 253 | ```bash 254 | source ~/robovision_ros2_ws/install/setup.bash 255 | ros2 run robovision_rgbd rgbd_reader 256 | ``` 257 | 258 | Can you see the 3D information of our middle point in the image? 259 | 260 | Finally, in a new terminal: 261 | 262 | ```bash 263 | ros2 topic info /object_centroid 264 | ``` 265 | 266 | and, then 267 | 268 | ```bash 269 | ros2 topic echo /object_centroid 270 | ``` 271 | 272 | You should be able to see the same information being published in our ROS topic! 273 | 274 | ### Homework 2.1 275 | 276 | * Provide the 3D position of five different points in our image (enter different row_id, and col_id). Can you see how the X, Y, and Z values change with respect to the central point? X is positive to the right of our middle point and Y is positive below the middle point. 277 | 278 | * What's the 3D information for point (row_id=0, col_id=0)? Please note that, when the information is not available for a given point due to the structured light reflection properties, the system returns 'nan' values. In Python you can check if a variable is `nan` with the `math.isnan()` function in the `math` library -- this function returns a `True` or `False` value. You can validate your data using the `if ( not math.isnan(( self.point_cloud_[row_id, col_id, 0][0] ) ):` structure, for example. Similarly, in C++ we have `std::isnan()`. 279 | 280 | # 3. Final project 281 | 282 | Now you have all the tools to program a nice robot vision project. 283 | 284 | The problem is this: We have a robot facing towards a moving person and **we want to find the relative position of this person to the center of the robot (represented here as the camera position).** 285 | 286 | In other words, we want to find the 3D position of person_1 287 | 288 |

289 | 290 |

291 | 292 | How can you do it? 293 | 294 | You can use the `person_static` and the `person_dynamic` ROS bags in the `~/robovision_ros2_ws/src/robovision_ros2/data/rosbags/` folder if you don't have an RGBD camera. 295 | 296 | **Hint** Remember that the Point Cloud returns all the 3D information of all points visible by the camera, so why not limit it? You can create a valid zone where you can track your person better without any extra information 297 | 298 |

299 | 300 |

301 | 302 | You can use a structure where you compare if every image 3D point is inside this box (first check if your point is not `nan`) 303 | 304 |

305 | 306 |

307 | 308 | Don't forget to use the appropriate signs, especially in the Y-axis, if the camera is 1.0 meter above the floor, what condition should any point different than the floor meet? Again, pay special attention to the Y-axis direction. 309 | 310 | **Hint** Remember the `ROLL`, `PITCH` and `YAW` definitions 311 | 312 |

313 | 314 |

315 | 316 | You can convert those values to quaternion form by using the Python function 317 | 318 | ```python 319 | from tf_transformations import quaternion_from_euler 320 | quaternion = quaternion_from_euler(roll, pitch, yaw) 321 | ``` 322 | 323 | and then store it in our variable 324 | 325 | ```python 326 | object_centroid.orientation.x = quaternion[0] 327 | object_centroid.orientation.y = quaternion[1] 328 | object_centroid.orientation.z = quaternion[2] 329 | object_centroid.orientation.w = quaternion[3] 330 | ``` 331 | 332 | Similarly, in C++ 333 | 334 | ```cpp 335 | tf2::Quaternion quaternion; 336 | quaternion.setRPY(roll, pitch, yaw); 337 | ``` 338 | 339 | and 340 | 341 | ```cpp 342 | centroid.orientation.x = quaternion.x(); 343 | centroid.orientation.y = quaternion.y(); 344 | centroid.orientation.z = quaternion.z(); 345 | centroid.orientation.w = quaternion.w(); 346 | ``` 347 | 348 | Now, try implementing it in Python and C++. Good luck! 349 | 350 | ### Challenge 3.1 351 | 352 | How can you improve the performance? **Important: Remember that each 2D image point has its corresponding 3D point in the Point Cloud, you can use this information!** Maybe you can try detecting the person's face first and then take the 3D points average inside that region. OpenCV provides a series of functions that might help you with that, e.g. 353 | 354 | > https://docs.opencv.org/3.4/db/d28/tutorial_cascade_classifier.html 355 | 356 | What about detecting the whole person's body and getting the 3D points average inside the person's bounding box? In the example above, you can add an extra classifier of the form: 357 | 358 | > bodydetection = cv2.CascadeClassifier('cascades/haarcascade_fullbody.xml') 359 | 360 | Would you accept the challenge? 361 | 362 | ## Authors 363 | 364 | * **Luis Contreras** - [ARTenshi](https://artenshi.github.io/) 365 | * **Hiroyuki Okada** - [AIBot](http://aibot.jp/) 366 | -------------------------------------------------------------------------------- /robovision_3/images/point_depth.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ARTenshi/robovision_ros2/a825a9236d40c8d98e7793a570fb3cbf49d2238b/robovision_3/images/point_depth.jpg -------------------------------------------------------------------------------- /robovision_3/images/rgbd_view.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ARTenshi/robovision_ros2/a825a9236d40c8d98e7793a570fb3cbf49d2238b/robovision_3/images/rgbd_view.jpg -------------------------------------------------------------------------------- /robovision_3/images/scene_box.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ARTenshi/robovision_ros2/a825a9236d40c8d98e7793a570fb3cbf49d2238b/robovision_3/images/scene_box.jpg -------------------------------------------------------------------------------- /robovision_3/images/scene_crop.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ARTenshi/robovision_ros2/a825a9236d40c8d98e7793a570fb3cbf49d2238b/robovision_3/images/scene_crop.jpg -------------------------------------------------------------------------------- /robovision_3/images/scene_full.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ARTenshi/robovision_ros2/a825a9236d40c8d98e7793a570fb3cbf49d2238b/robovision_3/images/scene_full.jpg -------------------------------------------------------------------------------- /robovision_3/images/scene_person.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ARTenshi/robovision_ros2/a825a9236d40c8d98e7793a570fb3cbf49d2238b/robovision_3/images/scene_person.jpg -------------------------------------------------------------------------------- /robovision_3/images/xy_view.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ARTenshi/robovision_ros2/a825a9236d40c8d98e7793a570fb3cbf49d2238b/robovision_3/images/xy_view.jpg -------------------------------------------------------------------------------- /robovision_3/images/xz_view.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ARTenshi/robovision_ros2/a825a9236d40c8d98e7793a570fb3cbf49d2238b/robovision_3/images/xz_view.jpg -------------------------------------------------------------------------------- /robovision_3/images/yz_view.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ARTenshi/robovision_ros2/a825a9236d40c8d98e7793a570fb3cbf49d2238b/robovision_3/images/yz_view.jpg -------------------------------------------------------------------------------- /robovision_3/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | robovision_rgbd 5 | 0.0.0 6 | TODO: Package description 7 | roboworks 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | ament_cmake_python 12 | 13 | rclcpp 14 | rclpy 15 | 16 | cv_bridge 17 | std_msgs 18 | sensor_msgs 19 | sensor_msgs_py 20 | geometry_msgs 21 | 22 | tf2 23 | tf2_geometry_msgs 24 | 25 | ament_lint_auto 26 | ament_lint_common 27 | 28 | 29 | ament_cmake 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /robovision_3/robovision_rgbd/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ARTenshi/robovision_ros2/a825a9236d40c8d98e7793a570fb3cbf49d2238b/robovision_3/robovision_rgbd/__init__.py -------------------------------------------------------------------------------- /robovision_3/scripts/rgbd_reader.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import rclpy 3 | from rclpy.node import Node 4 | 5 | from geometry_msgs.msg import Pose 6 | from sensor_msgs.msg import Image, PointCloud2 7 | import sensor_msgs_py.point_cloud2 as pc2 8 | 9 | import cv2 10 | from cv_bridge import CvBridge 11 | 12 | import numpy as np 13 | import traceback 14 | 15 | 16 | class PointCloudCentroidNode(Node): 17 | def __init__(self): 18 | super().__init__("point_cloud_centroid") 19 | 20 | self.rgb_ = [] 21 | self.depth_ = [] 22 | self.depth_mat_ = [] 23 | self.point_cloud_ = [] 24 | 25 | self.centroid_=Pose() 26 | 27 | self.is_ptcld_ = False 28 | self.display_ = True 29 | 30 | #Publishers 31 | self.centroid_publisher_ = self.create_publisher( 32 | Pose, "/object_centroid", 10) 33 | 34 | #Subscribers 35 | self.rgb_subscriber_ = self.create_subscription( 36 | Image, "/camera/rgb/image_rect_color", self.callback_rgb_rect, 10) 37 | self.depth_subscriber_ = self.create_subscription( 38 | Image, "/camera/depth_registered/image", self.callback_depth_rect, 10) 39 | self.point_cloud_subscriber_ = self.create_subscription( 40 | PointCloud2, "/camera/depth_registered/points", self.callback_point_cloud, 10) 41 | 42 | #Processing 43 | self.processing_timer_ = self.create_timer(0.030, self.point_cloud_processing) #update image each 30 miliseconds 44 | 45 | self.get_logger().info("Starting point_cloud_centroid application in python...") 46 | 47 | def callback_rgb_rect(self, msg): 48 | try: 49 | #Transform the new message to an OpenCV matrix 50 | bridge_rgb=CvBridge() 51 | self.rgb_ = bridge_rgb.imgmsg_to_cv2(msg,msg.encoding).copy() 52 | self.rgb_ = cv2.cvtColor(self.rgb_, cv2.COLOR_RGB2BGR) 53 | 54 | except: 55 | self.get_logger().error(traceback.format_exc()) 56 | 57 | def callback_depth_rect(self, msg): 58 | try: 59 | #Transform the new message to an OpenCV matrix 60 | bridge_depth=CvBridge() 61 | self.depth_=bridge_depth.imgmsg_to_cv2(msg,"32FC1").copy() 62 | 63 | self.depth_mat_ = np.array(self.depth_, dtype=np.float32) 64 | cv2.normalize(self.depth_mat_, self.depth_, 0, 1, cv2.NORM_MINMAX) 65 | 66 | except: 67 | self.get_logger().error(traceback.format_exc()) 68 | 69 | def callback_point_cloud(self, msg): 70 | try: 71 | self.is_ptcld_ = False 72 | # Read the point cloud as a list of tuples (x, y, z, ...) 73 | self.point_cloud_ = np.array(list(pc2.read_points(msg, field_names=None, skip_nans=False))) 74 | 75 | # Reshape if the point cloud is organized (msg.height > 1) 76 | if msg.height > 1: 77 | self.point_cloud_ = self.point_cloud_.reshape((msg.height, msg.width, -1)) 78 | 79 | rows, cols, _ = self.point_cloud_.shape 80 | print ('new message has arrived; point cloud size: rows: {}, cols: {}'.format(rows, cols)) 81 | 82 | self.is_ptcld_ = True 83 | except: 84 | self.get_logger().error(traceback.format_exc()) 85 | self.is_ptcld_ = False 86 | 87 | def point_cloud_processing(self): 88 | if self.is_ptcld_: 89 | 90 | #Access a point in the point_cloud 91 | rows, cols, _= self.point_cloud_.shape 92 | row_id = int(rows/2) 93 | col_id = int(cols/2) 94 | 95 | p = [float(self.point_cloud_[row_id, col_id, 0][0]), 96 | float(self.point_cloud_[row_id, col_id, 0][1]), 97 | float(self.point_cloud_[row_id, col_id, 0][2])] 98 | 99 | self.centroid_.position.x = p[0] 100 | self.centroid_.position.y = p[1] 101 | self.centroid_.position.z = p[2] 102 | 103 | self.centroid_.orientation.x=0.0 104 | self.centroid_.orientation.y=0.0 105 | self.centroid_.orientation.z=0.0 106 | self.centroid_.orientation.w=1.0 107 | 108 | print ('central point: {}'.format(p)) 109 | 110 | #Publish centroid pose 111 | self.centroid_publisher_.publish(self.centroid_) 112 | 113 | #Show your images 114 | if(self.display_): 115 | cv2.imshow("rgb", self.rgb_) 116 | cv2.imshow("depth", self.depth_) 117 | cv2.waitKey(1) 118 | 119 | 120 | def main(args=None): 121 | rclpy.init(args=args) 122 | node = PointCloudCentroidNode() 123 | 124 | try: 125 | rclpy.spin(node) 126 | except KeyboardInterrupt: 127 | pass 128 | finally: 129 | cv2.destroyAllWindows() 130 | node.destroy_node() 131 | rclpy.shutdown() 132 | 133 | 134 | if __name__ == "__main__": 135 | main() 136 | -------------------------------------------------------------------------------- /robovision_3/src/rgbd_reader.cpp: -------------------------------------------------------------------------------- 1 | #include "rclcpp/rclcpp.hpp" 2 | 3 | #include "geometry_msgs/msg/pose.hpp" 4 | #include "sensor_msgs/msg/image.hpp" 5 | #include "sensor_msgs/msg/point_field.hpp" 6 | #include "sensor_msgs/msg/point_cloud2.hpp" 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | class PointCloudCentroidNode : public rclcpp::Node 17 | { 18 | public: 19 | PointCloudCentroidNode() : Node("point_cloud_centroid"), is_ptcld_(false), display_(true) 20 | { 21 | // Publishers 22 | centroid_publisher_ = this->create_publisher( 23 | "/object_centroid", 10); 24 | 25 | // Subscribers 26 | rgb_sub_ = this->create_subscription( 27 | "/camera/rgb/image_rect_color", 10, 28 | std::bind(&PointCloudCentroidNode::callback_rgb_rect, this, std::placeholders::_1)); 29 | 30 | depth_sub_ = this->create_subscription( 31 | "/camera/depth_registered/image", 10, 32 | std::bind(&PointCloudCentroidNode::callback_depth_rect, this, std::placeholders::_1)); 33 | 34 | point_cloud_sub_ = this->create_subscription( 35 | "/camera/depth_registered/points", 10, 36 | std::bind(&PointCloudCentroidNode::callback_point_cloud, this, std::placeholders::_1)); 37 | 38 | // Processing 39 | processing_timer_ = this->create_wall_timer( 40 | std::chrono::milliseconds(30), 41 | std::bind(&PointCloudCentroidNode::point_cloud_processing, this)); 42 | 43 | RCLCPP_INFO(this->get_logger(), "Starting point_cloud_centroid application in cpp..."); 44 | } 45 | 46 | private: 47 | 48 | bool is_ptcld_; 49 | bool display_; 50 | cv::Mat rgb_, depth_, depth_mat_, point_cloud_; 51 | 52 | rclcpp::Publisher::SharedPtr centroid_publisher_; 53 | 54 | rclcpp::Subscription::SharedPtr rgb_sub_; 55 | rclcpp::Subscription::SharedPtr depth_sub_; 56 | rclcpp::Subscription::SharedPtr point_cloud_sub_; 57 | 58 | rclcpp::TimerBase::SharedPtr processing_timer_; 59 | 60 | void callback_rgb_rect(const sensor_msgs::msg::Image::SharedPtr msg) 61 | { 62 | try 63 | { 64 | cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, msg->encoding); 65 | rgb_ = cv_ptr->image.clone(); 66 | cv::cvtColor(rgb_, rgb_, cv::COLOR_RGB2BGR); 67 | } 68 | catch (const std::exception &e) 69 | { 70 | RCLCPP_ERROR(this->get_logger(), "Error processing RGB image: %s", e.what()); 71 | } 72 | } 73 | 74 | void callback_depth_rect(const sensor_msgs::msg::Image::SharedPtr msg) 75 | { 76 | try 77 | { 78 | cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "32FC1"); 79 | depth_ = cv_ptr->image.clone(); 80 | 81 | depth_mat_ = cv::Mat(depth_.size(), CV_32F); 82 | depth_.convertTo(depth_mat_, CV_32F); 83 | cv::normalize(depth_mat_, depth_, 0, 1, cv::NORM_MINMAX); 84 | } 85 | catch (const std::exception &e) 86 | { 87 | RCLCPP_ERROR(this->get_logger(), "Error processing Depth image: %s", e.what()); 88 | } 89 | } 90 | 91 | void callback_point_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg) 92 | { 93 | is_ptcld_ = false; 94 | try 95 | { 96 | // Ensure the point cloud is organized 97 | if (msg->height > 1) 98 | { 99 | // Convert PointCloud2 to cv::Mat 100 | point_cloud_ = cv::Mat(msg->height, msg->width, CV_32FC4); 101 | const uint8_t *data_ptr = msg->data.data(); 102 | 103 | for (size_t i = 0; i < msg->height; ++i) 104 | { 105 | for (size_t j = 0; j < msg->width; ++j) 106 | { 107 | const float *point = reinterpret_cast(data_ptr + (i * msg->row_step + j * msg->point_step)); 108 | point_cloud_.at(i, j) = cv::Vec4f(point[0], point[1], point[2], point[3]); 109 | } 110 | } 111 | 112 | is_ptcld_ = true; 113 | RCLCPP_INFO(this->get_logger(), "Point cloud converted to cv::Mat with size [%d, %d]", msg->height, msg->width); 114 | } 115 | else 116 | { 117 | RCLCPP_WARN(this->get_logger(), "Point cloud is not organized. Cannot convert to cv::Mat."); 118 | } 119 | } 120 | catch (const std::exception& e) 121 | { 122 | RCLCPP_ERROR(this->get_logger(), "Error converting point cloud: %s", e.what()); 123 | } 124 | } 125 | 126 | void point_cloud_processing() 127 | { 128 | if (is_ptcld_) 129 | { 130 | // Access a point in the point_cloud (e.g., midpoint of the matrix) 131 | int rows = point_cloud_.rows; 132 | int cols = point_cloud_.cols; 133 | int row_id = rows / 2; 134 | int col_id = cols / 2; 135 | 136 | // Ensure the indices are within valid bounds 137 | if (row_id >= 0 && row_id < rows && col_id >= 0 && col_id < cols) 138 | { 139 | // Extract the point at the specified location (row_id, col_id) 140 | cv::Vec4f point = point_cloud_.at(row_id, col_id); 141 | 142 | // Assign the extracted point's coordinates to centroid_ 143 | geometry_msgs::msg::Pose centroid; 144 | 145 | centroid.position.x = static_cast(point[0]); // x 146 | centroid.position.y = static_cast(point[1]); // y 147 | centroid.position.z = static_cast(point[2]); // z 148 | 149 | // Set a default orientation 150 | centroid.orientation.x = 0.0; 151 | centroid.orientation.y = 0.0; 152 | centroid.orientation.z = 0.0; 153 | centroid.orientation.w = 1.0; 154 | 155 | RCLCPP_INFO(this->get_logger(), "Central point: x=%.3f, y=%.3f, z=%.3f", point[0], point[1], point[2]); 156 | 157 | // Publish the centroid pose 158 | centroid_publisher_->publish(centroid); 159 | } 160 | else 161 | { 162 | RCLCPP_WARN(this->get_logger(), "PointCloud index out of bounds"); 163 | } 164 | 165 | if (display_) 166 | { 167 | cv::imshow("RGB Image", rgb_); 168 | cv::imshow("Depth Image", depth_); 169 | cv::waitKey(1); 170 | } 171 | } 172 | else 173 | { 174 | RCLCPP_WARN(this->get_logger(), "Empty point cloud matrix."); 175 | } 176 | } 177 | }; 178 | 179 | int main(int argc, char **argv) 180 | { 181 | rclcpp::init(argc, argv); 182 | auto node = std::make_shared(); 183 | rclcpp::spin(node); 184 | 185 | cv::destroyAllWindows(); 186 | rclcpp::shutdown(); 187 | 188 | return 0; 189 | } 190 | -------------------------------------------------------------------------------- /robovision_4/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(robovision_services) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(ament_cmake_python REQUIRED) 11 | find_package(rclcpp REQUIRED) 12 | 13 | find_package(OpenCV REQUIRED) 14 | find_package(cv_bridge REQUIRED) 15 | 16 | find_package(std_msgs REQUIRED) 17 | find_package(sensor_msgs REQUIRED) 18 | find_package(geometry_msgs REQUIRED) 19 | 20 | find_package(tf2 REQUIRED) 21 | find_package(tf2_geometry_msgs REQUIRED) 22 | find_package(robovision_interfaces REQUIRED) 23 | 24 | include_directories(${cv_bridge_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) 25 | 26 | # C++ section 27 | add_executable(robovision_service src/robovision_service.cpp) 28 | ament_target_dependencies(robovision_service 29 | rclcpp 30 | cv_bridge 31 | sensor_msgs 32 | geometry_msgs 33 | robovision_interfaces 34 | OpenCV 35 | tf2 36 | tf2_geometry_msgs 37 | ) 38 | 39 | add_executable(robovision_client src/robovision_client.cpp) 40 | ament_target_dependencies(robovision_client 41 | rclcpp 42 | cv_bridge 43 | sensor_msgs 44 | geometry_msgs 45 | robovision_interfaces 46 | OpenCV 47 | tf2 48 | tf2_geometry_msgs 49 | ) 50 | 51 | # Python section 52 | ament_python_install_package(${PROJECT_NAME}) 53 | 54 | # Ensure both are included 55 | install(TARGETS 56 | robovision_service 57 | robovision_client 58 | DESTINATION lib/${PROJECT_NAME} 59 | ) 60 | install(PROGRAMS 61 | scripts/robovision_service.py 62 | scripts/robovision_client.py 63 | DESTINATION lib/${PROJECT_NAME} 64 | ) 65 | 66 | 67 | ament_package() 68 | -------------------------------------------------------------------------------- /robovision_4/README.md: -------------------------------------------------------------------------------- 1 | # Services and Clients in ROS 2 | 3 | In ROS2, clients and services enable synchronous communication between nodes. Unlike publishers and subscribers, which facilitate continuous data streams, clients and services are designed for request-response interactions. A client node sends a request to a service, and the service node processes the request and sends back a response. This is ideal for tasks that require specific actions or immediate feedback, such as controlling a robot arm or querying a sensor's state. 4 | 5 | You can check some basic concepts for C++: 6 | 7 | > https://docs.ros.org/en/foxy/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Service-And-Client.html 8 | 9 | and for Python: 10 | 11 | > https://docs.ros.org/en/foxy/Tutorials/Beginner-Client-Libraries/Writing-A-Simple-Cpp-Service-And-Client.html 12 | 13 | # 1. ROS2 Interfaces 14 | 15 | In ROS2, interfaces allow nodes to communicate using predefined data structures. Interfaces come in two forms: 16 | - **Messages (`msg`)**: Define the structure of data for topics. 17 | - **Services (`srv`)**: Define request-response interactions for services. 18 | 19 | This tutorial uses examples from the `robovision_interfaces` package to demonstrate creating and using ROS2 interfaces. 20 | 21 | --- 22 | 23 | ## Setting Up the Package 24 | 25 | Organize the folder structure for your custom interfaces as follows: 26 | 27 | ``` 28 | robovision_interfaces/ 29 | ├── CMakeLists.txt 30 | ├── package.xml 31 | ├── msg/ 32 | │ └── ObjectCentroid.msg 33 | └── srv/ 34 | └── GetPointCenter.srv 35 | ``` 36 | 37 | This folder is at the same level as any new ROS2 package in your project. 38 | 39 | ## Defining a Custom Message: `ObjectCentroid.msg` 40 | 41 | A custom message describes the data structure for topics. The `ObjectCentroid.msg` defines the centroid coordinates and an array: 42 | 43 | ``` 44 | float64 x 45 | float64 y 46 | float64 z 47 | float64[] centroid 48 | ``` 49 | 50 | where 51 | 52 | - **`float64 x, y, z`**: Represent the 3D coordinates of the centroid. 53 | - **`float64[] centroid`**: A dynamic array to store additional data points. 54 | 55 | ## Defining a Custom Service: `GetPointCenter.srv` 56 | 57 | A custom service defines the structure of a request and a response. The `GetPointCenter.srv` file looks like this: 58 | 59 | ``` 60 | int64 x 61 | int64 y 62 | --- 63 | ObjectCentroid point 64 | ``` 65 | 66 | where 67 | 68 | - **Request (`int64 x, y`)**: Accepts two integer inputs (e.g., pixel coordinates). 69 | - **Response (`ObjectCentroid point`)**: Returns the computed centroid as an `ObjectCentroid` message. 70 | 71 | The `---` separates the request and response parts of the service definition. Notice that, if the message is defined in the same package, the package name does not appear in the service or message definition. If the message is defined elsewhere, we have to specify it, e.g. `robovision_interfaces/msg/ObjectCentroid point`. 72 | 73 | ## Integrating the Interfaces into the Build System 74 | 75 | Update the `CMakeLists.txt` to include the message and service definitions: 76 | 77 | ```cmake 78 | find_package(rosidl_default_generators REQUIRED) 79 | 80 | rosidl_generate_interfaces(${PROJECT_NAME} 81 | "msg/ObjectCentroid.msg" 82 | "srv/GetPointCenter.srv" 83 | ) 84 | 85 | ament_export_dependencies(rosidl_default_runtime) 86 | ``` 87 | 88 | and update the `package.xml` to declare dependencies: 89 | 90 | ```xml 91 | rosidl_default_generators 92 | rosidl_default_runtime 93 | ``` 94 | 95 | # 2. ROS Service 96 | 97 | The main difference between a topic and a service is that, while a topic is working, a service works under request (that might save resources). 98 | 99 | Let's compare our "rgbd_reader" and our "robovision_service" files (both in C++ and Python). They are very similar! We have two main changes. First, we don't have a publisher, as it sends a response under request. Second, we don't have a timer, as it is not working indefinitely. Instead, we create a ROS2 service that enters a callback function when we call the service. In Python it is 100 | 101 | ```python 102 | self.get_point_center_service_ = self.create_service( 103 | GetPointCenter, "get_point_center", self.point_cloud_processing) 104 | ``` 105 | 106 | and in C++ 107 | 108 | ```cpp 109 | get_point_center_service_ = this->create_service( 110 | "get_point_center", 111 | std::bind(&PointCloudCentroidNode::point_cloud_processing, this, 112 | std::placeholders::_1, std::placeholders::_2)); 113 | ``` 114 | 115 | Notice that we need to declare the service type, in this case, we are using our custom interface `robovision_interfaces/srv/GetPointCenter`. Also, we changed the definition of our callback function to incorporate the `request` and `response`. Finally, also note that we need to fill our `response` and return it (instead of publishing the result in a custom topic). 116 | 117 | ## 2.1 Test your code 118 | 119 | First, let's compile it 120 | 121 | ```bash 122 | cd ~/robovision_ros2_ws 123 | colcon build 124 | ``` 125 | 126 | and start it (don't forget to start your rosbag!) 127 | 128 | ```bash 129 | source ~/robovision_ros2_ws/install/setup.bash 130 | ros2 run robovision_services robovision_service 131 | ``` 132 | 133 | In a different terminal, enter 134 | 135 | ```bash 136 | ros2 service list 137 | ``` 138 | 139 | Can you see the service we just declared? Now enter 140 | 141 | ```bash 142 | ros2 service type /get_point_center 143 | ``` 144 | 145 | What's the interface it is using? You can inspect it 146 | 147 | ```bash 148 | ros2 interfaces show robovision_interfaces/srv/GetPointCenter 149 | ``` 150 | 151 | Finally, let's call our service 152 | 153 | ```bash 154 | ros2 service call /get_point_center robovision_interfaces/srv/GetPointCenter "{x: 320, y: 240}" 155 | ``` 156 | 157 | What was the result? 158 | 159 | # 3. ROS Client 160 | 161 | In ROS2, clients are used to interact with services in a synchronous request-response manner. This tutorial will demonstrate how to implement and utilize a ROS2 client. The example involves a `PointCloudCentroidNode` that calls a service named `get_point_center` to compute the centroid of a 2D point. 162 | 163 | ## 3.1 Client in Python 164 | 165 | The `robovision_client.py` file demonstrates a ROS2 client implementation for interacting with the `GetPointCenter` service. Below, we break down its key components. 166 | 167 | ### 3.1.1 **Node Initialization** 168 | 169 | The `PointCloudCentroidNode` class initializes a ROS2 node named `point_cloud_client`. 170 | 171 | ```python 172 | self.declare_parameter("x", 320) 173 | self.declare_parameter("y", 240) 174 | 175 | self.x_ = self.get_parameter("x").value 176 | self.y_ = self.get_parameter("y").value 177 | ``` 178 | Parameters `x` and `y` define the target coordinates to query from the service. 179 | 180 | ### 3.1.2 **Service Call Setup** 181 | 182 | To show that we can call a service at any moment, we create a ROS2 timer to call the service periodically every 2.5 seconds: 183 | 184 | ```python 185 | self.client_call_timer_ = self.create_timer(2.5, self.client_caller) 186 | ``` 187 | 188 | The `client_caller()` function prepares and sends a service request through a `call_get_point_center_server`: 189 | 190 | ```python 191 | def client_caller(self): 192 | self.call_get_point_center_server(self.x_, self.y_) 193 | ``` 194 | 195 | Every time we want to call our service, we use this function so, you can use it as a template. We will explain it next. 196 | 197 | #### **Creating the Client and Making a Request** 198 | 199 | A ROS2 client is created with the specified service type (`GetPointCenter`) and name (`get_point_center`): 200 | 201 | ```python 202 | client = self.create_client(GetPointCenter, "get_point_center") 203 | while not client.wait_for_service(1.0): 204 | self.get_logger().warn("Waiting for get_point_center service...") 205 | ``` 206 | 207 | The request message is populated, and an asynchronous call is made: 208 | 209 | ```python 210 | request = GetPointCenter.Request() 211 | request.x = _x 212 | request.y = _y 213 | 214 | future = client.call_async(request) 215 | future.add_done_callback( 216 | partial(self.callback_call_point_cloud, _x=request.x, _y=request.y)) 217 | ``` 218 | 219 | #### **Handling the Response** 220 | 221 | The callback `callback_call_point_cloud()` processes the service response (we store the response in a global variable for future use): 222 | 223 | ```python 224 | def callback_call_point_cloud(self, future, _x, _y): 225 | try: 226 | response = future.result() 227 | self.get_logger().info( 228 | "(" + str(_x) + ", " + str(_y) + ") position is: " + str(response.point)) 229 | 230 | self.point_ = response.point 231 | except Exception as e: 232 | self.get_logger().error("Service call failed %r" % (e,)) 233 | ``` 234 | 235 | --- 236 | 237 | ### 3.1.3 Test your code 238 | 239 | First, let's compile it 240 | 241 | ```bash 242 | cd ~/robovision_ros2_ws 243 | colcon build 244 | ``` 245 | 246 | and start our service (don't forget to start your rosbag!) 247 | 248 | ```bash 249 | source ~/robovision_ros2_ws/install/setup.bash 250 | ros2 run robovision_services robovision_service.py 251 | ``` 252 | 253 | In a different terminal, enter 254 | 255 | ```bash 256 | source ~/robovision_ros2_ws/install/setup.bash 257 | ros2 run robovision_services robovision_client.py 258 | ``` 259 | 260 | What was the result? 261 | 262 | 263 | ## 3.2 Client in C++ 264 | 265 | The `robovision_client.cpp` file provides an equivalent implementation of the ROS2 client in C++. Below are the highlights and key differences from the Python example. 266 | 267 | As in our Python implementation, we create a timer to show that we can call our service at any moment: 268 | 269 | ```cpp 270 | client_call_timer_ = this->create_wall_timer( 271 | std::chrono::milliseconds(2500), 272 | std::bind(&PointCloudCentroidNode::client_caller, this)); 273 | ``` 274 | 275 | The client_caller function calls a `get_point_center()` function; we use this structure as a template. A major difference with Python is that we need to call our serves in a thread, so the program flow can continue: 276 | 277 | ```cpp 278 | threads_.push_back(std::thread(std::bind(&PointCloudCentroidNode::call_get_point_center_server, this, x, y))); 279 | ``` 280 | 281 | The `call_get_point_center_server` in our thread works as in our Python implementation. A C++ client is created with the service type `robovision_interfaces::srv::GetPointCenter` and name `get_point_center`: 282 | 283 | ```cpp 284 | auto client = this->create_client("get_point_center"); 285 | while (!client->wait_for_service(std::chrono::seconds(1))) { 286 | RCLCPP_WARN(this->get_logger(), "Waiting for get_point_center service..."); 287 | } 288 | ``` 289 | 290 | The request is constructed and sent asynchronously: 291 | 292 | ```cpp 293 | auto request = std::make_shared(); 294 | request->x = x; 295 | request->y = y; 296 | 297 | auto future = client->async_send_request(request); 298 | ``` 299 | 300 | The response from the service is processed synchronously by waiting for the future object: 301 | 302 | ```cpp 303 | try { 304 | auto response = future.get(); 305 | RCLCPP_INFO(this->get_logger(), 306 | "(%d, %d) position is: [%f, %f, %f]", 307 | x, y, response->point.centroid[0], 308 | response->point.centroid[1], 309 | response->point.centroid[2]); 310 | 311 | point_ = response->point.centroid; 312 | } catch (const std::exception &e) { 313 | RCLCPP_ERROR(this->get_logger(), "Service call failed."); 314 | } 315 | ``` 316 | 317 | ### 3.2.1 Test your code 318 | 319 | First, let's compile it 320 | 321 | ```bash 322 | cd ~/robovision_ros2_ws 323 | colcon build 324 | ``` 325 | 326 | and start our service (don't forget to start your rosbag!) 327 | 328 | ```bash 329 | source ~/robovision_ros2_ws/install/setup.bash 330 | ros2 run robovision_services robovision_service 331 | ``` 332 | 333 | In a different terminal, enter 334 | 335 | ```bash 336 | source ~/robovision_ros2_ws/install/setup.bash 337 | ros2 run robovision_services robovision_client 338 | ``` 339 | 340 | What was the result? 341 | 342 | ## Authors 343 | 344 | * **Luis Contreras** - [ARTenshi](https://artenshi.github.io/) 345 | * **Hiroyuki Okada** - [AIBot](http://aibot.jp/) 346 | 347 | -------------------------------------------------------------------------------- /robovision_4/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | robovision_services 5 | 0.0.0 6 | TODO: Package description 7 | roboworks 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | ament_cmake_python 12 | 13 | rclcpp 14 | rclpy 15 | 16 | cv_bridge 17 | std_msgs 18 | sensor_msgs 19 | sensor_msgs_py 20 | geometry_msgs 21 | 22 | tf2 23 | tf2_geometry_msgs 24 | robovision_interfaces 25 | 26 | ament_lint_auto 27 | ament_lint_common 28 | 29 | 30 | ament_cmake 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /robovision_4/robovision_services/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ARTenshi/robovision_ros2/a825a9236d40c8d98e7793a570fb3cbf49d2238b/robovision_4/robovision_services/__init__.py -------------------------------------------------------------------------------- /robovision_4/scripts/robovision_client.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import rclpy 3 | from rclpy.node import Node 4 | from functools import partial 5 | 6 | from robovision_interfaces.msg import ObjectCentroid 7 | from robovision_interfaces.srv import GetPointCenter 8 | 9 | import numpy as np 10 | import traceback 11 | 12 | 13 | class PointCloudCentroidNode(Node): 14 | def __init__(self): 15 | super().__init__("point_cloud_client") 16 | 17 | #Declare parameters 18 | self.declare_parameter("x", 320) 19 | self.declare_parameter("y", 240) 20 | 21 | #Get parameter values 22 | self.x_ = self.get_parameter("x").value 23 | self.y_ = self.get_parameter("y").value 24 | 25 | self.point_ = [] 26 | 27 | #Processing timer 28 | #We call our service once each 2.5 seconds 29 | self.client_call_timer_ = self.create_timer(2.5, self.client_caller) 30 | 31 | self.get_logger().info("Starting point_cloud_client application in python...") 32 | 33 | def client_caller(self): 34 | 35 | self.call_get_point_center_server(self.x_, self.y_) 36 | 37 | #Do something with the result 38 | if (self.point_): 39 | self.get_logger().info("(" + str(self.x_) + ", " + str(self.y_) + ") position is: " + str(self.point_)) 40 | self.point_ = [] 41 | 42 | #Basic server call template 43 | def call_get_point_center_server(self, _x, _y): 44 | client = self.create_client(GetPointCenter, "get_point_center") 45 | while not client.wait_for_service(1.0): 46 | self.get_logger().warn("Waiting for get_point_center service...") 47 | 48 | request = GetPointCenter.Request() 49 | request.x = _x 50 | request.y = _y 51 | 52 | future = client.call_async(request) 53 | 54 | #The callback only contains the response; 55 | #we explicity add the request for debugging purposes 56 | future.add_done_callback( 57 | partial(self.callback_call_point_cloud, _x=request.x, _y=request.y)) 58 | 59 | def callback_call_point_cloud(self, future, _x, _y): 60 | try: 61 | response = future.result() 62 | self.get_logger().info("(" + str(_x) + ", " + str(_y) + ") position is: " + str(response.point)) 63 | 64 | self.point_ = response.point 65 | except Exception as e: 66 | self.get_logger().error("Service call failed %r" % (e,)) 67 | 68 | def main(args=None): 69 | rclpy.init(args=args) 70 | node = PointCloudCentroidNode() 71 | 72 | try: 73 | rclpy.spin(node) 74 | except KeyboardInterrupt: 75 | pass 76 | finally: 77 | node.destroy_node() 78 | rclpy.shutdown() 79 | 80 | 81 | if __name__ == "__main__": 82 | main() 83 | -------------------------------------------------------------------------------- /robovision_4/scripts/robovision_service.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import rclpy 3 | from rclpy.node import Node 4 | 5 | from geometry_msgs.msg import Pose 6 | from sensor_msgs.msg import Image, PointCloud2 7 | import sensor_msgs_py.point_cloud2 as pc2 8 | 9 | import cv2 10 | from cv_bridge import CvBridge 11 | 12 | import numpy as np 13 | import traceback 14 | 15 | from robovision_interfaces.msg import ObjectCentroid 16 | from robovision_interfaces.srv import GetPointCenter 17 | 18 | class PointCloudCentroidNode(Node): 19 | def __init__(self): 20 | super().__init__("point_cloud_service") 21 | 22 | self.rgb_ = [] 23 | self.depth_ = [] 24 | self.depth_mat_ = [] 25 | self.point_cloud_ = [] 26 | 27 | self.centroid_=Pose() 28 | 29 | self.is_ptcld_ = False 30 | self.display_ = True 31 | 32 | #Publishers 33 | #self.centroid_publisher_ = self.create_publisher( 34 | # Pose, "/object_centroid", 10) 35 | 36 | #Subscribers 37 | self.subscriber_ = self.create_subscription( 38 | Image, "/camera/rgb/image_rect_color", self.callback_rgb_rect, 10) 39 | self.subscriber_ = self.create_subscription( 40 | Image, "/camera/depth_registered/image", self.callback_depth_rect, 10) 41 | self.subscriber_ = self.create_subscription( 42 | PointCloud2, "/camera/depth_registered/points", self.callback_point_cloud, 10) 43 | 44 | #Processing 45 | #self.processing_timer_ = self.create_timer(0.030, self.point_cloud_processing) #update image each 30 miliseconds 46 | 47 | #Services 48 | self.get_point_center_service_ = self.create_service( 49 | GetPointCenter, "get_point_center", self.point_cloud_processing) 50 | 51 | self.get_logger().info("Starting point_cloud_service application in python...") 52 | 53 | def callback_rgb_rect(self, msg): 54 | try: 55 | #Transform the new message to an OpenCV matrix 56 | bridge_rgb=CvBridge() 57 | self.rgb_ = bridge_rgb.imgmsg_to_cv2(msg,msg.encoding).copy() 58 | self.rgb_ = cv2.cvtColor(self.rgb_, cv2.COLOR_RGB2BGR) 59 | 60 | except: 61 | self.get_logger().error(traceback.format_exc()) 62 | 63 | def callback_depth_rect(self, msg): 64 | try: 65 | #Transform the new message to an OpenCV matrix 66 | bridge_depth=CvBridge() 67 | self.depth_=bridge_depth.imgmsg_to_cv2(msg,"32FC1").copy() 68 | 69 | self.depth_mat_ = np.array(self.depth_, dtype=np.float32) 70 | cv2.normalize(self.depth_mat_, self.depth_, 0, 1, cv2.NORM_MINMAX) 71 | 72 | except: 73 | self.get_logger().error(traceback.format_exc()) 74 | 75 | def callback_point_cloud(self, msg): 76 | try: 77 | # Read the point cloud as a list of tuples (x, y, z, ...) 78 | self.point_cloud_ = np.array(list(pc2.read_points(msg, field_names=None, skip_nans=False))) 79 | 80 | # Reshape if the point cloud is organized (msg.height > 1) 81 | if msg.height > 1: 82 | self.point_cloud_ = self.point_cloud_.reshape((msg.height, msg.width, -1)) 83 | 84 | rows, cols, _ = self.point_cloud_.shape 85 | self.get_logger().info('Point cloud received with size: rows: {}, cols: {}'.format(rows, cols)) 86 | 87 | self.is_ptcld_ = True 88 | except: 89 | self.get_logger().error(traceback.format_exc()) 90 | 91 | def point_cloud_processing(self, request, response): 92 | 93 | response.point = ObjectCentroid() 94 | 95 | response.point.x = 0.0 96 | response.point.y = 0.0 97 | response.point.z = 0.0 98 | response.point.centroid = [0.0, 0.0, 0.0] 99 | 100 | if self.is_ptcld_: 101 | if request.x and request.y: 102 | #Access a point in the point_cloud 103 | col_id = request.x 104 | row_id = request.y 105 | 106 | p = [float(self.point_cloud_[row_id, col_id, 0][0]), 107 | float(self.point_cloud_[row_id, col_id, 0][1]), 108 | float(self.point_cloud_[row_id, col_id, 0][2])] 109 | 110 | response.point.x = p[0] 111 | response.point.y = p[1] 112 | response.point.z = p[2] 113 | response.point.centroid = p 114 | 115 | 116 | self.get_logger().info("Centroid at ({}, {}): [{}, {}, {}]".format(row_id, col_id, p[0], p[1], p[2])) 117 | 118 | else: 119 | self.get_logger().error("No PointCloud data available for processing.") 120 | 121 | #Publish centroid pose 122 | #self.centroid_publisher_.publish(self.centroid_) 123 | 124 | return response 125 | 126 | 127 | def main(args=None): 128 | rclpy.init(args=args) 129 | node = PointCloudCentroidNode() 130 | 131 | try: 132 | rclpy.spin(node) 133 | except KeyboardInterrupt: 134 | pass 135 | finally: 136 | cv2.destroyAllWindows() 137 | node.destroy_node() 138 | rclpy.shutdown() 139 | 140 | 141 | if __name__ == "__main__": 142 | main() 143 | -------------------------------------------------------------------------------- /robovision_4/src/robovision_client.cpp: -------------------------------------------------------------------------------- 1 | #include "rclcpp/rclcpp.hpp" 2 | 3 | #include "robovision_interfaces/srv/get_point_center.hpp" 4 | #include "robovision_interfaces/msg/object_centroid.hpp" 5 | 6 | #include 7 | 8 | 9 | class PointCloudCentroidNode : public rclcpp::Node 10 | { 11 | public: 12 | PointCloudCentroidNode() : Node("point_cloud_client"), point_({0.0, 0.0, 0.0}) 13 | { 14 | //Declare parameters 15 | this->declare_parameter("x", 320); 16 | this->declare_parameter("y", 240); 17 | 18 | // Get parameter values 19 | x_ = this->get_parameter("x").as_int(); 20 | y_ = this->get_parameter("y").as_int(); 21 | 22 | point_.clear(); 23 | 24 | // Processing timer 25 | //We call our service once each 2.5 seconds 26 | client_call_timer_ = this->create_wall_timer( 27 | std::chrono::milliseconds(2500), 28 | std::bind(&PointCloudCentroidNode::client_caller, this)); 29 | 30 | RCLCPP_INFO(this->get_logger(), "Starting point_cloud_client application in cpp..."); 31 | } 32 | 33 | private: 34 | 35 | int x_, y_; 36 | std::vector point_; 37 | 38 | rclcpp::TimerBase::SharedPtr client_call_timer_; 39 | std::vector threads_; 40 | 41 | void client_caller() 42 | { 43 | try 44 | { 45 | get_point_center(x_, y_); 46 | } 47 | catch(const std::exception& e) 48 | { 49 | RCLCPP_ERROR(this->get_logger(), "Service call failed."); 50 | } 51 | 52 | // Do something with the result 53 | if (!point_.empty()) 54 | { 55 | RCLCPP_INFO(this->get_logger(), 56 | "(%d, %d) position is: [%f, %f, %f]", 57 | x_, y_, point_[0], point_[1], point_[2]); 58 | 59 | // Clear point data 60 | point_.clear(); 61 | } 62 | } 63 | 64 | void get_point_center(int x, int y) 65 | { 66 | threads_.push_back(std::thread(std::bind(&PointCloudCentroidNode::call_get_point_center_server, this, x, y))); 67 | } 68 | 69 | void call_get_point_center_server(int x, int y) 70 | { 71 | auto client = this->create_client("get_point_center"); 72 | while (!client->wait_for_service(std::chrono::seconds(1))) 73 | { 74 | RCLCPP_WARN(this->get_logger(), "Waiting for get_point_center service..."); 75 | } 76 | 77 | auto request = std::make_shared(); 78 | request->x = x; 79 | request->y = y; 80 | 81 | auto future = client->async_send_request(request); 82 | 83 | try 84 | { 85 | auto response = future.get(); 86 | 87 | RCLCPP_INFO(this->get_logger(), 88 | "(%d, %d) position is: [%f, %f, %f]", 89 | x, y, response->point.centroid[0], response->point.centroid[1], response->point.centroid[2]); 90 | 91 | point_ = response->point.centroid; 92 | } 93 | catch (const std::exception &e) 94 | { 95 | RCLCPP_ERROR(this->get_logger(), "Service call failed."); 96 | } 97 | 98 | } 99 | 100 | }; 101 | 102 | int main(int argc, char **argv) 103 | { 104 | rclcpp::init(argc, argv); 105 | auto node = std::make_shared(); 106 | 107 | rclcpp::spin(node); 108 | rclcpp::shutdown(); 109 | return 0; 110 | } 111 | -------------------------------------------------------------------------------- /robovision_4/src/robovision_service.cpp: -------------------------------------------------------------------------------- 1 | #include "rclcpp/rclcpp.hpp" 2 | 3 | #include "geometry_msgs/msg/pose.hpp" 4 | #include "sensor_msgs/msg/image.hpp" 5 | #include "sensor_msgs/msg/point_cloud2.hpp" 6 | #include "sensor_msgs/msg/point_field.hpp" 7 | 8 | #include 9 | #include 10 | 11 | #include "robovision_interfaces/srv/get_point_center.hpp" 12 | #include "robovision_interfaces/msg/object_centroid.hpp" 13 | 14 | #include 15 | #include 16 | 17 | 18 | class PointCloudCentroidNode : public rclcpp::Node 19 | { 20 | public: 21 | PointCloudCentroidNode() : Node("point_cloud_service"), is_ptcld_(false), display_(true) 22 | { 23 | // Subscribers 24 | rgb_sub_ = this->create_subscription( 25 | "/camera/rgb/image_rect_color", 10, 26 | std::bind(&PointCloudCentroidNode::callback_rgb_rect, this, std::placeholders::_1)); 27 | 28 | depth_sub_ = this->create_subscription( 29 | "/camera/depth_registered/image", 10, 30 | std::bind(&PointCloudCentroidNode::callback_depth_rect, this, std::placeholders::_1)); 31 | 32 | point_cloud_sub_ = this->create_subscription( 33 | "/camera/depth_registered/points", 10, 34 | std::bind(&PointCloudCentroidNode::callback_point_cloud, this, std::placeholders::_1)); 35 | 36 | // Services 37 | get_point_center_service_ = this->create_service( 38 | "get_point_center", 39 | std::bind(&PointCloudCentroidNode::point_cloud_processing, this, 40 | std::placeholders::_1, std::placeholders::_2)); 41 | 42 | RCLCPP_INFO(this->get_logger(), "Starting point_cloud_service application in cpp..."); 43 | } 44 | 45 | private: 46 | 47 | bool is_ptcld_; 48 | bool display_; 49 | 50 | cv::Mat rgb_, depth_, depth_mat_, point_cloud_; 51 | 52 | geometry_msgs::msg::Pose centroid_; 53 | 54 | rclcpp::Subscription::SharedPtr rgb_sub_; 55 | rclcpp::Subscription::SharedPtr depth_sub_; 56 | rclcpp::Subscription::SharedPtr point_cloud_sub_; 57 | 58 | rclcpp::Service::SharedPtr get_point_center_service_; 59 | 60 | void callback_rgb_rect(const sensor_msgs::msg::Image::SharedPtr msg) 61 | { 62 | try 63 | { 64 | cv_bridge::CvImagePtr bridge_rgb = cv_bridge::toCvCopy(msg, msg->encoding); 65 | rgb_ = bridge_rgb->image.clone(); 66 | cv::cvtColor(rgb_, rgb_, cv::COLOR_RGB2BGR); 67 | } 68 | catch (const std::exception &e) 69 | { 70 | RCLCPP_ERROR(this->get_logger(), "Error processing RGB image: %s", e.what()); 71 | } 72 | } 73 | 74 | void callback_depth_rect(const sensor_msgs::msg::Image::SharedPtr msg) 75 | { 76 | try 77 | { 78 | cv_bridge::CvImagePtr bridge_depth = cv_bridge::toCvCopy(msg, "32FC1"); 79 | depth_ = bridge_depth->image.clone(); 80 | 81 | depth_mat_ = depth_.clone(); 82 | depth_.convertTo(depth_mat_, CV_32F); 83 | cv::normalize(depth_mat_, depth_, 0, 1, cv::NORM_MINMAX); 84 | } 85 | catch (const std::exception &e) 86 | { 87 | RCLCPP_ERROR(this->get_logger(), "Error processing Depth image: %s", e.what()); 88 | } 89 | } 90 | 91 | void callback_point_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg) 92 | { 93 | try 94 | { 95 | // Ensure the point cloud is organized 96 | if (msg->height > 1) 97 | { 98 | // Convert PointCloud2 to cv::Mat 99 | point_cloud_ = cv::Mat(msg->height, msg->width, CV_32FC4); 100 | const uint8_t *data_ptr = msg->data.data(); 101 | 102 | for (size_t i = 0; i < msg->height; ++i) 103 | { 104 | for (size_t j = 0; j < msg->width; ++j) 105 | { 106 | const float *point = reinterpret_cast(data_ptr + (i * msg->row_step + j * msg->point_step)); 107 | point_cloud_.at(i, j) = cv::Vec4f(point[0], point[1], point[2], point[3]); 108 | } 109 | } 110 | 111 | is_ptcld_ = true; 112 | RCLCPP_INFO(this->get_logger(), "Point cloud received with size: %d x %d", msg->height, msg->width); 113 | } 114 | else 115 | { 116 | RCLCPP_WARN(this->get_logger(), "Point cloud is not organized. Cannot convert to cv::Mat."); 117 | } 118 | } 119 | catch (const std::exception &e) 120 | { 121 | RCLCPP_ERROR(this->get_logger(), "Error processing PointCloud: %s", e.what()); 122 | } 123 | } 124 | 125 | void point_cloud_processing( 126 | const std::shared_ptr request, 127 | std::shared_ptr response) 128 | { 129 | response->point.x = 0.0; 130 | response->point.y = 0.0; 131 | response->point.z = 0.0; 132 | response->point.centroid = {0.0, 0.0, 0.0}; 133 | 134 | if (!is_ptcld_) 135 | { 136 | RCLCPP_ERROR(this->get_logger(), "No PointCloud data available for processing."); 137 | return; 138 | } 139 | 140 | try 141 | { 142 | int row_id = request->y; 143 | int col_id = request->x; 144 | 145 | if (row_id >= 0 && row_id < point_cloud_.rows && col_id >= 0 && col_id < point_cloud_.cols) 146 | { 147 | cv::Vec4f point = point_cloud_.at(row_id, col_id); 148 | 149 | response->point.x = point[0]; 150 | response->point.y = point[1]; 151 | response->point.z = point[2]; 152 | response->point.centroid = {point[0], point[1], point[2]}; 153 | 154 | RCLCPP_INFO(this->get_logger(), "Centroid at (%d, %d): [%f, %f, %f]", 155 | row_id, col_id, point[0], point[1], point[2]); 156 | } 157 | else 158 | { 159 | RCLCPP_ERROR(this->get_logger(), "Requested coordinates are out of bounds."); 160 | } 161 | } 162 | catch (const std::exception &e) 163 | { 164 | RCLCPP_ERROR(this->get_logger(), "Error in processing PointCloud: %s", e.what()); 165 | } 166 | } 167 | }; 168 | 169 | int main(int argc, char **argv) 170 | { 171 | rclcpp::init(argc, argv); 172 | auto node = std::make_shared(); 173 | rclcpp::spin(node); 174 | rclcpp::shutdown(); 175 | return 0; 176 | } 177 | -------------------------------------------------------------------------------- /robovision_interfaces/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(robovision_interfaces) 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 dependencies 14 | find_package(ament_cmake REQUIRED) 15 | find_package(rosidl_default_generators REQUIRED) 16 | 17 | set(msg_files 18 | "msg/ObjectCentroid.msg" 19 | "msg/ObjectCentroids.msg" 20 | ) 21 | 22 | set(srv_files 23 | "srv/GetPointCenter.srv" 24 | ) 25 | 26 | rosidl_generate_interfaces(${PROJECT_NAME} 27 | ${msg_files} 28 | ${srv_files} 29 | ) 30 | 31 | ament_export_dependencies(rosidl_default_runtime) 32 | 33 | ament_package() 34 | -------------------------------------------------------------------------------- /robovision_interfaces/msg/ObjectCentroid.msg: -------------------------------------------------------------------------------- 1 | float64 x 2 | float64 y 3 | float64 z 4 | float64[] centroid 5 | -------------------------------------------------------------------------------- /robovision_interfaces/msg/ObjectCentroids.msg: -------------------------------------------------------------------------------- 1 | #Note that if the message is defined in the same package, 2 | #the package name does not appear in the service or message definition. 3 | #If the message is defined elsewhere, we have to specify it, e.g. 4 | #robovision_interfaces/msg/ObjectCentroid[] centroids 5 | 6 | ObjectCentroid[] centroids 7 | -------------------------------------------------------------------------------- /robovision_interfaces/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | robovision_interfaces 5 | 0.0.0 6 | TODO: Package description 7 | roboworks 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | rosidl_default_generators 13 | rosidl_default_runtime 14 | rosidl_interface_packages 15 | 16 | ament_lint_auto 17 | ament_lint_common 18 | 19 | 20 | ament_cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /robovision_interfaces/srv/GetPointCenter.srv: -------------------------------------------------------------------------------- 1 | int64 x 2 | int64 y 3 | --- 4 | #Note that if the message is defined in the same package, 5 | #the package name does not appear in the service or message definition. 6 | #If the message is defined elsewhere, we have to specify it, e.g. 7 | #robovision_interfaces/msg/ObjectCentroid point 8 | 9 | ObjectCentroid point 10 | --------------------------------------------------------------------------------