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 not available with the ZED camera.

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

MULTI_CLASS_BOX_FAST: Any objects, bounding box based.
MULTI_CLASS_BOX_MEDIUM: Any objects, bounding box based, compromise between accuracy and speed.
MULTI_CLASS_BOX_ACCURATE: Any objects, bounding box based, more accurate but slower than the base model.
PERSON_HEAD_BOX_FAST: Bounding box detector specialized in person heads particularly well suited for crowded environments. The person localization is also improved.
PERSON_HEAD_BOX_ACCURATE: Bounding box detector specialized in person heads, particularly well suited for crowded environments. The person localization is also improved, more accurate but slower than the base model.

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 starts by setting the parameter object_detection.od_enabled to true in the file zed2.yaml or zed2i.yaml.

It is also possible to start the Object Detection processing manually by calling the service ~/enable_object_detection with true as parameter. In both cases, the Object Detection processing can be stopped by calling the service ~/enable_object_detection with false as parameter.

See the services documentation for more info.

Object Detection results in RVIZ 2 #

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 of 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 performance and compatibility.
Reliability Policy: Set the QoS reliability policy. Best Effort is suggested for performance 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 object 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 sub-messages are defined as follows:

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

or this command if you are using a ZED 2i:

$ roslaunch zed_wrapper zed2i.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 and ``params/zed2i.yamlis set totrue(defaultfalse`).

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 these commands:

ZED Mini:

$ rosrun zed_obj_det_sub_tutorial zed_obj_det_sub objects:=/zedm/zed_node/obj_det/objects

ZED 2:

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

ZED 2i:

$ rosrun zed_obj_det_sub_tutorial zed_obj_det_sub objects:=/zed2i/zed_node/obj_det/objects

ZED X:

$ rosrun zed_obj_det_sub_tutorial zed_obj_det_sub objects:=/zedx/zed_node/obj_det/objects

ZED X Mini:

$ rosrun zed_obj_det_sub_tutorial zed_obj_det_sub objects:=/zedxm/zed_node/obj_det/objects

If the ZED node is running and a camera 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 of the source code above:

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 detail 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.