10 #include <geometry_msgs/Pose.h> 11 #include <mrpt/ros1bridge/time.h> 12 #include <mrpt/ros1bridge/pose.h> 13 #include <mrpt_msgs/ObservationRangeBeacon.h> 17 #include <mrpt/obs/CObservationBeaconRanges.h> 25 const mrpt::poses::CPose3D& _pose, CObservationBeaconRanges& _obj)
27 _obj.timestamp = mrpt::ros1bridge::fromROS(_msg.header.stamp);
28 mrpt::poses::CPose3D cpose_obj;
30 _obj.stdError = _msg.sensor_std_range;
31 _obj.sensorLabel = _msg.header.frame_id;
32 _obj.maxSensorDistance = _msg.max_sensor_distance;
33 _obj.minSensorDistance = _msg.min_sensor_distance;
37 cpose_obj = mrpt::ros1bridge::fromROS(_msg.sensor_pose_on_robot);
38 _obj.setSensorPose(cpose_obj);
42 _obj.setSensorPose(_pose);
45 ASSERT_(_msg.sensed_data.size() >= 1);
46 const size_t N = _msg.sensed_data.size();
48 _obj.sensedData.resize(N);
50 for (std::size_t i_mrpt = 0; i_mrpt < N; i_mrpt++)
52 _obj.sensedData[i_mrpt].sensedDistance = _msg.sensed_data[i_mrpt].range;
53 _obj.sensedData[i_mrpt].beaconID = _msg.sensed_data[i_mrpt].id;
59 const CObservationBeaconRanges& _obj,
62 mrpt::poses::CPose3D cpose_obj;
64 _msg.header.stamp = mrpt::ros1bridge::toROS(_obj.timestamp);
65 _obj.getSensorPose(cpose_obj);
66 _msg.sensor_pose_on_robot = mrpt::ros1bridge::toROS_Pose(cpose_obj);
68 _msg.sensor_std_range = _obj.stdError;
69 _msg.header.frame_id = _obj.sensorLabel;
70 _msg.max_sensor_distance = _obj.maxSensorDistance;
71 _msg.min_sensor_distance = _obj.minSensorDistance;
73 ASSERT_(_obj.sensedData.size() >= 1);
74 const size_t N = _obj.sensedData.size();
76 _msg.sensed_data.resize(N);
78 for (std::size_t i_msg = 0; i_msg < N; i_msg++)
80 _msg.sensed_data[i_msg].range = _obj.sensedData[i_msg].sensedDistance;
81 _msg.sensed_data[i_msg].id = _obj.sensedData[i_msg].beaconID;
87 const CObservationBeaconRanges& _obj,
91 mrpt::poses::CPose3D pose;
92 _obj.getSensorPose(pose);
93 _pose = mrpt::ros1bridge::toROS_Pose(pose);
File includes methods for converting CNetworkOfPoses*DInf <=> NetworkOfPoses message types...
bool convert(const CObservationBeaconRanges &_obj, mrpt_msgs::ObservationRangeBeacon &_msg, geometry_msgs::Pose &_pose)