zoneset_configuration_ros_conversion.h
Go to the documentation of this file.
1 // Copyright (c) 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_ZONESET_CONFIGURATION_ROS_CONVERSION_H
17 #define PSEN_SCAN_V2_ZONESET_CONFIGURATION_ROS_CONVERSION_H
18 
19 #include <ros/console.h>
20 
21 #include <geometry_msgs/Polygon.h>
22 
24 #include "psen_scan_v2/ZoneSet.h"
25 #include "psen_scan_v2/ZoneSetConfiguration.h"
30 
34 
35 static inline double deg_to_rad(double deg)
36 {
37  return deg * M_PI / 180.0;
38 }
39 
40 geometry_msgs::Polygon fromPolar(const std::vector<unsigned long>& radii_in_mm,
41  const TenthOfDegree& phi_step,
42  const double& x_axis_rotation)
43 {
44  geometry_msgs::Polygon polygon;
45  size_t i = 0;
46  for (const auto& r : radii_in_mm)
47  {
48  geometry_msgs::Point32 point;
49 
50  const auto angle = phi_step.toRad() * i - x_axis_rotation;
51  point.x = (r / 1000.) * std::cos(angle);
52  point.y = (r / 1000.) * std::sin(angle);
53  point.z = 0;
54 
55  polygon.points.push_back(point);
56 
57  ++i;
58  }
59 
60  return polygon;
61 }
62 
63 psen_scan_v2::ZoneSet toRosMsg(const ZoneSetStandalone& zoneset,
64  const std::string& frame_id,
65  const ros::Time& stamp = ros::Time::now())
66 {
67  psen_scan_v2::ZoneSetMsgBuilder zoneset_msg_builder;
68  zoneset_msg_builder.headerStamp(stamp)
69  .headerFrameId(frame_id)
70  .safety1(fromPolar( // LCOV_EXCL_LINE gcov bug?
71  zoneset.safety1_,
72  zoneset.resolution_,
80 
81  if (zoneset.speed_range_)
82  {
83  zoneset_msg_builder.speedLower(zoneset.speed_range_->min_).speedUpper(zoneset.speed_range_->max_);
84  }
85 
86  return zoneset_msg_builder.build();
87 }
88 
89 psen_scan_v2::ZoneSetConfiguration toRosMsg(const ZoneSetConfigurationStandalone& zoneset_configuration,
90  const std::string& frame_id,
91  const ros::Time& stamp = ros::Time::now())
92 {
93  psen_scan_v2::ZoneSetConfiguration zoneset_config_msg;
94 
95  for (const auto& z : zoneset_configuration.zonesets_)
96  {
97  zoneset_config_msg.zonesets.push_back(toRosMsg(z, frame_id, stamp));
98  }
99  return zoneset_config_msg;
100 }
101 
102 #endif // PSEN_SCAN_V2_ZONESET_CONFIGURATION_ROS_CONVERSION_H
ZoneSetMsgBuilder & headerFrameId(const std::string &header_frame_id)
ZoneSetMsgBuilder & muting2(const geometry_msgs::Polygon &muting2)
ZoneSetMsgBuilder & safety2(const geometry_msgs::Polygon &safety2)
ZoneSetMsgBuilder & headerStamp(const ros::Time &header_stamp)
std::vector< unsigned long > safety3_
Definition: zoneset.h:76
ZoneSetMsgBuilder & speedUpper(const float &speed_upper)
std::vector< unsigned long > safety2_
Definition: zoneset.h:75
ZoneSetMsgBuilder & safety1(const geometry_msgs::Polygon &safety1)
boost::optional< ZoneSetSpeedRange > speed_range_
Definition: zoneset.h:86
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
psen_scan_v2_standalone::util::TenthOfDegree TenthOfDegree
ZoneSetMsgBuilder & speedLower(const float &speed_lower)
ZoneSetMsgBuilder & muting1(const geometry_msgs::Polygon &muting1)
ZoneSetMsgBuilder & warn2(const geometry_msgs::Polygon &warn2)
static constexpr double DEFAULT_X_AXIS_ROTATION
The 2D scan will be rotated around the z-axis.
std::vector< unsigned long > safety1_
Definition: zoneset.h:74
std::vector< unsigned long > warn1_
Definition: zoneset.h:77
std::vector< unsigned long > muting1_
Definition: zoneset.h:79
A set of simultanously active zones.
Definition: zoneset.h:71
std::vector< unsigned long > muting2_
Definition: zoneset.h:80
static double deg_to_rad(double deg)
ZoneSetMsgBuilder & safety3(const geometry_msgs::Polygon &safety3)
psen_scan_v2::ZoneSet toRosMsg(const ZoneSetStandalone &zoneset, const std::string &frame_id, const ros::Time &stamp=ros::Time::now())
ZoneSetMsgBuilder & warn1(const geometry_msgs::Polygon &warn1)
std::vector< unsigned long > warn2_
Definition: zoneset.h:78
static Time now()
r
geometry_msgs::Polygon fromPolar(const std::vector< unsigned long > &radii_in_mm, const TenthOfDegree &phi_step, const double &x_axis_rotation)
Helper class representing angles in tenth of degree.


psen_scan_v2
Author(s): Pilz GmbH + Co. KG
autogenerated on Sat Nov 5 2022 02:13:36