range.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 /*---------------------------------------------------------------
11  APPLICATION: mrpt_ros bridge
12  FILE: range.cpp
13  AUTHOR: Raghavender Sahdev <raghavendersahdev@gmail.com>
14  ---------------------------------------------------------------*/
15 #include "mrpt_bridge/range.h"
16 #include <iostream>
17 using namespace std;
18 
19 namespace mrpt_bridge
20 {
21 namespace range
22 {
23 /************************************************************************
24  * ros2mrpt *
25  ************************************************************************/
26 bool ros2mrpt(const sensor_msgs::Range& msg, CObservationRange& obj)
27 {
28  obj.minSensorDistance = msg.min_range;
29  obj.maxSensorDistance = msg.max_range;
30  obj.sensorConeApperture = msg.field_of_view;
31 
34  obj.sensedData.at(0).sensedDistance = msg.range;
35  return true;
36 }
37 
38 /************************************************************************
39  * mrpt2ros *
40  ************************************************************************/
41 bool mrpt2ros(
42  const CObservationRange& obj, const std_msgs::Header& msg_header,
43  sensor_msgs::Range* msg)
44 {
45  long num_range = obj.sensedData.size();
46 
47  // 1) sensor_msgs::Range:: header
48  for (int i = 0; i < num_range; i++) msg[i].header = msg_header;
49 
50  // 2) sensor_msg::Range parameters
51  for (int i = 0; i < num_range; i++)
52  {
53  msg[i].max_range = obj.maxSensorDistance;
54  msg[i].min_range = obj.minSensorDistance;
55  msg[i].field_of_view = obj.sensorConeApperture;
56  }
57 
61  for (int i = 0; i < num_range; i++)
62  msg[i].range = obj.sensedData.at(i).sensedDistance;
63 
67  // msg.radiation_type
68  return true;
69 }
70 } // namespace range
71 } // namespace mrpt_bridge
72 
74 /*
75 uint8 ULTRASOUND=0
76 uint8 INFRARED=1
77 std_msgs/Header header
78 uint8 radiation_type
79 float32 field_of_view
80 float32 min_range
81 float32 max_range
82 float32 range
83 */
mrpt_bridge::GPS::mrpt2ros
bool mrpt2ros(const CObservationGPS &obj, const std_msgs::Header &msg_header, sensor_msgs::NavSatFix &msg)
Definition: GPS.cpp:54
range.h
std_msgs::Header_
Definition: map.h:24
std
mrpt_bridge
File includes methods for converting CNetworkOfPoses*DInf <=> NetworkOfPoses message types.
Definition: include/mrpt_bridge/beacon.h:52
header
const std::string header
mrpt_bridge::GPS::ros2mrpt
bool ros2mrpt(const sensor_msgs::NavSatFix &msg, CObservationGPS &obj)
Definition: GPS.cpp:23


mrpt_bridge
Author(s): Markus Bader , Raphael Zack
autogenerated on Sun Mar 6 2022 03:48:10