marker_msgs.cpp
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 }


mrpt_bridge
Author(s): Markus Bader , Raphael Zack
autogenerated on Fri Apr 27 2018 05:10:54