Implementation

Let's go ahead and tackle the implementation. In order to maintain simplicity of execution, we update state for all fields in each respective callback, but we call the client->evaluate_all() function in the callback for the CLIP embeddings. Generally, these have the highest latency.

📘
CLIP Embeddings
This will be covered in more detail in the launch file section, but this tutorial makes use of a bare-bones PyTorch CLIP embeddings ROS node.
There are no optimizations like TensorRT, so we achieve a maximum latency of 8 FPS on the Orin.
So, the image topics from the dataset are throttled to 5 Hz in order to have sufficient headroom.

The constructor is pretty straightforward, we initialize our 5 different subscribers, while also initializing the Smart Capture Client. We do a couple convenience functions as well, setting the Smart Capture LogLevel to WARN and grabbing the trigger file path from rosparam.

Each of the callbacks is also pretty straightforward, with the rough outline as follows:

Pointcloud: iterate through the pointcloud and update the client with the distance form ego of the closest point. This assumes that the pointcloud is in ego-frame; if not, that would have to be accounted for.
IMU: Update state for each linear acceleration field: x, y, z
BBox: Iterate through each bounding box and update state for lowest confidence detection, and number of total detections.
Embeddings: Update embeddings state in order to evaluate autotag triggers.

The implementation in its entirety below.

#include "ros1_lidar.hpp"

using namespace std;

namespace SmartCapture {
    SmartCaptureROS1Lidar::SmartCaptureROS1Lidar() : it_(nh_) {

        image_sub_ = it_.subscribe("/zed2/camera/left/image_raw",
                                   1,
                                   &SmartCaptureROS1Lidar::image_callback,
                                   this);

        pcl_sub_ = nh_.subscribe("/velodyne_points",
                                 1,
                                 &SmartCaptureROS1Lidar::pointcloud_callback,
                                 this);

        imu_sub_ = nh_.subscribe("/imu/data",
                                 1,
                                 &SmartCaptureROS1Lidar::imu_callback,
                                 this);

        bbox_sub_ = nh_.subscribe("/darknet_ros/bounding_boxes",
                                  1,
                                  &SmartCaptureROS1Lidar::bbox_callback,
                                  this);

        emb_sub_ = nh_.subscribe("/clip/embeddings",
                                 1,
                                 &SmartCaptureROS1Lidar::emb_callback,
                                 this);

        std::string trigger_file_path;
        ros::param::get("/trigger_file_path", trigger_file_path);

        ROS_INFO("Trigger File Path: %s", trigger_file_path.c_str());
        client = Client::make_client(trigger_file_path).second;

        Logger::loglevel = LOGLEVEL::WARN;
    }

    SmartCaptureROS1Lidar::~SmartCaptureROS1Lidar() {}

    void SmartCaptureROS1Lidar::image_callback(const sensor_msgs::ImageConstPtr &msg) {
        ROS_WARN_THROTTLE(1, "Received image_callback");
    }

    void SmartCaptureROS1Lidar::pointcloud_callback(const sensor_msgs::PointCloud2ConstPtr &msg) {
        ROS_WARN_THROTTLE(1, "Received pointcloud_callback");
        float min_dist_sq = 1000;
        for (sensor_msgs::PointCloud2ConstIterator<float> it(*msg, "x"); it != it.end(); ++it) {
            float dist_sq = it[0] * it[0] + it[1] * it[1] + it[2] * it[2];
            if (dist_sq < min_dist_sq) { min_dist_sq = dist_sq; }
        }
        float min_dist = std::sqrt(min_dist_sq);
        ROS_WARN_THROTTLE(1, "Closest distance: %f", min_dist);
        client->update_state<float>("closest_distance", min_dist);
    }

    void SmartCaptureROS1Lidar::imu_callback(const sensor_msgs::Imu &msg) {
        ROS_WARN_THROTTLE(1, "Received imu_callback");

        client->update_state<float>("imu_linear_x", msg.linear_acceleration.x);
        client->update_state<float>("imu_linear_y", msg.linear_acceleration.y);
        client->update_state<float>("imu_linear_z", msg.linear_acceleration.z);
    }

    void SmartCaptureROS1Lidar::bbox_callback(const darknet_ros_msgs::BoundingBoxes &msg) {
        ROS_WARN_THROTTLE(1, "Received bbox_callback");

        float min_prob = 100;
        for (auto &bbox: msg.bounding_boxes) {
            if (bbox.probability < min_prob) { min_prob = bbox.probability; }
        }
        client->update_state<float>("model_confidence", min_prob);
        client->update_state<int>("model_objects_detected", msg.bounding_boxes.size());
    }

    void SmartCaptureROS1Lidar::emb_callback(const std_msgs::Float64MultiArray &msg) {
        ROS_WARN_THROTTLE(1, "Received emb_callback");

        double embedding[msg.data.size()];
        for (size_t i = 0; i < msg.data.size(); ++i) { embedding[i] = msg.data[i]; }

        client->update_state<double *>("embeddings", embedding);
        client->update_state<int>("embeddings_width", 1);
        client->update_state<int>("embeddings_height", msg.data.size());

        auto results = client->evaluate_all();
        for (auto result: results) {
            ROS_INFO("Trigger id: %s. Result: %u. Evaluated: %u\n",
                     result->trigger_id.c_str(),
                     result->result,
                     result->evaluated);
        }
    }
}