18 #include <mrpt/ros1bridge/time.h> 19 #include <marker_msgs/MarkerDetection.h> 20 #include <mrpt/obs/CObservationBearingRange.h> 29 mrpt::obs::CObservationBearingRange& des)
31 des.timestamp = mrpt::ros1bridge::fromROS(src.header.stamp);
32 des.setSensorPose(pose);
33 des.minSensorDistance = src.distance_min;
34 des.maxSensorDistance = src.distance_max;
36 des.sensedData.resize(src.markers.size());
37 for (
size_t i = 0; i < src.markers.size(); i++)
39 const marker_msgs::Marker& marker = src.markers[i];
40 mrpt::obs::CObservationBearingRange::TMeasurement& measurment =
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)
49 measurment.landmarkID = marker.ids[0];
53 measurment.landmarkID = -1;
60 mrpt::obs::CObservationBeaconRanges& des)
62 des.timestamp = mrpt::ros1bridge::fromROS(src.header.stamp);
64 des.setSensorPose(pose);
65 des.minSensorDistance = src.distance_min;
66 des.maxSensorDistance = src.distance_max;
68 des.sensedData.resize(src.markers.size());
69 for (
size_t i = 0; i < src.markers.size(); i++)
71 const marker_msgs::Marker& marker = src.markers[i];
72 mrpt::obs::CObservationBeaconRanges::TMeasurement& measurment =
74 measurment.sensedDistance = sqrt(
75 marker.pose.position.x * marker.pose.position.x +
76 marker.pose.position.y * marker.pose.position.y);
77 measurment.sensorLocationOnRobot.m_coords = pose.m_coords;
78 if (marker.ids.size() > 0)
80 measurment.beaconID = marker.ids[0];
84 measurment.beaconID = -1;
Funtions to convert marker_msgs to mrpt msgs.
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)