Object Detection Subscriber for Foxy alsora ros2-tensorflow 20220127
From wikidb
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)
