marker_msgs.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 
18 #include "mrpt_bridge/time.h"
19 #include <marker_msgs/MarkerDetection.h>
20 #include <mrpt/obs/CObservationBearingRange.h>
21 
22 #include <iostream>
23 using namespace std;
24 
25 namespace mrpt_bridge
26 {
27 bool convert(
30 {
31  convert(src.header.stamp, des.timestamp);
32  des.setSensorPose(pose);
33  des.minSensorDistance = src.distance_min;
34  des.maxSensorDistance = src.distance_max;
35 
36  des.sensedData.resize(src.markers.size());
37  for (size_t i = 0; i < src.markers.size(); i++)
38  {
39  const marker_msgs::Marker& marker = src.markers[i];
41  des.sensedData[i];
42  measurment.range = sqrt(
43  marker.pose.position.x * marker.pose.position.x +
44  marker.pose.position.y * marker.pose.position.y);
45  measurment.yaw = atan2(marker.pose.position.y, marker.pose.position.x);
46  measurment.pitch = 0.0;
47  if (marker.ids.size() > 0)
48  {
49  measurment.landmarkID = marker.ids[0];
50  }
51  else
52  {
53  measurment.landmarkID = -1;
54  }
55  }
56  return true;
57 };
58 bool convert(
61 {
62  convert(src.header.stamp, des.timestamp);
63  des.setSensorPose(pose);
64  des.minSensorDistance = src.distance_min;
65  des.maxSensorDistance = src.distance_max;
66 
67  des.sensedData.resize(src.markers.size());
68  for (size_t i = 0; i < src.markers.size(); i++)
69  {
70  const marker_msgs::Marker& marker = src.markers[i];
72  des.sensedData[i];
73  measurment.sensedDistance = sqrt(
74  marker.pose.position.x * marker.pose.position.x +
75  marker.pose.position.y * marker.pose.position.y);
76  measurment.sensorLocationOnRobot.m_coords = pose.m_coords;
77  if (marker.ids.size() > 0)
78  {
79  measurment.beaconID = marker.ids[0];
80  }
81  else
82  {
83  measurment.beaconID = -1;
84  }
85  }
86  return true;
87 };
88 } // namespace mrpt_bridge
std::deque< TMeasurement > sensedData
mrpt::system::TTimeStamp timestamp
GLuint src
Funtions to convert marker_msgs to mrpt msgs.
int marker
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
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose) MRPT_OVERRIDE
mrpt::math::CArrayDouble< 3 > m_coords
mrpt::math::CArrayDouble< 3 > m_coords
T atan2(const T v0, const T v1)


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