laserscan_ros_conversions.h
Go to the documentation of this file.
1 // Copyright (c) 2020-2021 Pilz GmbH & Co. KG
2 //
3 // This program is free software: you can redistribute it and/or modify
4 // it under the terms of the GNU Lesser General Public License as published by
5 // the Free Software Foundation, either version 3 of the License, or
6 // (at your option) any later version.
7 //
8 // This program is distributed in the hope that it will be useful,
9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 // GNU Lesser General Public License for more details.
12 //
13 // You should have received a copy of the GNU Lesser General Public License
14 // along with this program. If not, see <https://www.gnu.org/licenses/>.
15 
16 #ifndef PSEN_SCAN_V2_LASERSCAN_ROS_CONVERSIONS_H
17 #define PSEN_SCAN_V2_LASERSCAN_ROS_CONVERSIONS_H
18 
19 #include <sensor_msgs/LaserScan.h>
20 
23 
24 namespace psen_scan_v2
25 {
26 using namespace psen_scan_v2_standalone;
27 
28 sensor_msgs::LaserScan toLaserScanMsg(const LaserScan& laserscan,
29  const std::string& frame_id,
30  const double x_axis_rotation)
31 {
32  sensor_msgs::LaserScan ros_message;
33  if (laserscan.timestamp() < 0)
34  {
35  throw std::invalid_argument("Laserscan message has an invalid timestamp: " + std::to_string(laserscan.timestamp()));
36  }
37  ros_message.header.stamp = ros::Time{}.fromNSec(laserscan.timestamp());
38  ros_message.header.frame_id = frame_id;
39  ros_message.angle_min = laserscan.minScanAngle().toRad() - x_axis_rotation;
40  ros_message.angle_max = laserscan.maxScanAngle().toRad() - x_axis_rotation;
41  ros_message.angle_increment = laserscan.scanResolution().toRad();
42 
43  ros_message.time_increment = configuration::TIME_PER_SCAN_IN_S / (2 * M_PI) * laserscan.scanResolution().toRad();
44 
45  ros_message.scan_time = configuration::TIME_PER_SCAN_IN_S;
46  ros_message.range_min = configuration::RANGE_MIN_IN_M;
47  ros_message.range_max = configuration::RANGE_MAX_IN_M;
48 
49  ros_message.ranges.insert(
50  ros_message.ranges.begin(), laserscan.measurements().cbegin(), laserscan.measurements().cend());
51 
52  ros_message.intensities.insert(
53  ros_message.intensities.begin(), laserscan.intensities().cbegin(), laserscan.intensities().cend());
54 
55  return ros_message;
56 }
57 
58 } // namespace psen_scan_v2
59 
60 #endif // PSEN_SCAN_V2_LASERSCAN_ROS_CONVERSIONS_H
psen_scan_v2_standalone::configuration::TIME_PER_SCAN_IN_S
static constexpr double TIME_PER_SCAN_IN_S
Definition: default_parameters.h:45
psen_scan_v2_standalone::LaserScan::maxScanAngle
const util::TenthOfDegree & maxScanAngle() const
Definition: laserscan.cpp:131
psen_scan_v2_standalone::configuration::RANGE_MAX_IN_M
static constexpr double RANGE_MAX_IN_M
Definition: default_parameters.h:48
psen_scan_v2_standalone::LaserScan::scanResolution
const util::TenthOfDegree & scanResolution() const
Definition: laserscan.cpp:64
psen_scan_v2
Root namespace for the ROS part.
Definition: active_zoneset_node.h:29
psen_scan_v2::toLaserScanMsg
sensor_msgs::LaserScan toLaserScanMsg(const LaserScan &laserscan, const std::string &frame_id, const double x_axis_rotation)
Definition: laserscan_ros_conversions.h:28
psen_scan_v2_standalone::LaserScan::intensities
const IntensityData & intensities() const
Definition: laserscan.cpp:171
TimeBase< Time, Duration >::fromNSec
Time & fromNSec(uint64_t t)
ros::Time
default_parameters.h
psen_scan_v2_standalone::util::TenthOfDegree::toRad
constexpr double toRad() const
Definition: tenth_of_degree.h:54
psen_scan_v2_standalone::LaserScan::timestamp
int64_t timestamp() const
Definition: laserscan.cpp:156
psen_scan_v2_standalone::LaserScan
This class represents a single laser scan in the <tf_prefix> target frame.
Definition: laserscan.h:47
psen_scan_v2_standalone
Root namespace in which the software components to communicate with the scanner (firmware-version: 2)...
Definition: udp_client.h:41
psen_scan_v2_standalone::LaserScan::measurements
const MeasurementData & measurements() const
Definition: laserscan.cpp:141
laserscan.h
psen_scan_v2_standalone::configuration::RANGE_MIN_IN_M
static constexpr double RANGE_MIN_IN_M
Definition: default_parameters.h:47
psen_scan_v2_standalone::LaserScan::minScanAngle
const util::TenthOfDegree & minScanAngle() const
Definition: laserscan.cpp:126


psen_scan_v2
Author(s): Pilz GmbH + Co. KG
autogenerated on Sat Jun 22 2024 02:46:11