View source for Object Detection Subscriber for Foxy alsora ros2-tensorflow 20220127

= 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

[[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>