Go to the documentation of this file.00001
00009 #include "mrpt_bridge/marker_msgs.h"
00010 #include "mrpt_bridge/time.h"
00011 #include <marker_msgs/MarkerDetection.h>
00012 #include <mrpt/obs/CObservationBearingRange.h>
00013
00014
00015
00016 #include <iostream>
00017 using namespace std;
00018
00019
00020 namespace mrpt_bridge
00021 {
00022 bool convert(const marker_msgs::MarkerDetection& src, const mrpt::poses::CPose3D& pose, mrpt::obs::CObservationBearingRange& des) {
00023
00024 convert(src.header.stamp, des.timestamp);
00025 des.setSensorPose(pose);
00026 des.minSensorDistance = src.distance_min;
00027 des.maxSensorDistance = src.distance_max;
00028
00029 des.sensedData.resize(src.markers.size());
00030 for(size_t i = 0; i < src.markers.size(); i++) {
00031 const marker_msgs::Marker &marker = src.markers[i];
00032 mrpt::obs::CObservationBearingRange::TMeasurement &measurment = des.sensedData[i];
00033 measurment.range = sqrt(marker.pose.position.x * marker.pose.position.x + marker.pose.position.y * marker.pose.position.y);
00034 measurment.yaw = atan2(marker.pose.position.y, marker.pose.position.x);
00035 measurment.pitch = 0.0;
00036 if(marker.ids.size() > 0) {
00037 measurment.landmarkID = marker.ids[0];
00038 } else {
00039 measurment.landmarkID = -1;
00040 }
00041 }
00042 return true;
00043 };
00044 bool convert(const marker_msgs::MarkerDetection& src, const mrpt::poses::CPose3D& pose, mrpt::obs::CObservationBeaconRanges& des) {
00045
00046 convert(src.header.stamp, des.timestamp);
00047 des.setSensorPose(pose);
00048 des.minSensorDistance = src.distance_min;
00049 des.maxSensorDistance = src.distance_max;
00050
00051 des.sensedData.resize(src.markers.size());
00052 for(size_t i = 0; i < src.markers.size(); i++) {
00053 const marker_msgs::Marker &marker = src.markers[i];
00054 mrpt::obs::CObservationBeaconRanges::TMeasurement &measurment = des.sensedData[i];
00055 measurment.sensedDistance = sqrt(marker.pose.position.x * marker.pose.position.x + marker.pose.position.y * marker.pose.position.y);
00056 measurment.sensorLocationOnRobot.m_coords = pose.m_coords;
00057 if(marker.ids.size() > 0) {
00058 measurment.beaconID = marker.ids[0];
00059 } else {
00060 measurment.beaconID = -1;
00061 }
00062 }
00063 return true;
00064 };
00065 }