beacon.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <geometry_msgs/Pose.h>
11 #include "mrpt_bridge/time.h"
12 #include "mrpt_bridge/pose.h"
13 #include <mrpt_msgs/ObservationRangeBeacon.h>
14 #include "tf/transform_datatypes.h"
16 #include "mrpt_bridge/beacon.h"
17 
18 #include <mrpt/version.h>
19 #include <mrpt/obs/CObservationBeaconRanges.h>
20 using namespace mrpt::obs;
21 
22 namespace mrpt_bridge
23 {
24 bool convert(
26  const mrpt::poses::CPose3D& _pose, CObservationBeaconRanges& _obj)
27 {
28  mrpt_bridge::convert(_msg.header.stamp, _obj.timestamp);
29  mrpt::poses::CPose3D cpose_obj;
30 
31  _obj.stdError = _msg.sensor_std_range;
32  _obj.sensorLabel = _msg.header.frame_id;
33  _obj.maxSensorDistance = _msg.max_sensor_distance;
34  _obj.minSensorDistance = _msg.min_sensor_distance;
35 
36  if (_pose.empty())
37  {
38  convert(_msg.sensor_pose_on_robot, cpose_obj);
39  _obj.setSensorPose(cpose_obj);
40  }
41  else
42  {
43  _obj.setSensorPose(_pose);
44  }
45 
46  ASSERT_(_msg.sensed_data.size() >= 1);
47  const size_t N = _msg.sensed_data.size();
48 
49  _obj.sensedData.resize(N);
50 
51  for (std::size_t i_mrpt = 0; i_mrpt < N; i_mrpt++)
52  {
53  _obj.sensedData[i_mrpt].sensedDistance = _msg.sensed_data[i_mrpt].range;
54  _obj.sensedData[i_mrpt].beaconID = _msg.sensed_data[i_mrpt].id;
55  }
56  return true;
57 }
58 
59 bool convert(
60  const CObservationBeaconRanges& _obj,
62 {
63  mrpt::poses::CPose3D cpose_obj;
64 
65  mrpt_bridge::convert(_obj.timestamp, _msg.header.stamp);
66  _obj.getSensorPose(cpose_obj);
67  convert(cpose_obj, _msg.sensor_pose_on_robot);
68 
69  _msg.sensor_std_range = _obj.stdError;
70  _msg.header.frame_id = _obj.sensorLabel;
71  _msg.max_sensor_distance = _obj.maxSensorDistance;
72  _msg.min_sensor_distance = _obj.minSensorDistance;
73 
74  ASSERT_(_obj.sensedData.size() >= 1);
75  const size_t N = _obj.sensedData.size();
76 
77  _msg.sensed_data.resize(N);
78 
79  for (std::size_t i_msg = 0; i_msg < N; i_msg++)
80  {
81  _msg.sensed_data[i_msg].range = _obj.sensedData[i_msg].sensedDistance;
82  _msg.sensed_data[i_msg].id = _obj.sensedData[i_msg].beaconID;
83  }
84  return true;
85 }
86 
87 bool convert(
88  const CObservationBeaconRanges& _obj,
90 {
91  convert(_obj, _msg);
92  mrpt::poses::CPose3D pose;
93  _obj.getSensorPose(pose);
94  convert(pose, _pose);
95  return true;
96 }
97 } // namespace mrpt_bridge
pose.h
mrpt_msgs::ObservationRangeBeacon_
Definition: include/mrpt_bridge/beacon.h:31
mrpt::obs
Definition: include/mrpt_bridge/beacon.h:46
geometry_msgs::Pose_
Definition: include/mrpt_bridge/beacon.h:24
time.h
mrpt_bridge::convert
bool convert(const CObservationBeaconRanges &_obj, mrpt_msgs::ObservationRangeBeacon &_msg, geometry_msgs::Pose &_pose)
Definition: beacon.cpp:87
beacon.h
mrpt_bridge::convert
bool convert(const mrpt_msgs::ObservationRangeBeacon &_msg, const mrpt::poses::CPose3D &_pose, mrpt::obs::CObservationBeaconRanges &_obj)
transform_datatypes.h
mrpt_bridge
File includes methods for converting CNetworkOfPoses*DInf <=> NetworkOfPoses message types.
Definition: include/mrpt_bridge/beacon.h:52
Matrix3x3.h


mrpt_bridge
Author(s): Markus Bader , Raphael Zack
autogenerated on Sun Mar 6 2022 03:48:10