beacon.cpp
Go to the documentation of this file.
00001 
00002 #include <geometry_msgs/Pose.h>
00003 #include "mrpt_bridge/time.h"
00004 #include "mrpt_bridge/pose.h"
00005 #include <mrpt_msgs/ObservationRangeBeacon.h>
00006 #include "tf/transform_datatypes.h"
00007 #include "tf/LinearMath/Matrix3x3.h"
00008 #include "mrpt_bridge/beacon.h"
00009 
00010 #include <mrpt/version.h>
00011 #include <mrpt/obs/CObservationBeaconRanges.h>
00012 using namespace mrpt::obs;
00013 
00014 namespace mrpt_bridge
00015 {
00016 bool convert(
00017         const mrpt_msgs::ObservationRangeBeacon& _msg,
00018         const mrpt::poses::CPose3D& _pose, CObservationBeaconRanges& _obj)
00019 {
00020         mrpt_bridge::convert(_msg.header.stamp, _obj.timestamp);
00021         mrpt::poses::CPose3D cpose_obj;
00022 
00023         _obj.stdError = _msg.sensor_std_range;
00024         _obj.sensorLabel = _msg.header.frame_id;
00025         _obj.maxSensorDistance = _msg.max_sensor_distance;
00026         _obj.minSensorDistance = _msg.min_sensor_distance;
00027 
00028         if (_pose.empty())
00029         {
00030                 convert(_msg.sensor_pose_on_robot, cpose_obj);
00031                 _obj.setSensorPose(cpose_obj);
00032         }
00033         else
00034         {
00035                 _obj.setSensorPose(_pose);
00036         }
00037 
00038         ASSERT_(_msg.sensed_data.size() >= 1)
00039         const size_t N = _msg.sensed_data.size();
00040 
00041         _obj.sensedData.resize(N);
00042 
00043         for (std::size_t i_mrpt = 0; i_mrpt < N; i_mrpt++)
00044         {
00045                 _obj.sensedData[i_mrpt].sensedDistance = _msg.sensed_data[i_mrpt].range;
00046                 _obj.sensedData[i_mrpt].beaconID = _msg.sensed_data[i_mrpt].id;
00047         }
00048         return true;
00049 }
00050 
00051 bool convert(
00052         const CObservationBeaconRanges& _obj,
00053         mrpt_msgs::ObservationRangeBeacon& _msg)
00054 {
00055         mrpt::poses::CPose3D cpose_obj;
00056 
00057         mrpt_bridge::convert(_obj.timestamp, _msg.header.stamp);
00058         _obj.getSensorPose(cpose_obj);
00059         convert(cpose_obj, _msg.sensor_pose_on_robot);
00060 
00061         _msg.sensor_std_range = _obj.stdError;
00062         _msg.header.frame_id = _obj.sensorLabel;
00063         _msg.max_sensor_distance = _obj.maxSensorDistance;
00064         _msg.min_sensor_distance = _obj.minSensorDistance;
00065 
00066         ASSERT_(_obj.sensedData.size() >= 1);
00067         const size_t N = _obj.sensedData.size();
00068 
00069         _msg.sensed_data.resize(N);
00070 
00071         for (std::size_t i_msg = 0; i_msg < N; i_msg++)
00072         {
00073                 _msg.sensed_data[i_msg].range = _obj.sensedData[i_msg].sensedDistance;
00074                 _msg.sensed_data[i_msg].id = _obj.sensedData[i_msg].beaconID;
00075         }
00076         return true;
00077 }
00078 
00079 bool convert(
00080         const CObservationBeaconRanges& _obj,
00081         mrpt_msgs::ObservationRangeBeacon& _msg, geometry_msgs::Pose& _pose)
00082 {
00083         convert(_obj, _msg);
00084         mrpt::poses::CPose3D pose;
00085         _obj.getSensorPose(pose);
00086         convert(pose, _pose);
00087         return true;
00088 }
00089 }  // end namespace


mrpt_bridge
Author(s): Markus Bader , Raphael Zack
autogenerated on Mon Sep 18 2017 03:12:06