alb_publisher.cpp

Go to the documentation of this file.

// ROS headers
#include "geometry_msgs/PointStamped.h"
#include "geometry_msgs/TransformStamped.h"
#include "sensor_msgs/TimeReference.h"
 
#include "tf2/convert.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/transform_listener.h"
 
// Osef headers
#include "osefTypes.h"
#include "typeCaster.h"
 
// Local headers
#include "alb_common.h"
#include "alb_curl_helper.h"
#include "alb_helper.h"
#include "alb_publisher.h"
#include "outsight_alb_driver/AugmentedCloud.h"
#include "outsight_alb_driver/ObjectData.h"
#include "outsight_alb_driver/TrackedObjects.h"
 
// Constant definition.
constexpr const char *k_default_fixed_frame_id = "alb_fixed_frame";
constexpr const char *k_default_sensor_frame_id = "alb_sensor_frame";
constexpr const uint32_t k_queue_size = 1000;
 
AlbPublisher::AlbPublisher(ros::NodeHandle &nodeHandle)
{
        initPublishers(nodeHandle);
        initTransforms();
        buildZones();
 
        Helper::init_pose(odom_msg.pose.pose);
        last_timestamp = ros::Time::now();
}
 
void AlbPublisher::initPublishers(ros::NodeHandle &nodeHandle)
{
        ros::NodeHandle private_node("~");
        bool output;
 
        // Define TimeReference publisher.
        private_node.param<bool>(ALBCommon::k_alb_time_ref_config, output, false);
        if (output) {
                time_reference_publisher =
                        nodeHandle.advertise<sensor_msgs::TimeReference>(ALBCommon::k_alb_time_ref, k_queue_size);
        }
 
        // Define used timestamp (ALB or ROS).
        private_node.param<bool>(ALBCommon::k_alb_use_alb_time, use_alb_time, false);
 
        // Define point cloud order
        private_node.param<bool>(ALBCommon::k_alb_use_colwise_order, use_colwise_order, true);
 
        // Define Pose publisher.
        private_node.param<bool>(ALBCommon::k_alb_pose_config, output, false);
        if (output) {
                pose_publisher = nodeHandle.advertise<geometry_msgs::PoseStamped>(ALBCommon::k_alb_pose, k_queue_size);
        }
 
        // Define PointCloud publisher.
        private_node.param<bool>(ALBCommon::k_alb_point_cloud_config, output, false);
        if (output) {
                point_cloud_publisher =
                        nodeHandle.advertise<sensor_msgs::PointCloud2>(ALBCommon::k_alb_point_cloud, k_queue_size);
        }
 
        // Define TrackedObjects publisher.
        private_node.param<bool>(ALBCommon::k_alb_objects_config, output, false);
        if (output) {
                tracked_objects_publisher = nodeHandle.advertise<outsight_alb_driver::TrackedObjects>(
                        ALBCommon::k_alb_objects, k_queue_size);
        }
 
        // Define Odometry publisher.
        private_node.param<bool>(ALBCommon::k_alb_odometry_config, output, false);
        if (output) {
                odometry_publisher = nodeHandle.advertise<nav_msgs::Odometry>(ALBCommon::k_alb_odometry, k_queue_size);
        }
 
        // Define Egomotion publisher.
        private_node.param<bool>(ALBCommon::k_alb_egomotion_config, output, false);
        if (output) {
                egomotion_publisher =
                        nodeHandle.advertise<geometry_msgs::PoseStamped>(ALBCommon::k_alb_egomotion, k_queue_size);
        }
 
        // Define AugmentedCloud publisher.
        private_node.param<bool>(ALBCommon::k_alb_augmented_cloud_config, output, false);
        if (output) {
                augmented_cloud_publisher = nodeHandle.advertise<outsight_alb_driver::AugmentedCloud>(
                        ALBCommon::k_alb_augmented_cloud, k_queue_size);
        }
 
        // Define Zones publisher.
        private_node.param<bool>(ALBCommon::k_alb_zones_config, output, false);
        if (output) {
                zones_publisher =
                        nodeHandle.advertise<outsight_alb_driver::Zones>(ALBCommon::k_alb_zones, k_queue_size);
        }
}
 
void AlbPublisher::initTransforms()
{
        ros::NodeHandle private_node("~");
 
        // Define the frame ID.
        private_node.param<std::string>(ALBCommon::k_alb_fixed_frame_id, fixed_frame_id, k_default_fixed_frame_id);
        private_node.param<std::string>(ALBCommon::k_alb_sensor_frame_id, sensor_frame_id, k_default_sensor_frame_id);
        private_node.param<std::string>(ALBCommon::k_alb_base_frame_id, base_frame_id, "");
        private_node.param<std::string>(ALBCommon::k_alb_map_frame_id, map_frame_id, "");
 
        // Broadcast the transforms
        private_node.param<bool>(ALBCommon::k_alb_broadcast_tf_config, transform_broadcaster, false);
 
        tf2_ros::Buffer tfBuffer;
        tf2_ros::TransformListener tfListener(tfBuffer);
        try {
                if (base_frame_id.empty()) {
                        ROS_WARN("[AlbPublisher] No base_frame_id specified in the config file.");
                        return;
                }
                geometry_msgs::TransformStamped base_to_lidar =
                        tfBuffer.lookupTransform(base_frame_id, sensor_frame_id, ros::Time(0), ros::Duration(0.5));
                tf2::fromMsg(base_to_lidar.transform, base_to_lidar_tf);
                use_base_frame = true;
                ROS_INFO("[AlbPublisher] Base to lidar transform received.");
        } catch (tf2::TransformException &ex) {
                ROS_WARN(
                        "[AlbPublisher] No base to lidar transform provided, package will not be able to relocate Lidar frame in the robot frame.");
                ROS_WARN("[AlbPublisher] %s", ex.what());
                use_base_frame = false;
                return;
        }
}
 
void AlbPublisher::buildZones()
{
        ros::NodeHandle private_node("~");
        std::string ip_address;
        if (!private_node.hasParam(ALBCommon::k_alb_ip)) {
                ROS_ERROR("[AlbPublisher] IP address not defined");
                return;
        }
        private_node.getParam(ALBCommon::k_alb_ip, ip_address);
        AlbCurlHelper curl_helper(ip_address);
 
        std::string raw_zones;
        if (!curl_helper.executeProcessingZones(raw_zones)) {
                ROS_INFO("%s", curl_helper.getErrorMessage().c_str());
                return;
        }
 
        zones_msg.header.frame_id = map_frame_id;
 
        Helper::parse_zone_data(raw_zones, zones_msg);
        ROS_INFO("[AlbPublisher] %ld zones detected.", zones_msg.zones.size());
}
 
void AlbPublisher::publish(const Tlv::tlv_s *frame)
{
        ros_time = ros::Time::now();
        updateTimeStamp(frame);
        if (use_alb_time) {
                current_timestamp = alb_time;
        } else {
                current_timestamp = ros_time;
        }
 
        updatePose(frame);
 
        if (time_reference_publisher) {
                publishTimeReference(frame);
        }
 
        if (pose_publisher) {
                publishPose(frame);
        }
 
        if (point_cloud_publisher) {
                publishPointCloud(frame);
        }
 
        if (tracked_objects_publisher) {
                publishTrackedObjects(frame);
        }
 
        if (egomotion_publisher) {
                publishEgomotion(frame);
        }
 
        if (odometry_publisher) {
                publishOdometry(frame);
        }
 
        if (augmented_cloud_publisher) {
                publishAugmentedCloud(frame);
        }
 
        if (zones_publisher) {
                publishZones();
        }
 
        if (transform_broadcaster) {
                broadcastAlbTransform(current_pose);
        }
 
        if (pose_found) {
                last_pose = current_pose;
        }
        last_timestamp = current_timestamp;
}
 
void AlbPublisher::publishTimeReference(const Tlv::tlv_s *frame)
{
        Tlv::Parser parser(frame->getValue(), frame->getLength());
        const Tlv::tlv_s *timeTlv = parser.findTlv(OSEF_TYPE_TIMESTAMP_MICROSECOND);
        if (!timeTlv) {
                ROS_ERROR("[AlbPublisher] Timestamp not found, unable to publish TimeReference.");
                return;
        }
 
        const TimestampMicroseconds::timestamp_s timestamp_osef = TimestampMicroseconds::castValue(timeTlv);
 
        sensor_msgs::TimeReference time_message;
        time_message.header.stamp = current_timestamp;
        time_message.header.frame_id = fixed_frame_id;
        if (use_alb_time) {
                time_message.time_ref = ros_time;
                time_message.source = "alb_time";
        } else {
                time_message.time_ref = alb_time;
                time_message.source = "ros_time";
        }
 
        time_reference_publisher.publish(time_message);
}
 
void AlbPublisher::publishPointCloud(const Tlv::tlv_s *frame)
{
        Tlv::Parser parser(frame->getValue(), frame->getLength());
        const Tlv::tlv_s *frameTlv = parser.findTlv(OSEF_TYPE_SCAN_FRAME);
        if (!frameTlv) {
                ROS_ERROR("[AlbPublisher] Scan frame not found, unable to publish PointCloud.");
                return;
        }
 
        Tlv::Parser frameParser(frameTlv->getValue(), frameTlv->getLength());
        const Tlv::tlv_s *augmented_point_cloud = frameParser.findTlv(OSEF_TYPE_AUGMENTED_CLOUD);
        if (!augmented_point_cloud) {
                ROS_ERROR("[AlbPublisher] Augmented point cloud not found, unable to publish PointCloud.");
                return;
        }
 
        // To compute the point Cloud, get the cartesian coordinates and apply the pose transformation.
        const Tlv::Parser cloudParser(augmented_point_cloud->getValue(), augmented_point_cloud->getLength());
        const Tlv::tlv_s *cartesian_tlv = cloudParser.findTlv(OSEF_TYPE_CARTESIAN_COORDINATES);
        const Tlv::tlv_s *number_points_tlv = cloudParser.findTlv(OSEF_TYPE_NUMBER_OF_POINTS);
        const Tlv::tlv_s *number_layers_tlv = cloudParser.findTlv(OSEF_TYPE_NUMBER_OF_LAYERS);
 
        if (!cartesian_tlv || !number_points_tlv || !number_layers_tlv) {
                std::string error_msg(!cartesian_tlv ? "Cartesian not found. " : "");
                error_msg.append(!number_points_tlv ? "Number of points not found." : "");
                error_msg.append(!number_layers_tlv ? "Number of layers not found." : "");
 
                ROS_ERROR("[AlbPublisher] Unable to publish PointCloud: %s", error_msg.c_str());
                return;
        }
 
        const uint32_t number_points = NumberOfObjects::castValue(number_points_tlv);
        const uint32_t number_layers = NumberOfObjects::castValue(number_layers_tlv);
        sensor_msgs::PointCloud2 point_cloud_msg;
        point_cloud_msg.header.stamp = current_timestamp;
        point_cloud_msg.header.frame_id = sensor_frame_id;
 
        Helper::define_point_fields(point_cloud_msg);
        Helper::define_points_data(point_cloud_msg, number_layers, number_points, (float *)(cartesian_tlv->getValue()),
                                   use_colwise_order);
 
        point_cloud_publisher.publish(point_cloud_msg);
}
 
void AlbPublisher::publishPose(const Tlv::tlv_s *frame)
{
        if (!pose_found) {
                return;
        }
 
        geometry_msgs::PoseStamped pose_msg;
        pose_msg.header.stamp = current_timestamp;
        pose_msg.header.frame_id = map_frame_id;
        pose_msg.pose = current_pose;
        pose_publisher.publish(pose_msg);
}
 
void AlbPublisher::publishTrackedObjects(const Tlv::tlv_s *frame)
{
        Tlv::Parser parser(frame->getValue(), frame->getLength());
        const Tlv::tlv_s *frameTlv = parser.findTlv(OSEF_TYPE_SCAN_FRAME);
        if (!frameTlv) {
                ROS_ERROR("[AlbPublisher] Scan frame not found, unable to publish BoundingBoxes message.");
                return;
        }
 
        Tlv::Parser frameParser(frameTlv->getValue(), frameTlv->getLength());
        const Tlv::tlv_s *objectsTlv = frameParser.findTlv(OSEF_TYPE_TRACKED_OBJECTS);
        if (!objectsTlv) {
                ROS_ERROR("[AlbPublisher] Tracked objects not found, unable to publish BoundingBoxes message.");
                return;
        }
 
        Tlv::Parser objectsParser(objectsTlv->getValue(), objectsTlv->getLength());
        const Tlv::tlv_s *num_objects = objectsParser.findTlv(OSEF_TYPE_NUMBER_OF_OBJECTS);
        const Tlv::tlv_s *object_ids = objectsParser.findTlv(OSEF_TYPE_OBJECT_ID_32_BITS);
        const Tlv::tlv_s *bbox_sizes = objectsParser.findTlv(OSEF_TYPE_BBOX_SIZES);
        const Tlv::tlv_s *pose_array = objectsParser.findTlv(OSEF_TYPE_POSE_ARRAY);
        const Tlv::tlv_s *speed_vectors = objectsParser.findTlv(OSEF_TYPE_SPEED_VECTORS);
        const Tlv::tlv_s *class_ids = objectsParser.findTlv(OSEF_TYPE_CLASS_ID_ARRAY);
        if (!num_objects || !object_ids || !bbox_sizes || !pose_array || !speed_vectors || !class_ids) {
                std::string error_msg(!num_objects ? "Number of objects not found. " : "");
                error_msg.append(!object_ids ? "Object ids not found. " : "");
                error_msg.append(!bbox_sizes ? "Box sizes not found. " : "");
                error_msg.append(!pose_array ? "Pose array not found. " : "");
                error_msg.append(!speed_vectors ? "Speed vectors not found. " : "");
                error_msg.append(!class_ids ? "Class IDs not found. " : "");
 
                ROS_ERROR("[AlbPublisher] Unable to publish BoundingBoxes message: %s.", error_msg.c_str());
                return;
        }
 
        outsight_alb_driver::TrackedObjects tracked_message;
        tracked_message.header.stamp = current_timestamp;
        tracked_message.header.frame_id = fixed_frame_id;
        tracked_message.number_of_objects = NumberOfObjects::castValue(num_objects);
        uint32_t *ids = (uint32_t *)(object_ids->getValue());
 
        for (size_t box_index = 0; box_index < tracked_message.number_of_objects; box_index++) {
                outsight_alb_driver::ObjectData object;
 
                object.id = ids[box_index];
                Helper::define_box_size(object, bbox_sizes, box_index);
                Helper::define_box_pose(object, pose_array, box_index);
                Helper::define_object_speed(object, speed_vectors, box_index);
 
                uint32_t *class_id = (uint32_t *)(class_ids->getValue() + sizeof(uint32_t) * box_index);
                object.object_class = Helper::get_object_class(class_id[0]);
 
                tracked_message.objects.push_back(object);
        }
 
        tracked_objects_publisher.publish(tracked_message);
}
 
void AlbPublisher::publishEgomotion(const Tlv::tlv_s *frame)
{
        if (!pose_found) {
                return;
        }
 
        geometry_msgs::PoseStamped ego_msg;
        ego_msg.header.stamp = current_timestamp;
        ego_msg.header.frame_id = sensor_frame_id;
        if (!Helper::get_egomotion_from_tlv(frame, ego_msg)) {
                return;
        }
 
        egomotion_publisher.publish(ego_msg);
}
 
void AlbPublisher::publishOdometry(const Tlv::tlv_s *frame)
{
        if (!pose_found) {
                return;
        }
 
        odom_msg.header.stamp = current_timestamp;
        odom_msg.header.frame_id = fixed_frame_id;
        odom_msg.child_frame_id = sensor_frame_id;
 
        float dt = (current_timestamp - last_timestamp).toSec();
 
        Helper::computeOdometry(odom_msg, current_pose, last_pose, first_pose, dt);
 
        odometry_publisher.publish(odom_msg);
}
 
void AlbPublisher::publishAugmentedCloud(const Tlv::tlv_s *frame)
{
        Tlv::Parser parser(frame->getValue(), frame->getLength());
        const Tlv::tlv_s *frameTlv = parser.findTlv(OSEF_TYPE_SCAN_FRAME);
        if (!frameTlv) {
                ROS_ERROR("[AlbPublisher] Scan frame not found, unable to publish PointCloud.");
                return;
        }
 
        Tlv::Parser frameParser(frameTlv->getValue(), frameTlv->getLength());
        const Tlv::tlv_s *augmented_point_cloud = frameParser.findTlv(OSEF_TYPE_AUGMENTED_CLOUD);
        if (!augmented_point_cloud) {
                ROS_ERROR("[AlbPublisher] Augmented point cloud not found, unable to publish PointCloud.");
                return;
        }
 
        outsight_alb_driver::AugmentedCloud message;
        message.header.stamp = current_timestamp;
        message.header.frame_id = sensor_frame_id;
        Helper::define_augmented_cloud(message, augmented_point_cloud);
        augmented_cloud_publisher.publish(message);
}
 
void AlbPublisher::publishZones()
{
        zones_msg.header.stamp = current_timestamp;
 
        zones_publisher.publish(zones_msg);
}
 
void AlbPublisher::updatePose(const Tlv::tlv_s *frame)
{
        geometry_msgs::PoseStamped pose_msg;
        pose_found = Helper::get_pose_from_tlv(frame, pose_msg);
        if (!pose_found) {
                return;
        }
 
        if (!get_first_pose) {
                broadcastMapTransform(pose_msg.pose);
 
                first_pose = pose_msg.pose;
                last_pose = pose_msg.pose;
                get_first_pose = true;
        }
        current_pose = pose_msg.pose;
}
 
void AlbPublisher::updateTimeStamp(const Tlv::tlv_s *frame)
{
        Tlv::Parser parser(frame->getValue(), frame->getLength());
        const Tlv::tlv_s *timeTlv = parser.findTlv(OSEF_TYPE_TIMESTAMP_MICROSECOND);
        TimestampMicroseconds::timestamp_s timestamp_osef = TimestampMicroseconds::castValue(timeTlv);
        alb_time = ros::Time(timestamp_osef.seconds, timestamp_osef.micro_seconds * 1000);
}
 
void AlbPublisher::broadcastAlbTransform(const geometry_msgs::Pose &pose)
{
        if (!odometry_publisher) {
                ROS_WARN(
                        "[AlbPublisher] Transform between fixed frame and sensor frame requires odometry to be published. Set the odometry parameter to 'true' in order to broadcast this transform.");
                return;
        }
 
        static tf2_ros::TransformBroadcaster broadcaster;
 
        geometry_msgs::Transform odom_to_lidar;
        odom_to_lidar.translation.x = odom_msg.pose.pose.position.x;
        odom_to_lidar.translation.y = odom_msg.pose.pose.position.y;
        odom_to_lidar.translation.z = odom_msg.pose.pose.position.z;
        odom_to_lidar.rotation = odom_msg.pose.pose.orientation;
 
        tf2::Transform odom_to_lidar_tf;
        tf2::fromMsg(odom_to_lidar, odom_to_lidar_tf);
 
        geometry_msgs::TransformStamped transformStamped;
        transformStamped.header.stamp = current_timestamp;
        if (use_base_frame) {
                tf2::Transform odom_to_base_tf = odom_to_lidar_tf * base_to_lidar_tf.inverse();
                transformStamped.header.frame_id = fixed_frame_id;
                transformStamped.child_frame_id = base_frame_id;
                transformStamped.transform = tf2::toMsg(odom_to_base_tf);
        } else {
                transformStamped.header.frame_id = fixed_frame_id;
                transformStamped.child_frame_id = sensor_frame_id;
                transformStamped.transform = tf2::toMsg(odom_to_lidar_tf);
        }
 
        broadcaster.sendTransform(transformStamped);
}
 
void AlbPublisher::broadcastMapTransform(const geometry_msgs::Pose &pose)
{
        if (map_frame_id.empty()) {
                ROS_WARN("[AlbPublisher] No map_frame_id specified in the config file");
        }
        static tf2_ros::StaticTransformBroadcaster static_broadcaster;
        geometry_msgs::TransformStamped static_transformStamped;
 
        static_transformStamped.header.stamp = current_timestamp;
        static_transformStamped.header.frame_id = map_frame_id;
        static_transformStamped.child_frame_id = fixed_frame_id;
        static_transformStamped.transform.translation.x = pose.position.x;
        static_transformStamped.transform.translation.y = pose.position.y;
        static_transformStamped.transform.translation.z = pose.position.z;
        static_transformStamped.transform.rotation = pose.orientation;
 
        static_broadcaster.sendTransform(static_transformStamped);
}