├── data
├── rosbags
│ └── data
└── images
│ ├── baboon.png
│ └── mariachi.jpg
├── robovision_1
├── robovision_images
│ └── __init__.py
├── package.xml
├── CMakeLists.txt
├── scripts
│ └── my_subscriber.py
├── src
│ ├── my_subscriber.cpp
│ ├── my_publisher.cpp
│ └── my_camera_publisher.cpp
└── README.md
├── robovision_3
├── robovision_rgbd
│ └── __init__.py
├── images
│ ├── xy_view.jpg
│ ├── xz_view.jpg
│ ├── yz_view.jpg
│ ├── rgbd_view.jpg
│ ├── scene_box.jpg
│ ├── scene_crop.jpg
│ ├── scene_full.jpg
│ ├── point_depth.jpg
│ └── scene_person.jpg
├── package.xml
├── CMakeLists.txt
├── scripts
│ └── rgbd_reader.py
├── src
│ └── rgbd_reader.cpp
└── README.md
├── robovision_2
├── robovision_processing
│ └── __init__.py
├── images
│ ├── baboon_gray.jpg
│ ├── baboon_mask_red.jpg
│ ├── baboon_number.jpg
│ ├── baboon_rotated.jpg
│ ├── digital_image.jpg
│ └── baboon_mask_blue.jpg
├── package.xml
├── CMakeLists.txt
├── scripts
│ └── my_processing.py
├── src
│ └── my_processing.cpp
└── README.md
├── robovision_4
├── robovision_services
│ └── __init__.py
├── package.xml
├── CMakeLists.txt
├── scripts
│ ├── robovision_client.py
│ └── robovision_service.py
├── src
│ ├── robovision_client.cpp
│ └── robovision_service.cpp
└── README.md
├── robovision_interfaces
├── msg
│ ├── ObjectCentroid.msg
│ └── ObjectCentroids.msg
├── srv
│ └── GetPointCenter.srv
├── CMakeLists.txt
└── package.xml
└── README.md
/data/rosbags/data:
--------------------------------------------------------------------------------
1 |
--------------------------------------------------------------------------------
/robovision_1/robovision_images/__init__.py:
--------------------------------------------------------------------------------
1 |
--------------------------------------------------------------------------------
/robovision_3/robovision_rgbd/__init__.py:
--------------------------------------------------------------------------------
1 |
--------------------------------------------------------------------------------
/robovision_2/robovision_processing/__init__.py:
--------------------------------------------------------------------------------
1 |
--------------------------------------------------------------------------------
/robovision_4/robovision_services/__init__.py:
--------------------------------------------------------------------------------
1 |
--------------------------------------------------------------------------------
/data/images/baboon.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ARTenshi/robovision_ros2/HEAD/data/images/baboon.png
--------------------------------------------------------------------------------
/data/images/mariachi.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ARTenshi/robovision_ros2/HEAD/data/images/mariachi.jpg
--------------------------------------------------------------------------------
/robovision_interfaces/msg/ObjectCentroid.msg:
--------------------------------------------------------------------------------
1 | float64 x
2 | float64 y
3 | float64 z
4 | float64[] centroid
5 |
--------------------------------------------------------------------------------
/robovision_3/images/xy_view.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ARTenshi/robovision_ros2/HEAD/robovision_3/images/xy_view.jpg
--------------------------------------------------------------------------------
/robovision_3/images/xz_view.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ARTenshi/robovision_ros2/HEAD/robovision_3/images/xz_view.jpg
--------------------------------------------------------------------------------
/robovision_3/images/yz_view.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ARTenshi/robovision_ros2/HEAD/robovision_3/images/yz_view.jpg
--------------------------------------------------------------------------------
/robovision_3/images/rgbd_view.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ARTenshi/robovision_ros2/HEAD/robovision_3/images/rgbd_view.jpg
--------------------------------------------------------------------------------
/robovision_3/images/scene_box.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ARTenshi/robovision_ros2/HEAD/robovision_3/images/scene_box.jpg
--------------------------------------------------------------------------------
/robovision_3/images/scene_crop.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ARTenshi/robovision_ros2/HEAD/robovision_3/images/scene_crop.jpg
--------------------------------------------------------------------------------
/robovision_3/images/scene_full.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ARTenshi/robovision_ros2/HEAD/robovision_3/images/scene_full.jpg
--------------------------------------------------------------------------------
/robovision_2/images/baboon_gray.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ARTenshi/robovision_ros2/HEAD/robovision_2/images/baboon_gray.jpg
--------------------------------------------------------------------------------
/robovision_3/images/point_depth.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ARTenshi/robovision_ros2/HEAD/robovision_3/images/point_depth.jpg
--------------------------------------------------------------------------------
/robovision_3/images/scene_person.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ARTenshi/robovision_ros2/HEAD/robovision_3/images/scene_person.jpg
--------------------------------------------------------------------------------
/robovision_2/images/baboon_mask_red.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ARTenshi/robovision_ros2/HEAD/robovision_2/images/baboon_mask_red.jpg
--------------------------------------------------------------------------------
/robovision_2/images/baboon_number.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ARTenshi/robovision_ros2/HEAD/robovision_2/images/baboon_number.jpg
--------------------------------------------------------------------------------
/robovision_2/images/baboon_rotated.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ARTenshi/robovision_ros2/HEAD/robovision_2/images/baboon_rotated.jpg
--------------------------------------------------------------------------------
/robovision_2/images/digital_image.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ARTenshi/robovision_ros2/HEAD/robovision_2/images/digital_image.jpg
--------------------------------------------------------------------------------
/robovision_2/images/baboon_mask_blue.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ARTenshi/robovision_ros2/HEAD/robovision_2/images/baboon_mask_blue.jpg
--------------------------------------------------------------------------------
/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/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 |
--------------------------------------------------------------------------------
/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/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_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_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_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_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_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_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_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/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_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_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_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_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_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_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/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 |
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/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_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_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_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/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_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_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 | +(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 | +(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_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 |
--------------------------------------------------------------------------------