.. _program_listing_file__tmp_ws_src_locator_ros_bridge_bosch_locator_bridge_include_bosch_locator_bridge_rosmsgs_datagram_converter.hpp:

Program Listing for File rosmsgs_datagram_converter.hpp
=======================================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_locator_ros_bridge_bosch_locator_bridge_include_bosch_locator_bridge_rosmsgs_datagram_converter.hpp>` (``/tmp/ws/src/locator_ros_bridge/bosch_locator_bridge/include/bosch_locator_bridge/rosmsgs_datagram_converter.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   // 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_