Adding Object Detection in ROS

Object Detection with RVIZ

The ROS wrapper offers full support for the Object Detection module of the ZED SDK. The Object Detection module is available only using a ZED2 camera.

The Object Detection module can be configured to use one of four different detection models:

MULTI CLASS BOX: bounding boxes of objects of seven different classes (persons, vehicles, bags, animals, electronic devices, fruits and vegetables). Real time performance even on Jetson or low end GPU cards.
MULTI CLASS BOX ACCURATE: bounding boxes of objects of seven different classes (persons, vehicles, bags, animals, electronic devices, fruits and vegetables). State of the art accuracy, requires powerful GPU.
HUMAN BODY FAST: Keypoints based, specific to human skeleton. Real time performance even on Jetson or low end GPU cards.
HUMAN BODY ACCURATE: Keypoints based, specific to human skeleton. State of the art accuracy, requires powerful GPU.

The result of the detection is published using a new custom message of type zed_interfaces/ObjectsStamped defined in the package zed_interfaces.

Enable Object Detection

Object detection can be started automatically when the ZED Wrapper node start setting the parameter object_detection.od_enabled to true in the file zed2.yaml.

It is also possible to start the Object Detection processing manually calling the service ~/enable_obj_det with parameter True.

In both the cases the Object Detection processing can be stopped calling the service ~/enable_obj_det with parameter False.

See the services documentation for more info.

Object Detection results in RVIZ2

To visualize the results of the Object Detection processing in Rviz2 the new ZedOdDisplay plugin is required. The plugin is available in the zed-ros-examples Github repository and can be installed following the online instructions.

Note: the source code of the plugin is a valid example about how to process the data of the topics of type zed_interfaces/ObjectsStamped.

Parameters:

Topic: Selects the object detection topic to visualize from the list of available images in the combo box.
Depth: The depth of the incoming message queue.
History policy: Set the QoS history policy. Keep Last is suggested for performances and compatibility.
Reliability Policy: Set the QoS reliability policy. Best Effort is suggested for performances and compatibility.
Durability Policy: Set the QoS durability policy. Volatile is suggested for compatibility.
Transparency: the transparency level of the structures composing the detected objects.
Show skeleton: enable/disable the visualization of the skeleton of the detected persons (if available).
Show Labels: enable/disable the visualization of the objects label.
Show Bounding Boxes: enable/disable the visualization of the bounding boxes of the detected objects.
Link Size: the size of the bounding boxes corner lines and skeleton link lines.
Joint Radius: the radius of the spheres placed on the corners of the bounding boxes and on the skeleton joint points.
Label Scale: the scale of the label of the object.

Detected Objects message

The zed_interfaces/ObjectsStamped message is defined as:

# Standard Header
std_msgs/Header header

# Array of `object_stamped` topics
zed_interfaces/Object[] objects

where zed_interfaces/Object is defined as:

# Object label
string label

# Object label ID
int16 label_id

# Object sub
string sublabel

# Object confidence level (1-99)
float32 confidence

# Object centroid position
float32[3] position

# Position covariance
float32[6] position_covariance

# Object velocity
float32[3] velocity

# Tracking available
bool tracking_available

# Tracking state
# 0 -> OFF (object not valid)
# 1 -> OK
# 2 -> SEARCHING (occlusion occurred, trajectory is estimated)
int8 tracking_state

# Action state
# 0 -> IDLE
# 2 -> MOVING
int8 action_state

# 2D Bounding box projected to Camera image
zed_interfaces/BoundingBox2Di bounding_box_2d

# 3D Bounding box in world frame
zed_interfaces/BoundingBox3D bounding_box_3d

# 3D dimensions (width, height, lenght)
float32[3] dimensions_3d

# Is skeleton available?
bool skeleton_available

# 2D Bounding box projected to Camera image of the person head
zed_interfaces/BoundingBox2Df head_bounding_box_2d

# 3D Bounding box in world frame of the person head
zed_interfaces/BoundingBox3D head_bounding_box_3d

# 3D position of the centroid of the person head
float32[3] head_position

# 2D Person skeleton projected to Camera image
zed_interfaces/Skeleton2D skeleton_2d

# 3D Person skeleton in world frame
zed_interfaces/Skeleton3D skeleton_3d

And all the submessages are defined as following:

zed_interfaces/BoundingBox2Df:

#      0 ------- 1
#      |         |
#      |         |
#      |         |
#      3 ------- 2
zed_interfaces/Keypoint2Df[4] corners

zed_interfaces/BoundingBox2Df:

#      0 ------- 1
#      |         |
#      |         |
#      |         |
#      3 ------- 2
zed_interfaces/Keypoint2Di[4] corners

zed_interfaces/BoundingBox3D:

#      1 ------- 2
#     /.        /|
#    0 ------- 3 |
#    | .       | |
#    | 5.......| 6
#    |.        |/
#    4 ------- 7
zed_interfaces/Keypoint3D[8] corners

zed_interfaces/Keypoint2Df:

float32[2] kp

zed_interfaces/Keypoint2Di:

uint32[2] kp

zed_interfaces/Keypoint3D:

float32[3] kp

zed_interfaces/Skeleton2D:

# Skeleton joints indices
#        16-14   15-17
#             \ /
#              0
#              |
#       2------1------5
#       |    |   |    |
#       |    |   |    |
#       3    |   |    6
#       |    |   |    |
#       |    |   |    |
#       4    8   11   7
#            |   |
#            |   |
#            |   |
#            9   12
#            |   |
#            |   |
#            |   |
#           10   13
zed_interfaces/Keypoint2Df[18] keypoints

zed_interfaces/Skeleton3D:

# Skeleton joints indices
#        16-14   15-17
#             \ /
#              0
#              |
#       2------1------5
#       |    |   |    |
#       |    |   |    |
#       3    |   |    6
#       |    |   |    |
#       |    |   |    |
#       4    8   11   7
#            |   |
#            |   |
#            |   |
#            9   12
#            |   |
#            |   |
#            |   |
#           10   13
zed_interfaces/Keypoint3D[18] keypoints

Object list subscribing in C++

In this tutorial, you will learn how to write a simple C++ node that subscribes to messages of type zed_wrapper/ObjectsStamped. This lets you retrieve the list of detected object published by the ZED node for each camera frame.

Introduction

Use this command to connect the ZED 2 camera to the ROS network:

$ roslaunch zed_wrapper zed2.launch

The ZED node will start to publish object detection data in the network only if there is another node that subscribes to the relative topic and if the Object Detection module has been started.

Note: The Object Detection module in the ZED wrapper can start automatically only if the parameter object_detection/od_enabled in params/zed2.yaml is set to true (default false).

To start manually the module manually it is possible to use the service start_object_detection

Running the tutorial

If you properly followed the ROS Installation Guide, the executable of this tutorial has been compiled and you can run the subscriber node using this command:

$ rosrun zed_obj_det_sub_tutorial zed_obj_det_sub objes:=/zed2/zed_node/obj_det/objects

If the ZED node is running, and a ZED 2 is connected or you have loaded an SVO file, you will receive the following stream of messages confirming that you have correctly subscribed to the ZED image topics:

[ INFO] [1614264700.322685566]: ***** New object list *****
[ INFO] [1614264700.322717010]: Person [0] - Pos. [0.808921,-0.364787,-0.215571] [m]- Conf. 78.0129 - Tracking state: 1
[ INFO] [1614264700.370680525]: ***** New object list *****
[ INFO] [1614264700.370712147]: Person [0] - Pos. [0.809903,-0.365017,-0.215609] [m]- Conf. 80.6333 - Tracking state: 1
[ INFO] [1614264700.418698143]: ***** New object list *****
[ INFO] [1614264700.418729581]: Person [0] - Pos. [0.810399,-0.365178,-0.215623] [m]- Conf. 82.0206 - Tracking state: 1
[ INFO] [1614264700.426152141]: ***** New object list *****
[ INFO] [1614264700.426188225]: Person [0] - Pos. [0.810885,-0.365334,-0.215639] [m]- Conf. 80.0525 - Tracking state: 1
[ INFO] [1614264700.466679371]: ***** New object list *****
[ INFO] [1614264700.466709900]: Person [0] - Pos. [0.811356,-0.365492,-0.215652] [m]- Conf. 80.8249 - Tracking state: 1
[ INFO] [1614264700.514674426]: ***** New object list *****
[ INFO] [1614264700.514705119]: Person [0] - Pos. [0.811856,-0.365647,-0.215659] [m]- Conf. 83.1162 - Tracking state: 1
[ INFO] [1614264700.562679710]: ***** New object list *****
[ INFO] [1614264700.562711759]: Person [0] - Pos. [0.812835,-0.366073,-0.215665] [m]- Conf. 81.5075 - Tracking state: 1
[ INFO] [1614264700.610687663]: ***** New object list *****
[ INFO] [1614264700.610717479]: Person [0] - Pos. [0.813346,-0.366275,-0.215661] [m]- Conf. 81.6733 - Tracking state: 1
[ INFO] [1614264700.626001568]: ***** New object list *****
[ INFO] [1614264700.626036093]: Person [0] - Pos. [0.813875,-0.366408,-0.215656] [m]- Conf. 81.7185 - Tracking state: 1

[...]

Where the “Tracking state” values can be decoded as:

0 -> OFF (object not valid)
1 -> OK
2 -> SEARCHING (trajectory is estimated)

The code

The source code of the subscriber node zed_obj_det_sub_tutorial.cpp:


#include <ros/ros.h>
#include <zed_wrapper/object_stamped.h>
#include <zed_wrapper/objects.h>

/**
 * Subscriber callback
 */
void objectListCallback(const zed_interfaces::ObjectsStamped::ConstPtr& msg)
{
  ROS_INFO("***** New object list *****");
  for (int i = 0; i < msg->objects.size(); i++)
  {
    if (msg->objects[i].label_id == -1)
      continue;

    ROS_INFO_STREAM(msg->objects[i].label << " [" << msg->objects[i].label_id << "] - Pos. ["
                                          << msg->objects[i].position[0] << "," << msg->objects[i].position[1] << ","
                                          << msg->objects[i].position[2] << "] [m]"
                                          << "- Conf. " << msg->objects[i].confidence
                                          << " - Tracking state: " << static_cast<int>(msg->objects[i].tracking_state));
  }
}

/**
 * Node main function
 */
int main(int argc, char** argv) {
    ros::init(argc, argv, "zed_obj_det_sub_tutorial");

    ros::NodeHandle n;

    // Subscriber
    ros::Subscriber subObjList= n.subscribe("objects", 1, objectListCallback);

    ros::spin();

    return 0;
}

The code explained

The following is a brief explanation about the above source code:

void objectListCallback(const zed_wrapper::ObjectsStamped::ConstPtr& msg) {
    ROS_INFO( "***** New object list *****");
    for(int i=0; i<msg->objects.size();i++)
    {
        if(msg->objects[i].label_id == -1)
            continue;

        ROS_INFO_STREAM( msg->objects[i].label
                         << " ["
                         << msg->objects[i].label_id
                         << "] - Pos. ["
                         << msg->objects[i].position[0] << ","
                         << msg->objects[i].position[1] << ","
                         << msg->objects[i].position[2] << "] [m]"
                         << "- Conf. "
                         << msg->objects[i].confidence
                         << ""
                         << " - Tracking state: "
                         << static_cast<int>(msg->objects[i].tracking_state) );
    }
}

This callback is executed when the subscriber node receives a message of type zed_wrapper/ObjectsStamped that matches the subscribed topic. The parameter of the callback is a boost::shared_ptr to the received message. This means you don’t have to worry about memory management. The callback code is very simple and demonstrates how to access the fields in a message; in this case, the object list and for each object its label and label_id, the position and the tracking_state.

The main function is very standard and is explained in details in the “Talker/Listener” ROS tutorial.

The most important lesson of the above code is how the subscribers are defined:

    // Subscriber
    ros::Subscriber subObjList= n.subscribe("objects", 1, objectListCallback);

A ros::Subscriber is a ROS object that listens on the network and waits for its own topic message to be available. When a message is received, it executes the callback assigned to it.

We declared a single subscriber to the objects topic that calls the objectListCallback function when it receives a message of type zed_wrapper/OjectsStamped that matches that topic

Conclusion

The full source code of this tutorial is available on GitHub in the zed_obj_det_sub_tutorial sub-package. Along with the node source code are the package.xml and CMakeLists.txt files that complete the tutorial package.