waypoint.cpp
Go to the documentation of this file.
1 
9 /*
10  * Copyright 2014,2015,2016,2017,2018 Vladimir Ermakov.
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 
18 
19 #include <mavros_msgs/WaypointList.h>
20 #include <mavros_msgs/WaypointSetCurrent.h>
21 #include <mavros_msgs/WaypointReached.h>
22 
23 
24 namespace mavros {
25 namespace std_plugins {
30 public:
32  MissionBase("WP"),
33  wp_nh("~mission")
34  { }
35 
36  void initialize(UAS &uas_) override
37  {
38  PluginBase::initialize(uas_);
39  MissionBase::initialize(&wp_nh);
40 
42  wp_type = plugin::WP_TYPE::MISSION;
43 
44  wp_nh.param("pull_after_gcs", do_pull_after_gcs, true);
45  wp_nh.param("use_mission_item_int", use_mission_item_int, false);
46 
47  wp_list_pub = wp_nh.advertise<mavros_msgs::WaypointList>("waypoints", 2, true);
51 
52  wp_reached_pub = wp_nh.advertise<mavros_msgs::WaypointReached>("reached", 10, true);
54 
57  }
58 
60  return {
69  };
70  }
71 
72 private:
74 
79 
82 
83  /* -*- rx handlers -*- */
84 
91  void handle_mission_current(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MISSION_CURRENT &mcur)
92  {
93  unique_lock lock(mutex);
94 
95  if (wp_state == WP::SET_CUR) {
96  /* MISSION_SET_CURRENT ACK */
97  ROS_DEBUG_NAMED(log_ns, "%s: set current #%d done", log_ns.c_str(), mcur.seq);
98  go_idle();
99  wp_cur_active = mcur.seq;
101 
102  lock.unlock();
103  list_sending.notify_all();
105  }
106  else if (wp_state == WP::IDLE && wp_cur_active != mcur.seq) {
107  /* update active */
108  ROS_DEBUG_NAMED(log_ns, "%s: update current #%d", log_ns.c_str(), mcur.seq);
109  wp_cur_active = mcur.seq;
110  set_current_waypoint(wp_cur_active);
111 
112  lock.unlock();
114  }
115  }
116 
122  void handle_mission_item_reached(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MISSION_ITEM_REACHED &mitr)
123  {
124  /* in QGC used as informational message */
125  ROS_INFO_NAMED(log_ns, "%s: reached #%d", log_ns.c_str(), mitr.seq);
126 
127  auto wpr = boost::make_shared<mavros_msgs::WaypointReached>();
128 
129  wpr->header.stamp = ros::Time::now();
130  wpr->wp_seq = mitr.seq;
131 
132  wp_reached_pub.publish(wpr);
133  }
134 
135  /* -*- mid-level helpers -*- */
136 
137  // Acts when capabilities of the fcu are changed
138  void capabilities_cb(UAS::MAV_CAP capabilities) override {
139  lock_guard lock(mutex);
140  if (m_uas->has_capability(UAS::MAV_CAP::MISSION_INT)) {
141  use_mission_item_int = true;
143  ROS_INFO_NAMED(log_ns, "%s: Using MISSION_ITEM_INT", log_ns.c_str());
144  } else {
145  use_mission_item_int = false;
147  ROS_WARN_NAMED(log_ns, "%s: Falling back to MISSION_ITEM", log_ns.c_str());
148  }
149  }
150 
151  // Act on first heartbeat from FCU
152  void connection_cb(bool connected) override
153  {
154  lock_guard lock(mutex);
155  if (connected) {
157 
158  if (wp_nh.hasParam("enable_partial_push")) {
159  wp_nh.getParam("enable_partial_push", enable_partial_push);
160  }
161  else {
163  }
164  }
165  else {
167  }
168  }
169 
171  void publish_waypoints() override
172  {
173  auto wpl = boost::make_shared<mavros_msgs::WaypointList>();
174  unique_lock lock(mutex);
175 
176  wpl->current_seq = wp_cur_active;
177  wpl->waypoints.clear();
178  wpl->waypoints.reserve(waypoints.size());
179  for (auto &it : waypoints) {
180  wpl->waypoints.push_back(it);
181  }
182 
183  lock.unlock();
184  wp_list_pub.publish(wpl);
185  }
186 
187  /* -*- ROS callbacks -*- */
188 
189  bool pull_cb(mavros_msgs::WaypointPull::Request &req,
190  mavros_msgs::WaypointPull::Response &res)
191  {
192  unique_lock lock(mutex);
193 
194  if (wp_state != WP::IDLE)
195  // Wrong initial state, other operation in progress?
196  return false;
197 
199  wp_count = 0;
201 
202  lock.unlock();
204  res.success = wait_fetch_all();
205  lock.lock();
206 
207  res.wp_received = waypoints.size();
208  go_idle(); // not nessessary, but prevents from blocking
209  return true;
210  }
211 
212  bool push_cb(mavros_msgs::WaypointPush::Request &req,
213  mavros_msgs::WaypointPush::Response &res)
214  {
215  unique_lock lock(mutex);
216 
217  if (wp_state != WP::IDLE)
218  // Wrong initial state, other operation in progress?
219  return false;
220 
221  if (req.start_index) {
222  // Partial Waypoint update
223 
224  if (!enable_partial_push) {
225  ROS_WARN_NAMED(log_ns, "%s: Partial Push not enabled. (Only supported on APM)", log_ns.c_str());
226  res.success = false;
227  res.wp_transfered = 0;
228  return true;
229  }
230 
231  if (waypoints.size() < req.start_index + req.waypoints.size()) {
232  ROS_WARN_NAMED(log_ns, "%s: Partial push out of range rejected.", log_ns.c_str());
233  res.success = false;
234  res.wp_transfered = 0;
235  return true;
236  }
237 
240 
241  uint16_t seq = req.start_index;
242  for (auto &it : req.waypoints) {
243  send_waypoints[seq] = it;
244  seq++;
245  }
246 
247  wp_count = req.waypoints.size();
248  wp_start_id = req.start_index;
249  wp_end_id = req.start_index + wp_count;
250  wp_cur_id = req.start_index;
252 
253  lock.unlock();
255  res.success = wait_push_all();
256  lock.lock();
257 
258  res.wp_transfered = wp_cur_id - wp_start_id + 1;
259  }
260  else {
261  // Full waypoint update
263 
264  send_waypoints.clear();
265  send_waypoints.reserve(req.waypoints.size());
266  send_waypoints = req.waypoints;
267 
268  wp_count = send_waypoints.size();
270  wp_cur_id = 0;
272 
273  lock.unlock();
275  res.success = wait_push_all();
276  lock.lock();
277 
278  res.wp_transfered = wp_cur_id + 1;
279  }
280 
281  go_idle(); // same as in pull_cb
282  return true;
283  }
284 
285  bool clear_cb(mavros_msgs::WaypointClear::Request &req,
286  mavros_msgs::WaypointClear::Response &res)
287  {
288  unique_lock lock(mutex);
289 
290  if (wp_state != WP::IDLE)
291  return false;
292 
295 
296  lock.unlock();
298  res.success = wait_push_all();
299 
300  lock.lock();
301  go_idle(); // same as in pull_cb
302  return true;
303  }
304 
305  bool set_cur_cb(mavros_msgs::WaypointSetCurrent::Request &req,
306  mavros_msgs::WaypointSetCurrent::Response &res)
307  {
308  unique_lock lock(mutex);
309 
310  if (wp_state != WP::IDLE)
311  return false;
312 
314  wp_set_active = req.wp_seq;
316 
317  lock.unlock();
319  res.success = wait_push_all();
320 
321  lock.lock();
322  go_idle(); // same as in pull_cb
323  return true;
324  }
325 };
326 } // namespace std_plugins
327 } // namespace mavros
328 
MAVROS Plugin base class.
Definition: mavros_plugin.h:36
#define ROS_INFO_NAMED(name,...)
bool set_cur_cb(mavros_msgs::WaypointSetCurrent::Request &req, mavros_msgs::WaypointSetCurrent::Response &res)
Definition: waypoint.cpp:305
void initialize(UAS &uas_) override
Plugin initializer.
Definition: waypoint.cpp:36
void publish(const boost::shared_ptr< M > &message) const
#define ROS_WARN_NAMED(name,...)
HandlerInfo make_handler(const mavlink::msgid_t id, void(_C::*fn)(const mavlink::mavlink_message_t *msg, const mavconn::Framing framing))
Definition: mavros_plugin.h:87
void stop()
void handle_mission_item_int(const mavlink::mavlink_message_t *msg, WP_ITEM_INT &wpi)
handle MISSION_ITEM_INT mavlink msg handles and stores mission items when pulling waypoints ...
void set_current_waypoint(size_t seq)
set the FCU current waypoint
bool has_capability(T capability)
Function to check if the flight controller has a capability.
Definition: mavros_uas.h:324
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
void handle_mission_count(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MISSION_COUNT &mcnt)
handle MISSION_COUNT mavlink msg Handles a mission count from FCU in a Waypoint Pull Triggers a pull ...
bool clear_cb(mavros_msgs::WaypointClear::Request &req, mavros_msgs::WaypointClear::Response &res)
Definition: waypoint.cpp:285
bool push_cb(mavros_msgs::WaypointPush::Request &req, mavros_msgs::WaypointPush::Response &res)
Definition: waypoint.cpp:212
void handle_mission_current(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MISSION_CURRENT &mcur)
handle MISSION_CURRENT mavlink msg This confirms a SET_CUR action
Definition: waypoint.cpp:91
void mission_set_current(uint16_t seq)
#define ROS_DEBUG_NAMED(name,...)
bool pull_cb(mavros_msgs::WaypointPull::Request &req, mavros_msgs::WaypointPull::Response &res)
Definition: waypoint.cpp:189
void handle_mission_item_reached(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MISSION_ITEM_REACHED &mitr)
handle MISSION_ITEM_REACHED mavlink msg
Definition: waypoint.cpp:122
UAS for plugins.
Definition: mavros_uas.h:67
std::lock_guard< std::recursive_mutex > lock_guard
void publish_waypoints() override
publish the updated waypoint list after operation
Definition: waypoint.cpp:171
bool param(const std::string &param_name, T &param_val, const T &default_val) const
Mission manupulation plugin.
Definition: waypoint.cpp:29
bool is_ardupilotmega()
Check that FCU is APM.
Definition: mavros_uas.h:410
mavlink::common::MAV_PROTOCOL_CAPABILITY MAV_CAP
Definition: mavros_uas.h:74
bool wait_fetch_all()
wait until a waypoint pull is complete. Pull happens asynchronously, this function blocks until it is...
std::vector< mavros_msgs::Waypoint > send_waypoints
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool wait_push_all()
wait until a waypoint push is complete. Push happens asynchronously, this function blocks until it is...
void mission_write_partial_list(uint16_t start_index, uint16_t end_index)
std::unique_lock< std::recursive_mutex > unique_lock
Mission base plugin.
std::condition_variable list_sending
void handle_mission_request_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MISSION_REQUEST_INT &mreq)
handle MISSION_REQUEST_INT mavlink msg handles and acts on misison request from FCU ...
void handle_mission_item(const mavlink::mavlink_message_t *msg, WP_ITEM &wpi)
handle MISSION_ITEM mavlink msg handles and stores mission items when pulling waypoints ...
std::vector< HandlerInfo > Subscriptions
Subscriptions vector.
Definition: mavros_plugin.h:47
bool getParam(const std::string &key, std::string &s) const
void connection_cb(bool connected) override
Definition: waypoint.cpp:152
void capabilities_cb(UAS::MAV_CAP capabilities) override
Definition: waypoint.cpp:138
static Time now()
void handle_mission_ack(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MISSION_ACK &mack)
handle MISSION_ACK mavlink msg Handles a MISSION_ACK which marks the end of a push, or a failure
void schedule_pull(const ros::Duration &dt)
bool hasParam(const std::string &key) const
void handle_mission_request(const mavlink::mavlink_message_t *msg, mavlink::common::msg::MISSION_REQUEST &mreq)
handle MISSION_REQUEST mavlink msg handles and acts on misison request from FCU
std::vector< mavros_msgs::Waypoint > waypoints
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
ros::ServiceServer set_cur_srv
Definition: waypoint.cpp:81
Subscriptions get_subscriptions() override
Return vector of MAVLink message subscriptions (handlers)
Definition: waypoint.cpp:59


mavros
Author(s): Vladimir Ermakov
autogenerated on Tue Jun 1 2021 02:36:26