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 
88  gps1_raw_pub.publish(ros_msg);
89  }
90 
94  void handle_gps2_raw(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GPS2_RAW &mav_msg) {
95  auto ros_msg = boost::make_shared<mavros_msgs::GPSRAW>();
96  ros_msg->header = m_uas->synchronized_header("/wgs84", mav_msg.time_usec);
97  ros_msg->fix_type = mav_msg.fix_type;
98  ros_msg->lat = mav_msg.lat;
99  ros_msg->lon = mav_msg.lon;
100  ros_msg->alt = mav_msg.alt;
101  ros_msg->eph = mav_msg.eph;
102  ros_msg->epv = mav_msg.epv;
103  ros_msg->vel = mav_msg.vel;
104  ros_msg->cog = mav_msg.cog;
105  ros_msg->satellites_visible = mav_msg.satellites_visible;
106  ros_msg->alt_ellipsoid = INT32_MAX; // information not available in GPS2_RAW mavlink message
107  ros_msg->h_acc = UINT32_MAX;// information not available in GPS2_RAW mavlink message
108  ros_msg->v_acc = UINT32_MAX;// information not available in GPS2_RAW mavlink message
109  ros_msg->vel_acc = UINT32_MAX;// information not available in GPS2_RAW mavlink message
110  ros_msg->hdg_acc = UINT32_MAX;// information not available in GPS2_RAW mavlink message
111  ros_msg->dgps_numch = mav_msg.dgps_numch;
112  ros_msg->dgps_age = mav_msg.dgps_age;
113 
114  gps2_raw_pub.publish(ros_msg);
115  }
116 
120  void handle_gps_rtk(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GPS_RTK &mav_msg) {
121  auto ros_msg = boost::make_shared<mavros_msgs::GPSRTK>();
122  switch (static_cast<RTK_BASELINE_COORDINATE_SYSTEM>(mav_msg.baseline_coords_type))
123  {
124  case RTK_BASELINE_COORDINATE_SYSTEM::ECEF:
125  ros_msg->header.frame_id = "earth";
126  break;
127  case RTK_BASELINE_COORDINATE_SYSTEM::NED:
128  ros_msg->header.frame_id = "map";
129  break;
130  default:
131  ROS_ERROR_NAMED("gps_status", "GPS_RTK.baseline_coords_type MAVLink field has unknown \"%d\" value", mav_msg.baseline_coords_type);
132  }
133  ros_msg->header = m_uas->synchronized_header(ros_msg->header.frame_id, mav_msg.time_last_baseline_ms * 1000);
134  ros_msg->rtk_receiver_id = mav_msg.rtk_receiver_id;
135  ros_msg->wn = mav_msg.wn;
136  ros_msg->tow = mav_msg.tow;
137  ros_msg->rtk_health = mav_msg.rtk_health;
138  ros_msg->rtk_rate = mav_msg.rtk_rate;
139  ros_msg->nsats = mav_msg.nsats;
140  ros_msg->baseline_a = mav_msg.baseline_a_mm;
141  ros_msg->baseline_b = mav_msg.baseline_b_mm;
142  ros_msg->baseline_c = mav_msg.baseline_c_mm;
143  ros_msg->accuracy = mav_msg.accuracy;
144  ros_msg->iar_num_hypotheses = mav_msg.iar_num_hypotheses;
145 
146  gps1_rtk_pub.publish(ros_msg);
147  }
148 
152  void handle_gps2_rtk(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GPS2_RTK &mav_msg) {
153  auto ros_msg = boost::make_shared<mavros_msgs::GPSRTK>();
154  switch (static_cast<RTK_BASELINE_COORDINATE_SYSTEM>(mav_msg.baseline_coords_type))
155  {
156  case RTK_BASELINE_COORDINATE_SYSTEM::ECEF:
157  ros_msg->header.frame_id = "earth";
158  break;
159  case RTK_BASELINE_COORDINATE_SYSTEM::NED:
160  ros_msg->header.frame_id = "map";
161  break;
162  default:
163  ROS_ERROR_NAMED("gps_status", "GPS_RTK2.baseline_coords_type MAVLink field has unknown \"%d\" value", mav_msg.baseline_coords_type);
164  }
165  ros_msg->header = m_uas->synchronized_header(ros_msg->header.frame_id, mav_msg.time_last_baseline_ms * 1000);
166  ros_msg->rtk_receiver_id = mav_msg.rtk_receiver_id;
167  ros_msg->wn = mav_msg.wn;
168  ros_msg->tow = mav_msg.tow;
169  ros_msg->rtk_health = mav_msg.rtk_health;
170  ros_msg->rtk_rate = mav_msg.rtk_rate;
171  ros_msg->nsats = mav_msg.nsats;
172  ros_msg->baseline_a = mav_msg.baseline_a_mm;
173  ros_msg->baseline_b = mav_msg.baseline_b_mm;
174  ros_msg->baseline_c = mav_msg.baseline_c_mm;
175  ros_msg->accuracy = mav_msg.accuracy;
176  ros_msg->iar_num_hypotheses = mav_msg.iar_num_hypotheses;
177 
178  gps2_rtk_pub.publish(ros_msg);
179  }
180 };
181 } // namespace extra_plugins
182 } // namespace mavros
183 
184 #include <pluginlib/class_list_macros.h>
185 PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::GpsStatusPlugin, mavros::plugin::PluginBase)
mavros::extra_plugins::GpsStatusPlugin::gpsstatus_nh
ros::NodeHandle gpsstatus_nh
Definition: gps_status.cpp:57
mavros::UAS::synchronized_header
std_msgs::Header synchronized_header(const std::string &frame_id, const T time_stamp)
ros::NodeHandle
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
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:120
mavros_plugin.h
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::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:94
mavros::extra_plugins::GpsStatusPlugin::gps1_rtk_pub
ros::Publisher gps1_rtk_pub
Definition: gps_status.cpp:61
mavros::extra_plugins::GpsStatusPlugin::gps2_raw_pub
ros::Publisher gps2_raw_pub
Definition: gps_status.cpp:60
mavros::extra_plugins::GpsStatusPlugin::gps2_rtk_pub
ros::Publisher gps2_rtk_pub
Definition: gps_status.cpp:62
mavros::plugin::PluginBase::PluginBase
PluginBase()
mavros::extra_plugins::GpsStatusPlugin
Mavlink GPS status plugin.
Definition: gps_status.cpp:30
class_list_macros.h
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:152
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
mavros::extra_plugins::GpsStatusPlugin::GpsStatusPlugin
GpsStatusPlugin()
Definition: gps_status.cpp:32
mavros::plugin::PluginBase::m_uas
UAS * m_uas
mavros::plugin::PluginBase::Subscriptions
std::vector< HandlerInfo > Subscriptions
mavros::extra_plugins::GpsStatusPlugin::gps1_raw_pub
ros::Publisher gps1_raw_pub
Definition: gps_status.cpp:59
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
mavros::extra_plugins::GpsStatusPlugin::get_subscriptions
Subscriptions get_subscriptions() override
Definition: gps_status.cpp:46
mavros::extra_plugins::GpsStatusPlugin::initialize
void initialize(UAS &uas_) override
Definition: gps_status.cpp:36
initialize
void initialize(UAS &uas_) override
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
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_extras
Author(s): Vladimir Ermakov , Amilcar Lucas
autogenerated on Tue Jun 1 2021 02:36:36