alb_helper.cpp

Go to the documentation of this file.

// Standard headers
#include <algorithm>
#include <map>
 
// ROS headers
#include "ros/ros.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
 
// Osef headers
#include "osefTypes.h"
#include "typeCaster.h"
 
// Local headers
#include "alb_common.h"
#include "alb_helper.h"
 
using json = nlohmann::json;
 
// Constant definition.
const std::map<int, std::string> k_object_class = { { 0, "UNKNOWN" }, { 1, "PERSON" },  { 2, "LUGGAGE" },
                                                    { 3, "TROLLEY" }, { 4, "TRUCK" },   { 5, "BUS" },
                                                    { 6, "CAR" },     { 7, "VAN" },     { 8, "TWO_WHEELER" },
                                                    { 9, "MASK" },    { 10, "NO_MASK" } };
 
namespace Helper
{
void define_point_fields(sensor_msgs::PointCloud2 &pointCloud)
{
        sensor_msgs::PointField x_field;
        x_field.name = "x";
        x_field.offset = 0;
        x_field.datatype = sensor_msgs::PointField::FLOAT32;
        x_field.count = 1;
 
        sensor_msgs::PointField y_field;
        y_field.name = "y";
        y_field.offset = 4;
        y_field.datatype = sensor_msgs::PointField::FLOAT32;
        y_field.count = 1;
 
        sensor_msgs::PointField z_field;
        z_field.name = "z";
        z_field.offset = 8;
        z_field.datatype = sensor_msgs::PointField::FLOAT32;
        z_field.count = 1;
 
        pointCloud.fields.push_back(x_field);
        pointCloud.fields.push_back(y_field);
        pointCloud.fields.push_back(z_field);
}
 
void define_points_data(sensor_msgs::PointCloud2 &pointCloud, const uint32_t layers, const uint32_t points,
                        float *pointData, bool use_colwise_order)
{
        // Height and width define the number of points ("width" points in "height" dimension);
        if (!layers) {
                pointCloud.height = 1;
                pointCloud.width = points;
        } else {
                pointCloud.height = layers;
                pointCloud.width = points / layers;
        }
 
        // Steps define the data storage.
        constexpr size_t point_size = 3 * sizeof(float); // Size of a point in bytes
        pointCloud.point_step = point_size;
        pointCloud.row_step = pointCloud.width * pointCloud.point_step;
 
        // Point data have to be reinterpreted into uint8_t.
        const uint8_t *point_data = reinterpret_cast<uint8_t *>(pointData);
        const size_t total_size = point_size * points;
        if (use_colwise_order || !layers) {
                pointCloud.data.assign(point_data, point_data + total_size);
        } else {
                pointCloud.data.resize(total_size);
                size_t colwise_idx = 0;
                for (uint32_t w = 0; w < pointCloud.width; ++w) {
                        for (uint32_t h = 0; h < layers; ++h, colwise_idx += point_size) {
                                size_t rowwise_idx = (h * pointCloud.width + w) * point_size;
                                std::copy(&point_data[colwise_idx], &point_data[colwise_idx] + point_size,
                                          &(pointCloud.data[rowwise_idx]));
                        }
                }
        }
        pointCloud.is_dense = true;
}
 
void define_pose(geometry_msgs::Pose &pose, const std::array<float, 3> &position, const std::array<float, 9> &rotation)
{
        pose.position.x = position[0];
        pose.position.y = position[1];
        pose.position.z = position[2];
 
        // Convert rotation matrix to quaternion orientation. (Stuelpnagel 1964)
        float t = rotation[0] + rotation[4] + rotation[8];
 
        if (1 + t > 0.0f) {
                float r = sqrt(1 + t);
                float s = 1 / (2 * r);
 
                pose.orientation.x = (rotation[5] - rotation[7]) * s;
                pose.orientation.y = (rotation[6] - rotation[2]) * s;
                pose.orientation.z = (rotation[1] - rotation[3]) * s;
                pose.orientation.w = r / 2;
        } else if (rotation[0] > rotation[4] && rotation[0] > rotation[8]) {
                float r = sqrt(1.0f + rotation[0] - rotation[4] - rotation[8]);
                float s = 1 / (2 * r);
                pose.orientation.x = r / 2;
                pose.orientation.y = (rotation[1] + rotation[3]) * s;
                pose.orientation.z = (rotation[2] + rotation[6]) * s;
                pose.orientation.w = (rotation[5] - rotation[7]) * s;
        } else if (rotation[4] > rotation[8]) {
                float r = sqrt(1.0f + rotation[4] - rotation[0] - rotation[8]);
                float s = 1 / (2 * r);
                pose.orientation.x = (rotation[1] + rotation[3]) * s;
                pose.orientation.y = r / 2;
                pose.orientation.z = (rotation[5] + rotation[7]) * s;
                pose.orientation.w = (rotation[6] - rotation[2]) * s;
        } else {
                float r = sqrt(1.0f + rotation[8] - rotation[0] - rotation[4]);
                float s = 1 / (2 * r);
                pose.orientation.x = (rotation[6] + rotation[2]) * s;
                pose.orientation.y = (rotation[5] + rotation[7]) * s;
                pose.orientation.z = r / 2;
                pose.orientation.w = (rotation[1] - rotation[3]) * s;
        }
 
        // Normalize the quaternion.
        tf2::Quaternion current_quat;
        tf2::fromMsg(pose.orientation, current_quat);
        current_quat.normalize();
        pose.orientation = tf2::toMsg(current_quat);
}
 
void init_pose(geometry_msgs::Pose &pose)
{
        pose.position.x = 0.0f;
        pose.position.y = 0.0f;
        pose.position.z = 0.0f;
        pose.orientation.x = 0.0f;
        pose.orientation.y = 0.0f;
        pose.orientation.z = 0.0f;
        pose.orientation.w = 1.0f;
}
 
void parse_zone_data(const std::string &raw_zones, outsight_alb_driver::Zones &zones_msg)
{
        const json parsed_zones = json::parse(raw_zones);
 
        for (auto zone_it = parsed_zones[ALBCommon::k_zone_config_zones].begin();
             zone_it != parsed_zones[ALBCommon::k_zone_config_zones].end(); ++zone_it) {
                if ((*zone_it)[ALBCommon::k_zone_config_role].get<std::string>() == ALBCommon::k_zone_config_event) {
                        outsight_alb_driver::ZoneData zone;
                        zone.name = (*zone_it)[ALBCommon::k_zone_config_name].get<std::string>();
                        zone.id = (*zone_it)[ALBCommon::k_zone_config_id].get<std::string>();
 
                        for (auto pt_it = (*zone_it)[ALBCommon::k_zone_config_points].begin();
                             pt_it != (*zone_it)[ALBCommon::k_zone_config_points].end(); pt_it++) {
                                geometry_msgs::Point point;
                                point.x = (*pt_it)[0];
                                point.y = (*pt_it)[1];
                                zone.points.push_back(point);
                        }
 
                        zone.role = (*zone_it)[ALBCommon::k_zone_config_role].get<std::string>();
                        zones_msg.zones.push_back(zone);
                }
        }
}
 
bool get_pose_from_tlv(const Tlv::tlv_s *frame, geometry_msgs::PoseStamped &pose)
{
        Tlv::Parser parser(frame->getValue(), frame->getLength());
        const Tlv::tlv_s *frameTlv = parser.findTlv(OSEF_TYPE_SCAN_FRAME);
        if (!frameTlv) {
                ROS_ERROR("[AlbHelper] Scan frame not found, unable to publish PoseStamped message.");
                return false;
        }
 
        Tlv::Parser frameParser(frameTlv->getValue(), frameTlv->getLength());
        const Tlv::tlv_s *pose_tlv = frameParser.findTlv(OSEF_TYPE_POSE);
        if (!pose_tlv) {
                ROS_ERROR("[AlbHelper] Pose not found, unable to publish PoseStamped message.");
                return false;
        }
 
        std::array<float, 3> pose_translation(Pose::castTranslationValue(pose_tlv));
        std::array<float, 9> pose_rotation(Pose::castRotationValue(pose_tlv));
 
        define_pose(pose.pose, pose_translation, pose_rotation);
 
        return true;
}
 
bool get_egomotion_from_tlv(const Tlv::tlv_s *frame, geometry_msgs::PoseStamped &pose)
{
        Tlv::Parser parser(frame->getValue(), frame->getLength());
        const Tlv::tlv_s *frameTlv = parser.findTlv(OSEF_TYPE_SCAN_FRAME);
        if (!frameTlv) {
                ROS_ERROR("[AlbHelper] Scan frame not found, unable to publish PoseStamped message.");
                return false;
        }
 
        Tlv::Parser frameParser(frameTlv->getValue(), frameTlv->getLength());
        const Tlv::tlv_s *egomotion_tlv = frameParser.findTlv(OSEF_TYPE_EGO_MOTION);
        if (!egomotion_tlv) {
                ROS_ERROR("[AlbHelper] Egomotion not found, unable to publish PoseStamped message.");
                return false;
        }
 
        Tlv::Parser egomotionParser(egomotion_tlv->getValue(), egomotion_tlv->getLength());
        const Tlv::tlv_s *pose_tlv = egomotionParser.findTlv(OSEF_TYPE_POSE_RELATIVE);
        if (!pose_tlv) {
                ROS_ERROR("[AlbHelper] Pose not found, unable to publish PoseStamped message.");
                return false;
        }
 
        std::array<float, 3> pose_translation(Pose::castTranslationValue(pose_tlv));
        std::array<float, 9> pose_rotation(Pose::castRotationValue(pose_tlv));
 
        define_pose(pose.pose, pose_translation, pose_rotation);
 
        return true;
}
 
void computeOdometry(nav_msgs::Odometry &odom, const geometry_msgs::Pose &current_pose,
                     const geometry_msgs::Pose &last_pose, const geometry_msgs::Pose &first_pose, float dt)
{
        // Compute Pose displacement between first pose and current pose.
        geometry_msgs::PoseStamped global_pose;
        geometry_msgs::PoseStamped global_pose_from_first;
        global_pose.pose.position.x = current_pose.position.x - first_pose.position.x;
        global_pose.pose.position.y = current_pose.position.y - first_pose.position.y;
        global_pose.pose.position.z = current_pose.position.z - first_pose.position.z;
 
        // Define transform from first pose.
        geometry_msgs::TransformStamped local_transform;
        local_transform.transform.rotation = first_pose.orientation;
        local_transform.transform.rotation.w = -local_transform.transform.rotation.w;
 
        tf2::doTransform(global_pose, global_pose_from_first, local_transform);
 
        // Odometry is the pose displacement.
        odom.pose.pose.position.x = global_pose_from_first.pose.position.x;
        odom.pose.pose.position.y = global_pose_from_first.pose.position.y;
        odom.pose.pose.position.z = global_pose_from_first.pose.position.z;
 
        // Compute rotation between first and current pose.
        tf2::Quaternion current_orientation;
        tf2::Quaternion first_orientation;
        tf2::Quaternion odom_orientation;
        tf2::fromMsg(current_pose.orientation, current_orientation);
        tf2::fromMsg(first_pose.orientation, first_orientation);
        odom_orientation = current_orientation * first_orientation.inverse();
        odom_orientation.normalize();
        odom.pose.pose.orientation = tf2::toMsg(odom_orientation);
 
        // Compute displacement between the current and last frame in the fixed frame
        geometry_msgs::PoseStamped local_pose;
        geometry_msgs::PoseStamped local_pose_from_first;
        local_pose.pose.position.x = current_pose.position.x - last_pose.position.x;
        local_pose.pose.position.y = current_pose.position.y - last_pose.position.y;
        local_pose.pose.position.z = current_pose.position.z - last_pose.position.z;
 
        // Compute the linear instant speed in the fixed frame
        tf2::doTransform(local_pose, local_pose_from_first, local_transform);
 
        odom.twist.twist.linear.x = local_pose_from_first.pose.position.x / dt;
        odom.twist.twist.linear.y = local_pose_from_first.pose.position.y / dt;
        odom.twist.twist.linear.z = local_pose_from_first.pose.position.z / dt;
 
        // Compute the angular instant speed
        tf2::Quaternion egomotion_orientation;
        tf2::Quaternion last_orientation;
        tf2::fromMsg(last_pose.orientation, last_orientation);
        egomotion_orientation = current_orientation * last_orientation.inverse();
        egomotion_orientation.normalize();
        tf2::Matrix3x3 m(egomotion_orientation);
        double roll = 0.0;
        double pitch = 0.0;
        double yaw = 0.0;
        m.getRPY(roll, pitch, yaw);
 
        odom.twist.twist.angular.x = roll / dt;
        odom.twist.twist.angular.y = pitch / dt;
        odom.twist.twist.angular.z = yaw / dt;
}
 
std::string get_object_class(uint32_t classId)
{
        std::string object_class("UNKNOWN");
 
        if (classId < k_object_class.size()) {
                object_class = k_object_class.at(classId);
        }
 
        return object_class;
}
 
void define_box_size(outsight_alb_driver::ObjectData &tracked, const Tlv::tlv_s *bBoxSizes, size_t objectIndex)
{
        float *bbox_size_raw = (float *)(bBoxSizes->getValue() + 3 * sizeof(float) * objectIndex);
 
        tracked.box_size.x = bbox_size_raw[0];
        tracked.box_size.y = bbox_size_raw[1];
        tracked.box_size.z = bbox_size_raw[2];
}
 
void define_box_pose(outsight_alb_driver::ObjectData &tracked, const Tlv::tlv_s *bBoxPoses, size_t objectIndex)
{
        std::array<float, 3> pose_translation(Pose::castTranslationValue(bBoxPoses, objectIndex));
        std::array<float, 9> pose_rotation(Pose::castRotationValue(bBoxPoses, objectIndex));
        define_pose(tracked.pose, pose_translation, pose_rotation);
}
 
void define_object_speed(outsight_alb_driver::ObjectData &tracked, const Tlv::tlv_s *objectSpeeds, size_t objectIndex)
{
        float *speed_raw = (float *)(objectSpeeds->getValue() + 3 * sizeof(float) * objectIndex);
 
        tracked.speed.linear.x = speed_raw[0];
        tracked.speed.linear.y = speed_raw[1];
        tracked.speed.linear.z = speed_raw[2];
}
 
void define_augmented_cloud(outsight_alb_driver::AugmentedCloud &message, const Tlv::tlv_s *frame)
{
        Tlv::Parser aug_cloud_parser(frame->getValue(), frame->getLength());
 
        const Tlv::tlv_s *number_object_tlv = aug_cloud_parser.findTlv(OSEF_TYPE_NUMBER_OF_POINTS);
        const Tlv::tlv_s *number_layer_tlv = aug_cloud_parser.findTlv(OSEF_TYPE_NUMBER_OF_LAYERS);
        message.number_of_points = NumberOfObjects::castValue(number_object_tlv);
        message.number_of_layers = NumberOfObjects::castValue(number_layer_tlv);
 
        const Tlv::tlv_s *reflectivities_tlv = aug_cloud_parser.findTlv(OSEF_TYPE_REFLECTIVITIES);
        if (reflectivities_tlv) {
                message.reflectivities = std::vector<uint8_t>(
                        reflectivities_tlv->getValue(), reflectivities_tlv->getValue() + message.number_of_points);
        }
 
        const Tlv::tlv_s *cartesian_tlv = aug_cloud_parser.findTlv(OSEF_TYPE_CARTESIAN_COORDINATES);
        if (cartesian_tlv) {
                float *cartesian = (float *)(cartesian_tlv->getValue());
                message.cartesian_coordinates = std::vector<float>(cartesian, cartesian + 3 * message.number_of_points);
        }
 
        const Tlv::tlv_s *object_ids_tlv = aug_cloud_parser.findTlv(OSEF_TYPE_OBJECT_ID_32_BITS);
        if (object_ids_tlv) {
                message.object_ids = ObjectIds32Bits::castValue(object_ids_tlv);
        }
}
} // namespace Helper