GPS.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: GPS.cpp
13  AUTHOR: Raghavender Sahdev <raghavendersahdev@gmail.com>
14  ---------------------------------------------------------------*/
15 
16 #include "mrpt_bridge/GPS.h"
17 
18 namespace mrpt_bridge
19 {
20 /************************************************************************
21  * ros2mrpt *
22  ************************************************************************/
23 bool GPS::ros2mrpt(const sensor_msgs::NavSatFix& msg, CObservationGPS& obj)
24 {
25  mrpt::obs::gnss::Message_NMEA_GGA gga;
26  gga.fields.altitude_meters = msg.altitude;
27  gga.fields.latitude_degrees = msg.latitude;
28  gga.fields.longitude_degrees = msg.longitude;
29 
30  switch (msg.status.status)
31  {
32  case -1:
33  gga.fields.fix_quality = 0;
34  break;
35  case 0:
36  gga.fields.fix_quality = 1;
37  break;
38  case 2:
39  gga.fields.fix_quality = 2;
40  break;
41  case 1:
42  gga.fields.fix_quality = 3;
43  break;
44  default:
45  gga.fields.fix_quality = 0; // never going to execute default
46  }
47  obj.setMsg(gga);
48  return true;
49 }
50 
51 /************************************************************************
52  * mrpt2ros *
53  ************************************************************************/
54 bool GPS::mrpt2ros(
55  const CObservationGPS& obj, const std_msgs::Header& msg_header,
56  sensor_msgs::NavSatFix& msg)
57 {
58  // 1) sensor_msgs::NavSatFix:: header
59  msg.header = msg_header;
60 
61  // 2) other NavSatFix Parameters, the following 3 could be wrong too
62 
63  if (obj.hasMsgClass<mrpt::obs::gnss::Message_NMEA_GGA>())
64  {
65  const mrpt::obs::gnss::Message_NMEA_GGA& gga =
66  obj.getMsgByClass<mrpt::obs::gnss::Message_NMEA_GGA>();
67  msg.altitude = gga.fields.altitude_meters;
68  msg.latitude = gga.fields.latitude_degrees;
69  msg.longitude = gga.fields.longitude_degrees;
70 
74  switch (gga.fields.fix_quality)
75  {
76  case 0:
77  msg.status.status = -1;
78  break;
79  case 1:
80  msg.status.status = 0;
81  break;
82  case 2:
83  msg.status.status = 2;
84  break;
85  case 3:
86  msg.status.status = 1;
87  break;
88  default:
89  // this is based on literature available on GPS as the number of
90  // types in ROS and MRPT are not same
91  msg.status.status = 0;
92  }
93  // this might be incorrect as there is not matching field in mrpt
94  // message type
95  msg.status.service = 1;
96  }
99  return true;
100 }
101 } // namespace mrpt_bridge
102 
104 /*
105 uint8 COVARIANCE_TYPE_UNKNOWN=0
106 uint8 COVARIANCE_TYPE_APPROXIMATED=1
107 uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN=2
108 uint8 COVARIANCE_TYPE_KNOWN=3
109 std_msgs/Header header
110 sensor_msgs/NavSatStatus status
111 float64 latitude
112 float64 longitude
113 float64 altitude
114 float64[9] position_covariance
115 uint8 position_covariance_type
116 */
117 
119 /*
120 int8 STATUS_NO_FIX=-1
121 int8 STATUS_FIX=0
122 int8 STATUS_SBAS_FIX=1
123 int8 STATUS_GBAS_FIX=2
124 uint16 SERVICE_GPS=1
125 uint16 SERVICE_GLONASS=2
126 uint16 SERVICE_COMPASS=4
127 uint16 SERVICE_GALILEO=8
128 int8 status
129 uint16 service
130  */
mrpt_bridge::GPS::mrpt2ros
bool mrpt2ros(const CObservationGPS &obj, const std_msgs::Header &msg_header, sensor_msgs::NavSatFix &msg)
Definition: GPS.cpp:54
GPS.h
std_msgs::Header_
Definition: map.h:24
mrpt_bridge
File includes methods for converting CNetworkOfPoses*DInf <=> NetworkOfPoses message types.
Definition: include/mrpt_bridge/beacon.h:52
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