.. _program_listing_file__tmp_ws_src_gazebo_ros_pkgs_gazebo_ros_include_gazebo_ros_conversions_sensor_msgs.hpp:

Program Listing for File sensor_msgs.hpp
========================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_gazebo_ros_pkgs_gazebo_ros_include_gazebo_ros_conversions_sensor_msgs.hpp>` (``/tmp/ws/src/gazebo_ros_pkgs/gazebo_ros/include/gazebo_ros/conversions/sensor_msgs.hpp``)

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

.. code-block:: cpp

   // Copyright 2018 Open Source Robotics Foundation, Inc.
   //
   // 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 GAZEBO_ROS__CONVERSIONS__SENSOR_MSGS_HPP_
   #define GAZEBO_ROS__CONVERSIONS__SENSOR_MSGS_HPP_
   
   #include <math.h>
   
   #include <gazebo/msgs/laserscan_stamped.pb.h>
   #include <geometry_msgs/msg/point32.hpp>
   #include <rclcpp/logging.hpp>
   #include <sensor_msgs/msg/point_cloud2.hpp>
   #include <sensor_msgs/msg/point_cloud.hpp>
   #include <sensor_msgs/msg/laser_scan.hpp>
   #include <sensor_msgs/msg/range.hpp>
   #include <sensor_msgs/point_cloud2_iterator.hpp>
   
   #include <algorithm>
   #include <limits>
   
   #include "gazebo_ros/conversions/builtin_interfaces.hpp"
   #include "gazebo_ros/conversions/generic.hpp"
   
   namespace gazebo_ros
   {
   template<class T>
   inline
   T Convert(const gazebo::msgs::LaserScanStamped &, double min_intensity = 0.0)
   {
     (void)min_intensity;
     T::ConversionNotImplemented;
   }
   
   template<>
   inline
   sensor_msgs::msg::LaserScan Convert(const gazebo::msgs::LaserScanStamped & in, double min_intensity)
   {
     sensor_msgs::msg::LaserScan ls;
     ls.header.stamp = Convert<builtin_interfaces::msg::Time>(in.time());
     ls.angle_min = in.scan().angle_min();
     ls.angle_max = in.scan().angle_max();
     ls.angle_increment = in.scan().angle_step();
     ls.time_increment = 0;
     ls.scan_time = 0;
     ls.range_min = in.scan().range_min();
     ls.range_max = in.scan().range_max();
   
     auto count = in.scan().count();
     auto vertical_count = in.scan().vertical_count();
   
     // If there are multiple vertical beams, use the one in the middle
     size_t start = (vertical_count / 2) * count;
   
     // Copy ranges into ROS message
     ls.ranges.resize(count);
     std::copy(
       in.scan().ranges().begin() + start,
       in.scan().ranges().begin() + start + count,
       ls.ranges.begin());
   
     // Copy intensities into ROS message, clipping at min_intensity
     ls.intensities.resize(count);
     std::transform(
       in.scan().intensities().begin() + start,
       in.scan().intensities().begin() + start + count,
       ls.intensities.begin(), [min_intensity](double i) -> double {
         return i > min_intensity ? i : min_intensity;
       });
   
     return ls;
   }
   
   template<>
   inline
   sensor_msgs::msg::PointCloud Convert(
     const gazebo::msgs::LaserScanStamped & in,
     double min_intensity)
   {
     // Create message to send
     sensor_msgs::msg::PointCloud pc;
   
     // Fill header
     pc.header.stamp = Convert<builtin_interfaces::msg::Time>(in.time());
   
     // Cache values that are repeatedly used
     auto count = in.scan().count();
     auto vertical_count = in.scan().vertical_count();
     auto angle_step = in.scan().angle_step();
     auto vertical_angle_step = in.scan().vertical_angle_step();
   
     // Gazebo sends an infinite vertical step if the number of samples is 1
     // Surprisingly, not setting the <vertical> tag results in nan instead of inf, which is ok
     if (std::isinf(vertical_angle_step)) {
       RCLCPP_WARN_ONCE(conversions_logger, "Infinite angle step results in wrong PointCloud");
     }
   
     // Setup point cloud fields
     pc.points.reserve(count * vertical_count);
     pc.channels.push_back(sensor_msgs::msg::ChannelFloat32());
     pc.channels[0].values.reserve(count * vertical_count);
     pc.channels[0].name = "intensity";
   
     // Iterators to range and intensities
     auto range_iter = in.scan().ranges().begin();
     auto intensity_iter = in.scan().intensities().begin();
   
     // Angles of ray currently processing, azimuth is horizontal, inclination is vertical
     double azimuth, inclination;
   
     // Index in vertical and horizontal loops
     size_t i, j;
   
     // Fill pointcloud with laser scan data, converting spherical to Cartesian
     for (j = 0, inclination = in.scan().vertical_angle_min();
       j < vertical_count;
       ++j, inclination += vertical_angle_step)
     {
       double c_inclination = cos(inclination);
       double s_inclination = sin(inclination);
       for (i = 0, azimuth = in.scan().angle_min();
         i < count;
         ++i, azimuth += angle_step, ++range_iter, ++intensity_iter)
       {
         double c_azimuth = cos(azimuth);
         double s_azimuth = sin(azimuth);
   
         double r = *range_iter;
         // Skip NaN / inf points
         if (!std::isfinite(r)) {
           continue;
         }
   
         // Get intensity, clipping at min_intensity
         double intensity = *intensity_iter;
         if (intensity < min_intensity) {
           intensity = min_intensity;
         }
   
         // Convert spherical coordinates to Cartesian for pointcloud
         // See https://en.wikipedia.org/wiki/Spherical_coordinate_system
         geometry_msgs::msg::Point32 point;
         point.x = r * c_inclination * c_azimuth;
         point.y = r * c_inclination * s_azimuth;
         point.z = r * s_inclination;
         pc.points.push_back(point);
         pc.channels[0].values.push_back(intensity);
       }
     }
   
     return pc;
   }
   
   template<>
   inline
   sensor_msgs::msg::PointCloud2 Convert(
     const gazebo::msgs::LaserScanStamped & in,
     double min_intensity)
   {
     // Create message to send
     sensor_msgs::msg::PointCloud2 pc;
   
     // Pointcloud will be dense, unordered
     pc.height = 1;
     pc.is_dense = true;
   
     // Fill header
     pc.header.stamp = Convert<builtin_interfaces::msg::Time>(in.time());
   
     // Cache values that are repeatedly used
     auto count = in.scan().count();
     auto vertical_count = in.scan().vertical_count();
     auto angle_step = in.scan().angle_step();
     auto vertical_angle_step = in.scan().vertical_angle_step();
   
     // Gazebo sends an infinite vertical step if the number of samples is 1
     // Surprisingly, not setting the <vertical> tag results in nan instead of inf, which is ok
     if (std::isinf(vertical_angle_step)) {
       RCLCPP_WARN_ONCE(conversions_logger, "Infinite angle step results in wrong PointCloud2");
     }
   
     // Create fields in pointcloud
     sensor_msgs::PointCloud2Modifier pcd_modifier(pc);
     pcd_modifier.setPointCloud2Fields(
       4,
       "x", 1, sensor_msgs::msg::PointField::FLOAT32,
       "y", 1, sensor_msgs::msg::PointField::FLOAT32,
       "z", 1, sensor_msgs::msg::PointField::FLOAT32,
       "intensity", 1, sensor_msgs::msg::PointField::FLOAT32);
     pcd_modifier.resize(vertical_count * count);
     sensor_msgs::PointCloud2Iterator<float> iter_x(pc, "x");
     sensor_msgs::PointCloud2Iterator<float> iter_y(pc, "y");
     sensor_msgs::PointCloud2Iterator<float> iter_z(pc, "z");
     sensor_msgs::PointCloud2Iterator<float> iter_intensity(pc, "intensity");
   
     // Iterators to range and intensities
     auto range_iter = in.scan().ranges().begin();
     auto intensity_iter = in.scan().intensities().begin();
   
     // Number of points actually added
     size_t points_added = 0;
   
     // Angles of ray currently processing, azimuth is horizontal, inclination is vertical
     double azimuth, inclination;
   
     // Index in vertical and horizontal loops
     size_t i, j;
   
     // Fill pointcloud with laser scan data, converting spherical to Cartesian
     for (j = 0, inclination = in.scan().vertical_angle_min();
       j < vertical_count;
       ++j, inclination += vertical_angle_step)
     {
       double c_inclination = cos(inclination);
       double s_inclination = sin(inclination);
       for (i = 0, azimuth = in.scan().angle_min();
         i < count;
         ++i, azimuth += angle_step, ++range_iter, ++intensity_iter)
       {
         double c_azimuth = cos(azimuth);
         double s_azimuth = sin(azimuth);
   
         double r = *range_iter;
         // Skip NaN / inf points
         if (!std::isfinite(r)) {
           continue;
         }
   
         // Get intensity, clipping at min_intensity
         double intensity = *intensity_iter;
         if (intensity < min_intensity) {
           intensity = min_intensity;
         }
   
         // Convert spherical coordinates to Cartesian for pointcloud
         // See https://en.wikipedia.org/wiki/Spherical_coordinate_system
         *iter_x = r * c_inclination * c_azimuth;
         *iter_y = r * c_inclination * s_azimuth;
         *iter_z = r * s_inclination;
         *iter_intensity = intensity;
   
         // Increment ouput iterators
         ++points_added;
         ++iter_x;
         ++iter_y;
         ++iter_z;
         ++iter_intensity;
       }
     }
   
     pcd_modifier.resize(points_added);
   
     return pc;
   }
   
   template<>
   inline
   sensor_msgs::msg::Range Convert(const gazebo::msgs::LaserScanStamped & in, double min_intensity)
   {
     (void) min_intensity;
   
     // Create message
     sensor_msgs::msg::Range range_msg;
   
     // Set stamp from header
     range_msg.header.stamp = Convert<builtin_interfaces::msg::Time>(in.time());
   
     double horizontal_fov = in.scan().angle_max() - in.scan().angle_min();
     double vertical_fov = in.scan().vertical_angle_max() - in.scan().vertical_angle_min();
     range_msg.field_of_view = std::max(horizontal_fov, vertical_fov);
     range_msg.min_range = in.scan().range_min();
     range_msg.max_range = in.scan().range_max();
   
     // Set range to the minimum of the ray ranges
     // For single rays, this will just be the range of the ray
     range_msg.range = std::numeric_limits<sensor_msgs::msg::Range::_range_type>::max();
     for (double range : in.scan().ranges()) {
       if (range < range_msg.range) {
         range_msg.range = range;
       }
     }
     return range_msg;
   }
   
   }  // namespace gazebo_ros
   #endif  // GAZEBO_ROS__CONVERSIONS__SENSOR_MSGS_HPP_