Program Listing for File rosmsgs_datagram_converter.hpp

↰ Return to documentation for file (/tmp/ws/src/locator_ros_bridge/bosch_locator_bridge/include/bosch_locator_bridge/rosmsgs_datagram_converter.hpp)

// Copyright (c) 2021 - for information on the respective copyright owner
// see the NOTICE file and/or the repository https://github.com/boschglobal/locator_ros_bridge.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef BOSCH_LOCATOR_BRIDGE__ROSMSGS_DATAGRAM_CONVERTER_HPP_
#define BOSCH_LOCATOR_BRIDGE__ROSMSGS_DATAGRAM_CONVERTER_HPP_

#include <Poco/BinaryReader.h>
#include <Poco/JSON/Object.h>

#include <algorithm>
#include <iostream>
#include <vector>

#include "geometry_msgs/msg/pose_array.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose2_d.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"

#include "bosch_locator_bridge/msg/client_control_mode.hpp"
#include "bosch_locator_bridge/msg/client_global_align_visualization.hpp"
#include "bosch_locator_bridge/msg/client_map_visualization.hpp"
#include "bosch_locator_bridge/msg/client_localization_pose.hpp"
#include "bosch_locator_bridge/msg/client_localization_visualization.hpp"
#include "bosch_locator_bridge/msg/client_recording_visualization.hpp"

#define MAP_FRAME_ID "map"
#define ODOM_FRAME_ID "odom"

class RosMsgsDatagramConverter
{
public:
  static size_t convertClientControlMode2Message(
    const std::vector<char> & datagram, const rclcpp::Time & stamp,
    bosch_locator_bridge::msg::ClientControlMode & client_control_mode);

  static size_t convertMapDatagram2Message(
    const std::vector<char> & datagram, const rclcpp::Time & stamp,
    sensor_msgs::msg::PointCloud2 & out_pointcloud);

  static size_t convertClientGlobalAlignVisualizationDatagram2Message(
    const std::vector<char> & datagram,
    bosch_locator_bridge::msg::ClientGlobalAlignVisualization & client_global_align_visualization,
    geometry_msgs::msg::PoseArray & poses, geometry_msgs::msg::PoseArray & landmark_poses);

  static size_t convertClientLocalizationPoseDatagram2Message(
    const std::vector<char> & datagram,
    bosch_locator_bridge::msg::ClientLocalizationPose & client_localization_pose,
    geometry_msgs::msg::PoseStamped & pose, double covariance[6],
    geometry_msgs::msg::PoseStamped & lidar_odo_pose);

  static size_t convertClientLocalizationVisualizationDatagram2Message(
    const std::vector<char> & datagram,
    bosch_locator_bridge::msg::ClientLocalizationVisualization & client_localization_visualization,
    geometry_msgs::msg::PoseStamped & pose, sensor_msgs::msg::PointCloud2 & scan);

  static size_t convertClientMapVisualizationDatagram2Message(
    const std::vector<char> & datagram,
    bosch_locator_bridge::msg::ClientMapVisualization & client_map_visualization,
    geometry_msgs::msg::PoseStamped & pose, sensor_msgs::msg::PointCloud2 & scan,
    geometry_msgs::msg::PoseArray & path_poses);

  static size_t convertClientRecordingVisualizationDatagram2Message(
    const std::vector<char> & datagram,
    bosch_locator_bridge::msg::ClientRecordingVisualization & client_recording_visualization,
    geometry_msgs::msg::PoseStamped & pose, sensor_msgs::msg::PointCloud2 & scan,
    geometry_msgs::msg::PoseArray & path_poses);

  static size_t convertPose2DDoubleDatagram2Message(
    Poco::BinaryReader & binary_reader,
    geometry_msgs::msg::Pose & pose);

  static size_t convertPose2DSingleDatagram2Message(
    Poco::BinaryReader & binary_reader,
    geometry_msgs::msg::Pose & pose);

  static Poco::Buffer<char> convertLaserScan2DataGram(
    const sensor_msgs::msg::LaserScan::SharedPtr msg, size_t scan_num,
    rclcpp::Node::SharedPtr node);

  static Poco::Buffer<char> convertOdometry2DataGram(
    const nav_msgs::msg::Odometry::SharedPtr msg, size_t odom_num, rclcpp::Node::SharedPtr node);

  static Poco::JSON::Object makePose2d(const geometry_msgs::msg::Pose2D & pose);

private:
  static size_t convertMapDatagram2Message(
    Poco::BinaryReader & binary_reader, const rclcpp::Time & stamp,
    sensor_msgs::msg::PointCloud2 & out_pointcloud);
  static size_t convertMapDatagram2PointCloud(
    Poco::BinaryReader & binary_reader,
    pcl::PointCloud<pcl::PointXYZRGB> & out_pointcloud);
  static void colorizePointCloud(
    pcl::PointCloud<pcl::PointXYZRGB> & point_cloud,
    const std::vector<uint64_t> & sensor_offsets);
  static size_t discardExtension(Poco::BinaryReader & binary_reader);
  static void readIntensities(Poco::BinaryReader & binary_reader);
  static std::vector<uint64_t> readSensorOffsets(Poco::BinaryReader & binary_reader);

  static float clamp_range(float r, float min, float max)
  {
    return std::min({std::max({r, min}), max});
  }
};

#endif  // BOSCH_LOCATOR_BRIDGE__ROSMSGS_DATAGRAM_CONVERTER_HPP_