gps_status.cpp
Go to the documentation of this file.
1 
9 /*
10  * Copyright 2019 Ardupilot.
11  *
12  * This file is part of the mavros package and subject to the license terms
13  * in the top-level LICENSE file of the mavros repository.
14  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
15  */
16 
17 #include <mavros/mavros_plugin.h>
18 #include <mavros_msgs/GPSRAW.h>
19 #include <mavros_msgs/GPSRTK.h>
20 
21 namespace mavros {
22 namespace extra_plugins {
23 using mavlink::common::RTK_BASELINE_COORDINATE_SYSTEM;
24 
30 class GpsStatusPlugin : public plugin::PluginBase {
31 public:
32  GpsStatusPlugin() : PluginBase(),
33  gpsstatus_nh("~gpsstatus")
34  { }
35 
36  void initialize(UAS &uas_) override
37  {
38  PluginBase::initialize(uas_);
39 
40  gps1_raw_pub = gpsstatus_nh.advertise<mavros_msgs::GPSRAW>("gps1/raw", 10);
41  gps2_raw_pub = gpsstatus_nh.advertise<mavros_msgs::GPSRAW>("gps2/raw", 10);
42  gps1_rtk_pub = gpsstatus_nh.advertise<mavros_msgs::GPSRTK>("gps1/rtk", 10);
43  gps2_rtk_pub = gpsstatus_nh.advertise<mavros_msgs::GPSRTK>("gps2/rtk", 10);
44  }
45 
46  Subscriptions get_subscriptions() override
47  {
48  return {
49  make_handler(&GpsStatusPlugin::handle_gps_raw_int),
50  make_handler(&GpsStatusPlugin::handle_gps2_raw),
51  make_handler(&GpsStatusPlugin::handle_gps_rtk),
52  make_handler(&GpsStatusPlugin::handle_gps2_rtk)
53  };
54  }
55 
56 private:
57  ros::NodeHandle gpsstatus_nh;
58 
59  ros::Publisher gps1_raw_pub;
60  ros::Publisher gps2_raw_pub;
61  ros::Publisher gps1_rtk_pub;
62  ros::Publisher gps2_rtk_pub;
63 
64  /* -*- callbacks -*- */
68  void handle_gps_raw_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GPS_RAW_INT &mav_msg) {
69  auto ros_msg = boost::make_shared<mavros_msgs::GPSRAW>();
70  ros_msg->header = m_uas->synchronized_header("/wgs84", mav_msg.time_usec);
71  ros_msg->fix_type = mav_msg.fix_type;
72  ros_msg->lat = mav_msg.lat;
73  ros_msg->lon = mav_msg.lon;
74  ros_msg->alt = mav_msg.alt;
75  ros_msg->eph = mav_msg.eph;
76  ros_msg->epv = mav_msg.epv;
77  ros_msg->vel = mav_msg.vel;
78  ros_msg->cog = mav_msg.cog;
79  ros_msg->satellites_visible = mav_msg.satellites_visible;
80  ros_msg->alt_ellipsoid = mav_msg.alt_ellipsoid;
81  ros_msg->h_acc = mav_msg.h_acc;
82  ros_msg->v_acc = mav_msg.v_acc;
83  ros_msg->vel_acc = mav_msg.vel_acc;
84  ros_msg->hdg_acc = mav_msg.hdg_acc;
85  ros_msg->dgps_numch = UINT8_MAX; // information not available in GPS_RAW_INT mavlink message
86  ros_msg->dgps_age = UINT32_MAX;// information not available in GPS_RAW_INT mavlink message
87  ros_msg->yaw = mav_msg.yaw;
88 
89  gps1_raw_pub.publish(ros_msg);
90  }
91 
95  void handle_gps2_raw(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GPS2_RAW &mav_msg) {
96  auto ros_msg = boost::make_shared<mavros_msgs::GPSRAW>();
97  ros_msg->header = m_uas->synchronized_header("/wgs84", mav_msg.time_usec);
98  ros_msg->fix_type = mav_msg.fix_type;
99  ros_msg->lat = mav_msg.lat;
100  ros_msg->lon = mav_msg.lon;
101  ros_msg->alt = mav_msg.alt;
102  ros_msg->eph = mav_msg.eph;
103  ros_msg->epv = mav_msg.epv;
104  ros_msg->vel = mav_msg.vel;
105  ros_msg->cog = mav_msg.cog;
106  ros_msg->satellites_visible = mav_msg.satellites_visible;
107  ros_msg->alt_ellipsoid = mav_msg.alt_ellipsoid;
108  ros_msg->h_acc = mav_msg.h_acc;
109  ros_msg->v_acc = mav_msg.v_acc;
110  ros_msg->vel_acc = mav_msg.vel_acc;
111  ros_msg->hdg_acc = mav_msg.hdg_acc;
112  ros_msg->dgps_numch = mav_msg.dgps_numch;
113  ros_msg->dgps_age = mav_msg.dgps_age;
114  ros_msg->yaw = mav_msg.yaw;
115 
116  gps2_raw_pub.publish(ros_msg);
117  }
118 
122  void handle_gps_rtk(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GPS_RTK &mav_msg) {
123  auto ros_msg = boost::make_shared<mavros_msgs::GPSRTK>();
124  switch (static_cast<RTK_BASELINE_COORDINATE_SYSTEM>(mav_msg.baseline_coords_type))
125  {
126  case RTK_BASELINE_COORDINATE_SYSTEM::ECEF:
127  ros_msg->header.frame_id = "earth";
128  break;
129  case RTK_BASELINE_COORDINATE_SYSTEM::NED:
130  ros_msg->header.frame_id = "map";
131  break;
132  default:
133  ROS_ERROR_NAMED("gps_status", "GPS_RTK.baseline_coords_type MAVLink field has unknown \"%d\" value", mav_msg.baseline_coords_type);
134  }
135  ros_msg->header = m_uas->synchronized_header(ros_msg->header.frame_id, mav_msg.time_last_baseline_ms * 1000);
136  ros_msg->rtk_receiver_id = mav_msg.rtk_receiver_id;
137  ros_msg->wn = mav_msg.wn;
138  ros_msg->tow = mav_msg.tow;
139  ros_msg->rtk_health = mav_msg.rtk_health;
140  ros_msg->rtk_rate = mav_msg.rtk_rate;
141  ros_msg->nsats = mav_msg.nsats;
142  ros_msg->baseline_a = mav_msg.baseline_a_mm;
143  ros_msg->baseline_b = mav_msg.baseline_b_mm;
144  ros_msg->baseline_c = mav_msg.baseline_c_mm;
145  ros_msg->accuracy = mav_msg.accuracy;
146  ros_msg->iar_num_hypotheses = mav_msg.iar_num_hypotheses;
147 
148  gps1_rtk_pub.publish(ros_msg);
149  }
150 
154  void handle_gps2_rtk(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GPS2_RTK &mav_msg) {
155  auto ros_msg = boost::make_shared<mavros_msgs::GPSRTK>();
156  switch (static_cast<RTK_BASELINE_COORDINATE_SYSTEM>(mav_msg.baseline_coords_type))
157  {
158  case RTK_BASELINE_COORDINATE_SYSTEM::ECEF:
159  ros_msg->header.frame_id = "earth";
160  break;
161  case RTK_BASELINE_COORDINATE_SYSTEM::NED:
162  ros_msg->header.frame_id = "map";
163  break;
164  default:
165  ROS_ERROR_NAMED("gps_status", "GPS_RTK2.baseline_coords_type MAVLink field has unknown \"%d\" value", mav_msg.baseline_coords_type);
166  }
167  ros_msg->header = m_uas->synchronized_header(ros_msg->header.frame_id, mav_msg.time_last_baseline_ms * 1000);
168  ros_msg->rtk_receiver_id = mav_msg.rtk_receiver_id;
169  ros_msg->wn = mav_msg.wn;
170  ros_msg->tow = mav_msg.tow;
171  ros_msg->rtk_health = mav_msg.rtk_health;
172  ros_msg->rtk_rate = mav_msg.rtk_rate;
173  ros_msg->nsats = mav_msg.nsats;
174  ros_msg->baseline_a = mav_msg.baseline_a_mm;
175  ros_msg->baseline_b = mav_msg.baseline_b_mm;
176  ros_msg->baseline_c = mav_msg.baseline_c_mm;
177  ros_msg->accuracy = mav_msg.accuracy;
178  ros_msg->iar_num_hypotheses = mav_msg.iar_num_hypotheses;
179 
180  gps2_rtk_pub.publish(ros_msg);
181  }
182 };
183 } // namespace extra_plugins
184 } // namespace mavros
185 
186 #include <pluginlib/class_list_macros.hpp>
187 PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::GpsStatusPlugin, mavros::plugin::PluginBase)
mavros::plugin::PluginBase::m_uas
UAS * m_uas
mavros::plugin::PluginBase::Subscriptions
std::vector< HandlerInfo > Subscriptions
ros::Publisher
mavros::extra_plugins::GpsStatusPlugin
Mavlink GPS status plugin.
Definition: gps_status.cpp:30
mavros::plugin::PluginBase::PluginBase
PluginBase()
mavros::UAS
mavros::extra_plugins::GpsStatusPlugin::handle_gps_raw_int
void handle_gps_raw_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GPS_RAW_INT &mav_msg)
Publish mavlink GPS_RAW_INT message into the gps1/raw topic.
Definition: gps_status.cpp:68
mavros::extra_plugins::GpsStatusPlugin::handle_gps_rtk
void handle_gps_rtk(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GPS_RTK &mav_msg)
Publish mavlink GPS_RTK message into the gps1/rtk topic.
Definition: gps_status.cpp:122
mavros::extra_plugins::GpsStatusPlugin::handle_gps2_rtk
void handle_gps2_rtk(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GPS2_RTK &mav_msg)
Publish mavlink GPS2_RTK message into the gps2/rtk topic.
Definition: gps_status.cpp:154
mavros::extra_plugins::GpsStatusPlugin::gpsstatus_nh
ros::NodeHandle gpsstatus_nh
Definition: gps_status.cpp:57
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
mavros::extra_plugins::GpsStatusPlugin::initialize
void initialize(UAS &uas_) override
Definition: gps_status.cpp:36
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
mavros_plugin.h
mavros::extra_plugins::GpsStatusPlugin::get_subscriptions
Subscriptions get_subscriptions() override
Definition: gps_status.cpp:46
mavros::extra_plugins::GpsStatusPlugin::handle_gps2_raw
void handle_gps2_raw(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GPS2_RAW &mav_msg)
Publish mavlink GPS2_RAW message into the gps2/raw topic.
Definition: gps_status.cpp:95
mavros
mavros::UAS::synchronized_header
std_msgs::Header synchronized_header(const std::string &frame_id, const T time_stamp)
mavros::extra_plugins::GpsStatusPlugin::gps2_raw_pub
ros::Publisher gps2_raw_pub
Definition: gps_status.cpp:60
initialize
virtual void initialize(UAS &uas)
mavros::plugin::PluginBase
mavros::extra_plugins::GpsStatusPlugin::gps2_rtk_pub
ros::Publisher gps2_rtk_pub
Definition: gps_status.cpp:62
class_list_macros.hpp
mavros::extra_plugins::GpsStatusPlugin::gps1_rtk_pub
ros::Publisher gps1_rtk_pub
Definition: gps_status.cpp:61
mavros::plugin::PluginBase::make_handler
HandlerInfo make_handler(const mavlink::msgid_t id, void(_C::*fn)(const mavlink::mavlink_message_t *msg, const mavconn::Framing framing))
mavros::extra_plugins::GpsStatusPlugin::gps1_raw_pub
ros::Publisher gps1_raw_pub
Definition: gps_status.cpp:59
ros::NodeHandle
mavros::extra_plugins::GpsStatusPlugin::GpsStatusPlugin
GpsStatusPlugin()
Definition: gps_status.cpp:32

mavros_extras
Author(s): Vladimir Ermakov , Amilcar Lucas
autogenerated on Tue May 6 2025 02:34:08