landmark.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 /*
11  * File: landmark.cpp
12  * Author: Vladislav Tananaev
13  *
14  */
15 
16 #include <geometry_msgs/Pose.h>
17 #include "mrpt_bridge/time.h"
18 #include "mrpt_bridge/pose.h"
19 #include <mrpt_msgs/ObservationRangeBearing.h>
20 #include "tf/transform_datatypes.h"
22 #include "mrpt_bridge/landmark.h"
23 
24 #include <mrpt/version.h>
25 #include <mrpt/obs/CObservationBearingRange.h>
26 using namespace mrpt::obs;
27 
28 namespace mrpt_bridge
29 {
30 bool convert(
33 
34 {
35  // mrpt_bridge::convert(_msg.header.stamp, _obj.timestamp);
36  mrpt::poses::CPose3D cpose_obj;
37 
38  //_obj.stdError = _msg.sensor_std_range;
39  //_obj.sensorLabel = _msg.header.frame_id;
40  _obj.maxSensorDistance = _msg.max_sensor_distance;
41  _obj.minSensorDistance = _msg.min_sensor_distance;
42  _obj.sensor_std_yaw = _msg.sensor_std_yaw;
43  _obj.sensor_std_pitch = _msg.sensor_std_pitch;
44  _obj.sensor_std_range = _msg.sensor_std_range;
45 
46  if (_pose.empty())
47  {
48  convert(_msg.sensor_pose_on_robot, cpose_obj);
49  _obj.setSensorPose(cpose_obj);
50  }
51  else
52  {
53  _obj.setSensorPose(_pose);
54  }
55 
56  ASSERT_(_msg.sensed_data.size() >= 1);
57  const size_t N = _msg.sensed_data.size();
58 
59  _obj.sensedData.resize(N);
60 
61  for (std::size_t i_mrpt = 0; i_mrpt < N; i_mrpt++)
62  {
63  _obj.sensedData[i_mrpt].range = _msg.sensed_data[i_mrpt].range;
64  _obj.sensedData[i_mrpt].landmarkID = _msg.sensed_data[i_mrpt].id;
65  _obj.sensedData[i_mrpt].yaw = _msg.sensed_data[i_mrpt].yaw;
66  _obj.sensedData[i_mrpt].pitch = _msg.sensed_data[i_mrpt].pitch;
67  }
68  return true;
69 }
70 
71 bool convert(
72  const CObservationBearingRange& _obj,
74 {
75  mrpt::poses::CPose3D cpose_obj;
76 
77  // mrpt_bridge::convert(_obj.timestamp, _msg.header.stamp);
78  _obj.getSensorPose(cpose_obj);
79  convert(cpose_obj, _msg.sensor_pose_on_robot);
80 
81  _msg.max_sensor_distance = _obj.maxSensorDistance;
82  _msg.min_sensor_distance = _obj.minSensorDistance;
83  _msg.sensor_std_yaw = _obj.sensor_std_yaw;
84  _msg.sensor_std_pitch = _obj.sensor_std_pitch;
85  _msg.sensor_std_range = _obj.sensor_std_range;
86 
87  ASSERT_(_obj.sensedData.size() >= 1);
88  const size_t N = _obj.sensedData.size();
89 
90  _msg.sensed_data.resize(N);
91 
92  for (std::size_t i_msg = 0; i_msg < N; i_msg++)
93  {
94  _msg.sensed_data[i_msg].range = _obj.sensedData[i_msg].range;
95  _msg.sensed_data[i_msg].id = _obj.sensedData[i_msg].landmarkID;
96  _msg.sensed_data[i_msg].yaw = _obj.sensedData[i_msg].yaw;
97  _msg.sensed_data[i_msg].pitch = _obj.sensedData[i_msg].pitch;
98  }
99  return true;
100 }
101 
102 bool convert(
103  const CObservationBearingRange& _obj,
105 {
106  convert(_obj, _msg);
108  _obj.getSensorPose(pose);
109  convert(pose, _pose);
110  return true;
111 }
112 } // namespace mrpt_bridge
#define N(a, b, c)
File includes methods for converting CNetworkOfPoses*DInf <=> NetworkOfPoses message types...
bool convert(const mrpt_msgs::ObservationRangeBeacon &_msg, const mrpt::poses::CPose3D &_pose, mrpt::obs::CObservationBeaconRanges &_obj)
Definition: beacon.cpp:24
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose) MRPT_OVERRIDE
#define ASSERT_(f)
static bool empty()
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const MRPT_OVERRIDE


mrpt_bridge
Author(s): Markus Bader , Raphael Zack
autogenerated on Fri Feb 28 2020 03:22:14