Difference between revisions of "Object Detection Subscriber for Foxy alsora ros2-tensorflow 20220127"
(Created page with "= References = Start with * Install Turtlebot3 Hood Waffle Foxy alsora ros2-tensorflow 20210615 * Install_Turtlebot3_Hood_Waffle_Foxy_Tensorflow_20210615#Object_Detecti...") |
(→Referencres) |
||
(75 intermediate revisions by the same user not shown) | |||
Line 3: | Line 3: | ||
Start with | Start with | ||
* [[Install Turtlebot3 Hood Waffle Foxy alsora ros2-tensorflow 20210615]] | * [[Install Turtlebot3 Hood Waffle Foxy alsora ros2-tensorflow 20210615]] | ||
− | * [[ | + | * [[Install Turtlebot3 Hood Waffle Foxy alsora ros2-tensorflow 20210615#Object_Detection_Task_2]] |
= Alsora ros2-tensorflow Object Detection Task 2 = | = Alsora ros2-tensorflow Object Detection Task 2 = | ||
+ | |||
+ | Plug in webcam and do the follow source in each terminal | ||
+ | |||
+ | '''TERMINAL 1''' | ||
+ | |||
+ | * Subscribe to the image topic | ||
+ | * Find objects in image and publish detections and detections_image topics. | ||
+ | |||
+ | $ source ~/tf_ws/install/local_setup.sh | ||
+ | |||
+ | $ ros2 run tf_detection_py server | ||
+ | |||
+ | '''TERMINAL 2''' | ||
+ | |||
+ | Publish the webcam image as a topic | ||
+ | |||
+ | $ ros2 run image_tools cam2image --ros-args -p frequency:=2.0 | ||
+ | |||
+ | '''TERMINAL 3''' | ||
+ | |||
+ | $ ros2 topic list | ||
+ | /clicked_point | ||
+ | /detections | ||
+ | /detections_image | ||
+ | /flip_image | ||
+ | /goal_pose | ||
+ | /image | ||
+ | /initialpose | ||
+ | /parameter_events | ||
+ | /rosout | ||
+ | /tf | ||
+ | /tf_static | ||
+ | /vision_info | ||
+ | |||
+ | * '''Note:''' the "image" topic is the raw output of the webcam. | ||
+ | * '''Note:''' the "detections_image" contains the "image" topic data with the addition of detection bounding boxes and labels. | ||
+ | * '''Note:''' this page will focus on capturing and exploring "detections" data | ||
+ | * Display the detections_image topic. See display below. | ||
+ | |||
+ | $ ros2 run image_tools showimage image:=detections_image | ||
+ | |||
+ | [[File: Image_detect_20210828.png | 400px]] | ||
+ | |||
+ | = Workspace = | ||
+ | |||
+ | /home/eepp/eepp_ws/src/eepp_tf_detections/src | ||
+ | |||
+ | = Create a Detections Subscriber - Proof of Concept = | ||
+ | |||
+ | == References == | ||
+ | |||
+ | * [https://github.com/ros-perception/vision_msgs vision_msgs] | ||
+ | |||
+ | == Subscriber Callback == | ||
+ | |||
+ | Start with the object oriented image subscriber I modified from a subscriber/publisher tutorial. | ||
+ | |||
+ | * [[image_subscriber_experiment_oop.cpp 20210414]] | ||
+ | * Source location is <tt>~/eepp_ws/src/eepp_tf_detections/src</tt> | ||
+ | * Copy it to detections_list.cpp and make the following modifications | ||
+ | |||
+ | Include detection array message header. | ||
+ | |||
+ | #include "vision_msgs/msg/detection2_d_array.hpp" | ||
+ | |||
+ | Add detection callback function. This is a proof of concept version. The detections object is still being explored. | ||
+ | |||
+ | //---------------------------------------------------------------------------------- | ||
+ | // detection callback function | ||
+ | void detections_callback(const vision_msgs::msg::Detection2DArray::SharedPtr detections) const | ||
+ | { | ||
+ | RCLCPP_INFO(this->get_logger(), "----"); | ||
+ | |||
+ | for (int i=0; i<5; i++) | ||
+ | { | ||
+ | //RCLCPP_INFO(this->get_logger(), "--- Detection "); | ||
+ | RCLCPP_INFO(this->get_logger(), " time stamp seconds %i", detections->header.stamp.sec); | ||
+ | RCLCPP_INFO(this->get_logger(), " id %i %i", i, detections->detections[i].results[0].id[0]); | ||
+ | RCLCPP_INFO(this->get_logger(), " score 0 %f", detections->detections[i].results[0].score); | ||
+ | } | ||
+ | } | ||
+ | |||
+ | Create the detections subscriber. | ||
+ | |||
+ | // Subscriber instance variable | ||
+ | detections_subscription_ = this->create_subscription<vision_msgs::msg::Detection2DArray>( | ||
+ | "detections", 10, std::bind(&MinimalImageSubscriber::detections_callback, this, _1)); | ||
+ | |||
+ | rclcpp::Subscription<vision_msgs::msg::Detection2DArray>::SharedPtr detections_subscription_; | ||
+ | |||
+ | == Subscriber Embedded in Program == | ||
+ | |||
+ | * '''TBD:''' Proof of concept code: Update documentation and clean up code. | ||
+ | * '''Note:''' small errors can result in voluminous meaningless errors that are impossible to match to an error. In particular the message paths and must match exactly. If they don't the complier gets lost in a string of package names. | ||
+ | ** For example: I missed changing one of the message pointers from image to detections: vision_msgs::msg::Detection2DArray. Toke an hour or two to see the typo. | ||
+ | |||
+ | * [[detections_list.cpp 20220127]] | ||
+ | |||
+ | == Build Specifications == | ||
+ | |||
+ | $ diff CMakeLists.txt.org CMakeLists.txt | ||
+ | 20,22c20,30 | ||
+ | < # uncomment the following section in order to fill in | ||
+ | < # further dependencies manually. | ||
+ | < # find_package(<dependency> REQUIRED) | ||
+ | --- | ||
+ | > find_package(rclcpp REQUIRED) | ||
+ | > find_package(sensor_msgs REQUIRED) | ||
+ | > find_package(vision_msgs REQUIRED) | ||
+ | > find_package(std_msgs REQUIRED) | ||
+ | > | ||
+ | > add_executable(detections_list src/detections_list.cpp) | ||
+ | > ament_target_dependencies(detections_list rclcpp std_msgs sensor_msgs vision_msgs) | ||
+ | > | ||
+ | > install(TARGETS | ||
+ | > detections_list | ||
+ | > DESTINATION lib/${PROJECT_NAME}) | ||
+ | |||
+ | == Resource locations == | ||
+ | |||
+ | sensor_mags include image messages | ||
+ | |||
+ | ls /opt/ros/foxy/share/sensor_msgs/msg/Image.msg | ||
+ | ls /opt/ros/foxy/include/sensor_msgs/msg/image.hpp | ||
+ | |||
+ | vision_msgs include object detection message such as Detection2DArray. | ||
+ | |||
+ | ls /opt/ros/foxy/share/vision_msgs/msg/Detection2D.msg | ||
+ | ls /opt/ros/foxy/include/vision_msgs/msg/detection2_d_array.hpp | ||
+ | ls /opt/ros/foxy/include/vision_msgs/msg/detail/detection2_d_array__struct.hpp | ||
+ | |||
+ | It was helpful to match | ||
+ | |||
+ | $ cat /opt/ros/foxy/include/vision_msgs/msg/detail/detection2_d_array__struct.hpp | ||
+ | |||
+ | $ ros2 topic echo detections | ||
+ | |||
+ | == Build == | ||
+ | |||
+ | $ cd ~/eepp_ws | ||
+ | |||
+ | $ colcon build --symlink-install --parallel-workers 1 | ||
+ | Starting >>> eepp_tf_detections | ||
+ | Finished <<< eepp_tf_detections [7.16s] | ||
+ | ... | ||
+ | ... | ||
+ | Summary: 4 packages finished [8.36s] | ||
+ | |||
+ | == Execute == | ||
+ | |||
+ | '''TERMINAL 4''' | ||
+ | |||
+ | $ source ~/eepp_ws/install/setup.bash | ||
+ | |||
+ | $ ros2 run eepp_tf_detections detections_list | ||
+ | ---- | ||
+ | time stamp seconds 1643496780 | ||
+ | id 0 49 | ||
+ | score 0 0.857445 | ||
+ | time stamp seconds 1643496780 | ||
+ | id 1 56 | ||
+ | score 0 0.637981 | ||
+ | time stamp seconds 1643496780 | ||
+ | id 2 56 | ||
+ | score 0 0.616743 | ||
+ | time stamp seconds 1643496780 | ||
+ | id 3 56 | ||
+ | score 0 0.579421 | ||
+ | time stamp seconds 1643496780 | ||
+ | id 4 55 | ||
+ | score 0 0.513886 | ||
+ | |||
+ | We found 5 objects belonging to classes 49, 55 and 56. Probabilities were 86, 64, 62, 58 and 51 percent. | ||
+ | |||
+ | '''TBD:''' Clean up the output give more useful information, list any number of objects, print object labels, etc | ||
+ | |||
+ | == TBD: cleanup - Other Potential Resources == | ||
+ | |||
+ | === ROS Org Answers === | ||
+ | |||
+ | * [https://answers.ros.org/question/395696/how-do-i-parse-a-detection2darray-message/ How do I parse a Detection2DArray message] | ||
+ | * [https://gist.github.com/alexsleat/1372845/7e39518cfa12ac91aca4378843e55862eb9ed41d#file-subscribe-cpp-L35 | ||
+ | alexsleat/Publish.cpp] | ||
+ | |||
+ | === visualizer === | ||
+ | See note detections_msg20220122.txt for a visualizer. | ||
+ | |||
+ | |||
+ | === documention === | ||
+ | |||
+ | in tf_detections20220121.txt | ||
+ | |||
+ | <pre> | ||
+ | ---- | ||
+ | expand detector listener | ||
+ | |||
+ | interesting | ||
+ | https://jeffzzq.medium.com/ros2-image-pipeline-tutorial-3b18903e7329 | ||
+ | |||
+ | |||
+ | |||
+ | https://docs.ros2.org/foxy/api/ | ||
+ | https://docs.ros2.org/foxy/api/sensor_msgs/msg/ | ||
+ | |||
+ | https://docs.ros2.org/foxy/api/sensor_msgs/msg/Image.html | ||
+ | std_msgs/msg/Header header | ||
+ | uint32 height | ||
+ | uint32 width | ||
+ | string encoding | ||
+ | uint8 is_bigendian | ||
+ | uint32 step | ||
+ | uint8[] data | ||
+ | </pre> | ||
+ | |||
+ | = Refine Detections Subscriber - Parse Detections Message = | ||
+ | == Subscriber Callback == | ||
+ | |||
+ | <pre> | ||
+ | void detections_callback(const vision_msgs::msg::Detection2DArray::SharedPtr detections2dArray) const | ||
+ | { | ||
+ | int isDone = false; | ||
+ | int detectionsCount = 0; | ||
+ | |||
+ | // TBD: get this from the image listener | ||
+ | int IMAGE_WIDTH = 320; | ||
+ | int TOLLERANCE = 10; | ||
+ | |||
+ | RCLCPP_INFO(this->get_logger(), "---- "); | ||
+ | RCLCPP_INFO(this->get_logger(), "Object detection message"); | ||
+ | |||
+ | try | ||
+ | { | ||
+ | while (not isDone) | ||
+ | { | ||
+ | // Extract a single object detection | ||
+ | vision_msgs::msg::Detection2D aDetection = detections2dArray->detections[detectionsCount++]; | ||
+ | RCLCPP_INFO(this->get_logger(), " "); | ||
+ | |||
+ | // Extract the object's id and confidence | ||
+ | vision_msgs::msg::ObjectHypothesisWithPose result = aDetection.results[0]; | ||
+ | int64_t id = stoi(result.id); | ||
+ | float score = result.score; | ||
+ | RCLCPP_INFO(this->get_logger(), " id %i", id); | ||
+ | RCLCPP_INFO(this->get_logger(), " score %f", score); | ||
+ | |||
+ | // Asspocate a few object id's with their string name. | ||
+ | // TBD: pull this information from COCO labels | ||
+ | // https://gist.github.com/aallan/fbdf008cffd1e08a619ad11a02b74fa8 | ||
+ | //int64_t CAR = 3; | ||
+ | //int64_t SPORTS_BALL = 37; | ||
+ | int64_t BOTTLE = 44; | ||
+ | |||
+ | // Bounding boxes | ||
+ | vision_msgs::msg::BoundingBox2D boundingBox2d = aDetection.bbox; | ||
+ | geometry_msgs::msg::Pose2D center = boundingBox2d.center; | ||
+ | RCLCPP_INFO(this->get_logger(), " position (%f, %f)", center.x, center.y); | ||
+ | RCLCPP_INFO(this->get_logger(), " size %f x %f", boundingBox2d.size_x, boundingBox2d.size_y); | ||
+ | |||
+ | // The rest of the code in in the xxx list below | ||
+ | // If it found a BOTTLE it build a Twist message | ||
+ | // Sets the z rotation to stop, turn left or right | ||
+ | // Publishes the Twist Message | ||
+ | ... | ||
+ | ... | ||
+ | } | ||
+ | |||
+ | // Exception handling for array out of bounds behavior is undefined. The behavior in | ||
+ | // this example may not be the same in other C++ environments. Behavior was susceptible to | ||
+ | // https://stackoverflow.com/questions/1239938/accessing-an-array-out-of-bounds-gives-no-error-why | ||
+ | // Behavior is consistent enough in my environment to explore message parsing. | ||
+ | catch (exception& e) | ||
+ | { | ||
+ | RCLCPP_INFO(this->get_logger(), "Exception %s", e.what()); | ||
+ | isDone = true; | ||
+ | } | ||
+ | } | ||
+ | </pre> | ||
+ | |||
+ | == Subscriber Embedded in Program == | ||
+ | |||
+ | * '''Note:''' small errors can result in voluminous meaningless errors that are impossible to match to an error. In particular the message paths and must match exactly. If they don't the complier gets lost in a string of package names. | ||
+ | ** For example: I missed changing one of the message pointers from image to detections: vision_msgs::msg::Detection2DArray. Toke an hour or two to see the typo. | ||
+ | |||
+ | * [[detections_list.cpp 20220219]] | ||
+ | |||
+ | |||
+ | == Subscriber Input == | ||
+ | |||
+ | * This is the webcam image capture by the /cam2image node and published as an "/image message (topic)." The /dectection_server subscribes to the "/image" message and publishes a "/dectections" and "/dectections_image" message which looks like this: | ||
+ | * [[File: 12ProofOfConcept.png | 500px]] | ||
+ | |||
+ | * '''Notice''': | ||
+ | * Object ids are defined in [https://gist.github.com/aallan/fbdf008cffd1e08a619ad11a02b74fa8 labels.txt] | ||
+ | ** int64_t CAR = 3; | ||
+ | ** int64_t SPORTS_BALL = 37; | ||
+ | ** int64_t BOTTLE = 44; | ||
+ | |||
+ | * As stated above the "/detections" message is created by the /detection_server node. The /image_Subscriber_OOP_Test node subscribes to it. This is the code we are writing. | ||
+ | * See rqt_graph below. | ||
+ | * [[File: FullDetectionsAndTwish20220222.png | 1000px]] | ||
+ | |||
+ | * The "/detections" message is define in | ||
+ | ** [https://github.com/ros-perception/vision_msgs vision_messages] | ||
+ | * It is of the following form. | ||
+ | |||
+ | Detection2DArray | ||
+ | Header header | ||
+ | Detection2D[] detections | ||
+ | Header header | ||
+ | ObjectHypothesisWithPose[] results | ||
+ | int64 id | ||
+ | float64 score | ||
+ | PoseWithCovariance pose | ||
+ | BoundingVox2D bbox | ||
+ | Pose2D center | ||
+ | float64 size_x | ||
+ | float64 size_y | ||
+ | Image source_image | ||
+ | |||
+ | <pre> | ||
+ | |||
+ | * This is an example of a "\detections" message based on the above detections_image | ||
+ | |||
+ | $ ros2 topic echo detections | ||
+ | --- | ||
+ | header: | ||
+ | stamp: | ||
+ | sec: 1644692469 | ||
+ | nanosec: 503464021 | ||
+ | frame_id: '' | ||
+ | detections: | ||
+ | - header: | ||
+ | stamp: | ||
+ | sec: 1644692469 | ||
+ | nanosec: 503464021 | ||
+ | frame_id: '' | ||
+ | results: | ||
+ | - id: '3' | ||
+ | score: 0.9104859828948975 | ||
+ | pose: | ||
+ | pose: | ||
+ | position: | ||
+ | x: 0.0 | ||
+ | y: 0.0 | ||
+ | z: 0.0 | ||
+ | orientation: | ||
+ | x: 0.0 | ||
+ | y: 0.0 | ||
+ | z: 0.0 | ||
+ | w: 1.0 | ||
+ | covariance: | ||
+ | - 0.0 | ||
+ | - 0.0 | ||
+ | - 0.0 | ||
+ | - 0.0 | ||
+ | - '...' repeats '0.0' for 30 or so times | ||
+ | bbox: | ||
+ | center: | ||
+ | x: 161.57681465148926 | ||
+ | y: 244.36246871948242 | ||
+ | theta: 0.0 | ||
+ | size_x: 104.41673278808594 | ||
+ | size_y: 54.783639907836914 | ||
+ | source_img: | ||
+ | header: | ||
+ | stamp: | ||
+ | sec: 0 | ||
+ | nanosec: 0 | ||
+ | frame_id: map | ||
+ | height: 55 | ||
+ | width: 104 | ||
+ | encoding: bgr8 | ||
+ | is_bigendian: 0 | ||
+ | step: 312 | ||
+ | data: | ||
+ | - 209 | ||
+ | - 174 | ||
+ | - 148 | ||
+ | - 209 | ||
+ | - 173 | ||
+ | - '...' repeats one byte integers for a long time | ||
+ | is_tracking: false | ||
+ | tracking_id: '' | ||
+ | - header: | ||
+ | stamp: | ||
+ | sec: 1644692469 | ||
+ | nanosec: 503464021 | ||
+ | frame_id: '' | ||
+ | results: | ||
+ | - id: '44' | ||
+ | score: 0.8835442066192627 | ||
+ | pose: | ||
+ | pose: | ||
+ | position: | ||
+ | x: 0.0 | ||
+ | y: 0.0 | ||
+ | z: 0.0 | ||
+ | orientation: | ||
+ | x: 0.0 | ||
+ | y: 0.0 | ||
+ | z: 0.0 | ||
+ | w: 1.0 | ||
+ | covariance: | ||
+ | - 0.0 | ||
+ | - 0.0 | ||
+ | - 0.0 | ||
+ | - 0.0 | ||
+ | |||
+ | </pre> | ||
+ | |||
+ | * [[detectionList20220211.txt]] | ||
+ | |||
+ | * '''TBD''': Rename /image_Subscriber_OOP_Test to something meaningful. | ||
+ | |||
+ | == Logger Output == | ||
+ | |||
+ | <pre> | ||
+ | |||
+ | [INFO] [1644692166.415408150] [Image_Subscriber_OOP_Test]: Detection image message: encoding bgr8 | ||
+ | [INFO] [1644692166.415550033] [Image_Subscriber_OOP_Test]: size 320 x 240 | ||
+ | [INFO] [1644692166.511600888] [Image_Subscriber_OOP_Test]: ---- | ||
+ | [INFO] [1644692166.511658790] [Image_Subscriber_OOP_Test]: Object detection message | ||
+ | [INFO] [1644692166.511695117] [Image_Subscriber_OOP_Test]: | ||
+ | [INFO] [1644692166.511729759] [Image_Subscriber_OOP_Test]: id 3 | ||
+ | [INFO] [1644692166.511752299] [Image_Subscriber_OOP_Test]: score 0.920012 | ||
+ | [INFO] [1644692166.511815646] [Image_Subscriber_OOP_Test]: position (161.534085, 244.812870) | ||
+ | [INFO] [1644692166.511845515] [Image_Subscriber_OOP_Test]: size 104.727211 x 55.202351 | ||
+ | [INFO] [1644692166.511886772] [Image_Subscriber_OOP_Test]: | ||
+ | [INFO] [1644692166.511908266] [Image_Subscriber_OOP_Test]: id 44 | ||
+ | [INFO] [1644692166.511927532] [Image_Subscriber_OOP_Test]: score 0.888334 | ||
+ | [INFO] [1644692166.511949070] [Image_Subscriber_OOP_Test]: position (32.790173, 152.766781) | ||
+ | [INFO] [1644692166.511977648] [Image_Subscriber_OOP_Test]: size 63.921785 x 177.125902 | ||
+ | [INFO] [1644692166.512038127] [Image_Subscriber_OOP_Test]: Exception basic_string::_M_create | ||
+ | [INFO] [1644692166.915096688] [Image_Subscriber_OOP_Test]: ---- | ||
+ | [INFO] [1644692166.915242760] [Image_Subscriber_OOP_Test]: Detection image message: encoding bgr8 | ||
+ | [INFO] [1644692166.915314105] [Image_Subscriber_OOP_Test]: size 320 x 240 | ||
+ | [INFO] [1644692167.016898240] [Image_Subscriber_OOP_Test]: ---- | ||
+ | [INFO] [1644692167.016990839] [Image_Subscriber_OOP_Test]: Object detection message | ||
+ | [INFO] [1644692167.017032332] [Image_Subscriber_OOP_Test]: | ||
+ | [INFO] [1644692167.017085759] [Image_Subscriber_OOP_Test]: id 3 | ||
+ | [INFO] [1644692167.017111215] [Image_Subscriber_OOP_Test]: score 0.911544 | ||
+ | [INFO] [1644692167.017147306] [Image_Subscriber_OOP_Test]: position (161.732769, 244.791164) | ||
+ | [INFO] [1644692167.017184411] [Image_Subscriber_OOP_Test]: size 104.508286 x 55.179605 | ||
+ | [INFO] [1644692167.017220862] [Image_Subscriber_OOP_Test]: | ||
+ | [INFO] [1644692167.017251966] [Image_Subscriber_OOP_Test]: id 44 | ||
+ | [INFO] [1644692167.017269509] [Image_Subscriber_OOP_Test]: score 0.892449 | ||
+ | [INFO] [1644692167.017294431] [Image_Subscriber_OOP_Test]: position (32.734455, 152.264280) | ||
+ | [INFO] [1644692167.017320379] [Image_Subscriber_OOP_Test]: size 63.703346 x 175.604768 | ||
+ | [INFO] [1644692167.017380784] [Image_Subscriber_OOP_Test]: Exception basic_string::_M_create | ||
+ | ^C[INFO] [1644692167.049173827] [rclcpp]: signal_handler(signal_value=2) | ||
+ | |||
+ | </pre> |
Latest revision as of 11:10, 31 May 2022
Contents
References
Start with
- Install Turtlebot3 Hood Waffle Foxy alsora ros2-tensorflow 20210615
- Install Turtlebot3 Hood Waffle Foxy alsora ros2-tensorflow 20210615#Object_Detection_Task_2
Alsora ros2-tensorflow Object Detection Task 2
Plug in webcam and do the follow source in each terminal
TERMINAL 1
- Subscribe to the image topic
- Find objects in image and publish detections and detections_image topics.
$ source ~/tf_ws/install/local_setup.sh $ ros2 run tf_detection_py server
TERMINAL 2
Publish the webcam image as a topic
$ ros2 run image_tools cam2image --ros-args -p frequency:=2.0
TERMINAL 3
$ ros2 topic list /clicked_point /detections /detections_image /flip_image /goal_pose /image /initialpose /parameter_events /rosout /tf /tf_static /vision_info
- Note: the "image" topic is the raw output of the webcam.
- Note: the "detections_image" contains the "image" topic data with the addition of detection bounding boxes and labels.
- Note: this page will focus on capturing and exploring "detections" data
- Display the detections_image topic. See display below.
$ ros2 run image_tools showimage image:=detections_image
Workspace
/home/eepp/eepp_ws/src/eepp_tf_detections/src
Create a Detections Subscriber - Proof of Concept
References
Subscriber Callback
Start with the object oriented image subscriber I modified from a subscriber/publisher tutorial.
- image_subscriber_experiment_oop.cpp 20210414
- Source location is ~/eepp_ws/src/eepp_tf_detections/src
- Copy it to detections_list.cpp and make the following modifications
Include detection array message header.
#include "vision_msgs/msg/detection2_d_array.hpp"
Add detection callback function. This is a proof of concept version. The detections object is still being explored.
//---------------------------------------------------------------------------------- // detection callback function void detections_callback(const vision_msgs::msg::Detection2DArray::SharedPtr detections) const { RCLCPP_INFO(this->get_logger(), "----"); for (int i=0; i<5; i++) { //RCLCPP_INFO(this->get_logger(), "--- Detection "); RCLCPP_INFO(this->get_logger(), " time stamp seconds %i", detections->header.stamp.sec); RCLCPP_INFO(this->get_logger(), " id %i %i", i, detections->detections[i].results[0].id[0]); RCLCPP_INFO(this->get_logger(), " score 0 %f", detections->detections[i].results[0].score); } }
Create the detections subscriber.
// Subscriber instance variable detections_subscription_ = this->create_subscription<vision_msgs::msg::Detection2DArray>( "detections", 10, std::bind(&MinimalImageSubscriber::detections_callback, this, _1)); rclcpp::Subscription<vision_msgs::msg::Detection2DArray>::SharedPtr detections_subscription_;
Subscriber Embedded in Program
- TBD: Proof of concept code: Update documentation and clean up code.
- Note: small errors can result in voluminous meaningless errors that are impossible to match to an error. In particular the message paths and must match exactly. If they don't the complier gets lost in a string of package names.
- For example: I missed changing one of the message pointers from image to detections: vision_msgs::msg::Detection2DArray. Toke an hour or two to see the typo.
Build Specifications
$ diff CMakeLists.txt.org CMakeLists.txt 20,22c20,30 < # uncomment the following section in order to fill in < # further dependencies manually. < # find_package(<dependency> REQUIRED) --- > find_package(rclcpp REQUIRED) > find_package(sensor_msgs REQUIRED) > find_package(vision_msgs REQUIRED) > find_package(std_msgs REQUIRED) > > add_executable(detections_list src/detections_list.cpp) > ament_target_dependencies(detections_list rclcpp std_msgs sensor_msgs vision_msgs) > > install(TARGETS > detections_list > DESTINATION lib/${PROJECT_NAME})
Resource locations
sensor_mags include image messages
ls /opt/ros/foxy/share/sensor_msgs/msg/Image.msg ls /opt/ros/foxy/include/sensor_msgs/msg/image.hpp
vision_msgs include object detection message such as Detection2DArray.
ls /opt/ros/foxy/share/vision_msgs/msg/Detection2D.msg ls /opt/ros/foxy/include/vision_msgs/msg/detection2_d_array.hpp ls /opt/ros/foxy/include/vision_msgs/msg/detail/detection2_d_array__struct.hpp
It was helpful to match
$ cat /opt/ros/foxy/include/vision_msgs/msg/detail/detection2_d_array__struct.hpp $ ros2 topic echo detections
Build
$ cd ~/eepp_ws $ colcon build --symlink-install --parallel-workers 1 Starting >>> eepp_tf_detections Finished <<< eepp_tf_detections [7.16s] ... ... Summary: 4 packages finished [8.36s]
Execute
TERMINAL 4
$ source ~/eepp_ws/install/setup.bash $ ros2 run eepp_tf_detections detections_list ---- time stamp seconds 1643496780 id 0 49 score 0 0.857445 time stamp seconds 1643496780 id 1 56 score 0 0.637981 time stamp seconds 1643496780 id 2 56 score 0 0.616743 time stamp seconds 1643496780 id 3 56 score 0 0.579421 time stamp seconds 1643496780 id 4 55 score 0 0.513886
We found 5 objects belonging to classes 49, 55 and 56. Probabilities were 86, 64, 62, 58 and 51 percent.
TBD: Clean up the output give more useful information, list any number of objects, print object labels, etc
TBD: cleanup - Other Potential Resources
ROS Org Answers
- How do I parse a Detection2DArray message
- [https://gist.github.com/alexsleat/1372845/7e39518cfa12ac91aca4378843e55862eb9ed41d#file-subscribe-cpp-L35
alexsleat/Publish.cpp]
visualizer
See note detections_msg20220122.txt for a visualizer.
documention
in tf_detections20220121.txt
---- expand detector listener interesting https://jeffzzq.medium.com/ros2-image-pipeline-tutorial-3b18903e7329 https://docs.ros2.org/foxy/api/ https://docs.ros2.org/foxy/api/sensor_msgs/msg/ https://docs.ros2.org/foxy/api/sensor_msgs/msg/Image.html std_msgs/msg/Header header uint32 height uint32 width string encoding uint8 is_bigendian uint32 step uint8[] data
Refine Detections Subscriber - Parse Detections Message
Subscriber Callback
void detections_callback(const vision_msgs::msg::Detection2DArray::SharedPtr detections2dArray) const { int isDone = false; int detectionsCount = 0; // TBD: get this from the image listener int IMAGE_WIDTH = 320; int TOLLERANCE = 10; RCLCPP_INFO(this->get_logger(), "---- "); RCLCPP_INFO(this->get_logger(), "Object detection message"); try { while (not isDone) { // Extract a single object detection vision_msgs::msg::Detection2D aDetection = detections2dArray->detections[detectionsCount++]; RCLCPP_INFO(this->get_logger(), " "); // Extract the object's id and confidence vision_msgs::msg::ObjectHypothesisWithPose result = aDetection.results[0]; int64_t id = stoi(result.id); float score = result.score; RCLCPP_INFO(this->get_logger(), " id %i", id); RCLCPP_INFO(this->get_logger(), " score %f", score); // Asspocate a few object id's with their string name. // TBD: pull this information from COCO labels // https://gist.github.com/aallan/fbdf008cffd1e08a619ad11a02b74fa8 //int64_t CAR = 3; //int64_t SPORTS_BALL = 37; int64_t BOTTLE = 44; // Bounding boxes vision_msgs::msg::BoundingBox2D boundingBox2d = aDetection.bbox; geometry_msgs::msg::Pose2D center = boundingBox2d.center; RCLCPP_INFO(this->get_logger(), " position (%f, %f)", center.x, center.y); RCLCPP_INFO(this->get_logger(), " size %f x %f", boundingBox2d.size_x, boundingBox2d.size_y); // The rest of the code in in the xxx list below // If it found a BOTTLE it build a Twist message // Sets the z rotation to stop, turn left or right // Publishes the Twist Message ... ... } // Exception handling for array out of bounds behavior is undefined. The behavior in // this example may not be the same in other C++ environments. Behavior was susceptible to // https://stackoverflow.com/questions/1239938/accessing-an-array-out-of-bounds-gives-no-error-why // Behavior is consistent enough in my environment to explore message parsing. catch (exception& e) { RCLCPP_INFO(this->get_logger(), "Exception %s", e.what()); isDone = true; } }
Subscriber Embedded in Program
- Note: small errors can result in voluminous meaningless errors that are impossible to match to an error. In particular the message paths and must match exactly. If they don't the complier gets lost in a string of package names.
- For example: I missed changing one of the message pointers from image to detections: vision_msgs::msg::Detection2DArray. Toke an hour or two to see the typo.
Subscriber Input
- This is the webcam image capture by the /cam2image node and published as an "/image message (topic)." The /dectection_server subscribes to the "/image" message and publishes a "/dectections" and "/dectections_image" message which looks like this:
- Notice:
- Object ids are defined in labels.txt
- int64_t CAR = 3;
- int64_t SPORTS_BALL = 37;
- int64_t BOTTLE = 44;
- As stated above the "/detections" message is created by the /detection_server node. The /image_Subscriber_OOP_Test node subscribes to it. This is the code we are writing.
- See rqt_graph below.
- The "/detections" message is define in
- It is of the following form.
Detection2DArray Header header Detection2D[] detections Header header ObjectHypothesisWithPose[] results int64 id float64 score PoseWithCovariance pose BoundingVox2D bbox Pose2D center float64 size_x float64 size_y Image source_image
* This is an example of a "\detections" message based on the above detections_image $ ros2 topic echo detections --- header: stamp: sec: 1644692469 nanosec: 503464021 frame_id: '' detections: - header: stamp: sec: 1644692469 nanosec: 503464021 frame_id: '' results: - id: '3' score: 0.9104859828948975 pose: pose: position: x: 0.0 y: 0.0 z: 0.0 orientation: x: 0.0 y: 0.0 z: 0.0 w: 1.0 covariance: - 0.0 - 0.0 - 0.0 - 0.0 - '...' repeats '0.0' for 30 or so times bbox: center: x: 161.57681465148926 y: 244.36246871948242 theta: 0.0 size_x: 104.41673278808594 size_y: 54.783639907836914 source_img: header: stamp: sec: 0 nanosec: 0 frame_id: map height: 55 width: 104 encoding: bgr8 is_bigendian: 0 step: 312 data: - 209 - 174 - 148 - 209 - 173 - '...' repeats one byte integers for a long time is_tracking: false tracking_id: '' - header: stamp: sec: 1644692469 nanosec: 503464021 frame_id: '' results: - id: '44' score: 0.8835442066192627 pose: pose: position: x: 0.0 y: 0.0 z: 0.0 orientation: x: 0.0 y: 0.0 z: 0.0 w: 1.0 covariance: - 0.0 - 0.0 - 0.0 - 0.0
- TBD: Rename /image_Subscriber_OOP_Test to something meaningful.
Logger Output
[INFO] [1644692166.415408150] [Image_Subscriber_OOP_Test]: Detection image message: encoding bgr8 [INFO] [1644692166.415550033] [Image_Subscriber_OOP_Test]: size 320 x 240 [INFO] [1644692166.511600888] [Image_Subscriber_OOP_Test]: ---- [INFO] [1644692166.511658790] [Image_Subscriber_OOP_Test]: Object detection message [INFO] [1644692166.511695117] [Image_Subscriber_OOP_Test]: [INFO] [1644692166.511729759] [Image_Subscriber_OOP_Test]: id 3 [INFO] [1644692166.511752299] [Image_Subscriber_OOP_Test]: score 0.920012 [INFO] [1644692166.511815646] [Image_Subscriber_OOP_Test]: position (161.534085, 244.812870) [INFO] [1644692166.511845515] [Image_Subscriber_OOP_Test]: size 104.727211 x 55.202351 [INFO] [1644692166.511886772] [Image_Subscriber_OOP_Test]: [INFO] [1644692166.511908266] [Image_Subscriber_OOP_Test]: id 44 [INFO] [1644692166.511927532] [Image_Subscriber_OOP_Test]: score 0.888334 [INFO] [1644692166.511949070] [Image_Subscriber_OOP_Test]: position (32.790173, 152.766781) [INFO] [1644692166.511977648] [Image_Subscriber_OOP_Test]: size 63.921785 x 177.125902 [INFO] [1644692166.512038127] [Image_Subscriber_OOP_Test]: Exception basic_string::_M_create [INFO] [1644692166.915096688] [Image_Subscriber_OOP_Test]: ---- [INFO] [1644692166.915242760] [Image_Subscriber_OOP_Test]: Detection image message: encoding bgr8 [INFO] [1644692166.915314105] [Image_Subscriber_OOP_Test]: size 320 x 240 [INFO] [1644692167.016898240] [Image_Subscriber_OOP_Test]: ---- [INFO] [1644692167.016990839] [Image_Subscriber_OOP_Test]: Object detection message [INFO] [1644692167.017032332] [Image_Subscriber_OOP_Test]: [INFO] [1644692167.017085759] [Image_Subscriber_OOP_Test]: id 3 [INFO] [1644692167.017111215] [Image_Subscriber_OOP_Test]: score 0.911544 [INFO] [1644692167.017147306] [Image_Subscriber_OOP_Test]: position (161.732769, 244.791164) [INFO] [1644692167.017184411] [Image_Subscriber_OOP_Test]: size 104.508286 x 55.179605 [INFO] [1644692167.017220862] [Image_Subscriber_OOP_Test]: [INFO] [1644692167.017251966] [Image_Subscriber_OOP_Test]: id 44 [INFO] [1644692167.017269509] [Image_Subscriber_OOP_Test]: score 0.892449 [INFO] [1644692167.017294431] [Image_Subscriber_OOP_Test]: position (32.734455, 152.264280) [INFO] [1644692167.017320379] [Image_Subscriber_OOP_Test]: size 63.703346 x 175.604768 [INFO] [1644692167.017380784] [Image_Subscriber_OOP_Test]: Exception basic_string::_M_create ^C[INFO] [1644692167.049173827] [rclcpp]: signal_handler(signal_value=2)