rallypoint.cpp
Go to the documentation of this file.
1 
9 /*
10  * Copyright 2021 Charlie Burge.
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 namespace mavros {
20 namespace std_plugins {
25 public:
27  MissionBase("RP"),
28  rp_nh("~rallypoint")
29  { }
30 
31  void initialize(UAS &uas_) override
32  {
33  PluginBase::initialize(uas_);
34  MissionBase::initialize(&rp_nh);
35 
37  wp_type = plugin::WP_TYPE::RALLY;
38 
39  rp_nh.param("pull_after_gcs", do_pull_after_gcs, true);
40  rp_nh.param("use_mission_item_int", use_mission_item_int, false);
41 
42  rp_list_pub = rp_nh.advertise<mavros_msgs::WaypointList>("waypoints", 2, true);
46 
49  }
50 
52  return {
59  };
60  }
61 
62 private:
64 
69 
70  /* -*- mid-level helpers -*- */
71 
72  // Acts when capabilities of the fcu are changed
73  void capabilities_cb(UAS::MAV_CAP capabilities) override {
74  lock_guard lock(mutex);
75  if (m_uas->has_capability(UAS::MAV_CAP::MISSION_INT)) {
76  use_mission_item_int = true;
78  ROS_INFO_NAMED(log_ns, "%s: Using MISSION_ITEM_INT", log_ns.c_str());
79  } else {
80  use_mission_item_int = false;
82  ROS_WARN_NAMED(log_ns, "%s: Falling back to MISSION_ITEM", log_ns.c_str());
83  }
84  }
85 
86  // Act on first heartbeat from FCU
87  void connection_cb(bool connected) override
88  {
89  lock_guard lock(mutex);
90  if (connected) {
92 
93  if (rp_nh.hasParam("enable_partial_push")) {
94  rp_nh.getParam("enable_partial_push", enable_partial_push);
95  }
96  else {
98  }
99  }
100  else {
102  }
103  }
104 
106  void publish_waypoints() override
107  {
108  auto wpl = boost::make_shared<mavros_msgs::WaypointList>();
109  unique_lock lock(mutex);
110 
111  wpl->current_seq = wp_cur_active;
112  wpl->waypoints.clear();
113  wpl->waypoints.reserve(waypoints.size());
114  for (auto &it : waypoints) {
115  wpl->waypoints.push_back(it);
116  }
117 
118  lock.unlock();
119  rp_list_pub.publish(wpl);
120  }
121 
122  /* -*- ROS callbacks -*- */
123 
124  bool pull_cb(mavros_msgs::WaypointPull::Request &req,
125  mavros_msgs::WaypointPull::Response &res)
126  {
127  unique_lock lock(mutex);
128 
129  if (wp_state != WP::IDLE)
130  // Wrong initial state, other operation in progress?
131  return false;
132 
134  wp_count = 0;
136 
137  lock.unlock();
139  res.success = wait_fetch_all();
140  lock.lock();
141 
142  res.wp_received = waypoints.size();
143  go_idle(); // not nessessary, but prevents from blocking
144  return true;
145  }
146 
147  bool push_cb(mavros_msgs::WaypointPush::Request &req,
148  mavros_msgs::WaypointPush::Response &res)
149  {
150  unique_lock lock(mutex);
151 
152  if (wp_state != WP::IDLE)
153  // Wrong initial state, other operation in progress?
154  return false;
155 
156  if (req.start_index) {
157  // Partial Waypoint update
158 
159  if (!enable_partial_push) {
160  ROS_WARN_NAMED(log_ns, "%s: Partial Push not enabled. (Only supported on APM)", log_ns.c_str());
161  res.success = false;
162  res.wp_transfered = 0;
163  return true;
164  }
165 
166  if (waypoints.size() < req.start_index + req.waypoints.size()) {
167  ROS_WARN_NAMED(log_ns, "%s: Partial push out of range rejected.", log_ns.c_str());
168  res.success = false;
169  res.wp_transfered = 0;
170  return true;
171  }
172 
175 
176  uint16_t seq = req.start_index;
177  for (auto &it : req.waypoints) {
178  send_waypoints[seq] = it;
179  seq++;
180  }
181 
182  wp_count = req.waypoints.size();
183  wp_start_id = req.start_index;
184  wp_end_id = req.start_index + wp_count;
185  wp_cur_id = req.start_index;
187 
188  lock.unlock();
190  res.success = wait_push_all();
191  lock.lock();
192 
193  res.wp_transfered = wp_cur_id - wp_start_id + 1;
194  }
195  else {
196  // Full waypoint update
198 
199  send_waypoints.clear();
200  send_waypoints.reserve(req.waypoints.size());
201  send_waypoints = req.waypoints;
202 
203  wp_count = send_waypoints.size();
205  wp_cur_id = 0;
207 
208  lock.unlock();
210  res.success = wait_push_all();
211  lock.lock();
212 
213  res.wp_transfered = wp_cur_id + 1;
214  }
215 
216  go_idle(); // same as in pull_cb
217  return true;
218  }
219 
220  bool clear_cb(mavros_msgs::WaypointClear::Request &req,
221  mavros_msgs::WaypointClear::Response &res)
222  {
223  unique_lock lock(mutex);
224 
225  if (wp_state != WP::IDLE)
226  return false;
227 
230 
231  lock.unlock();
233  res.success = wait_push_all();
234 
235  lock.lock();
236  go_idle(); // same as in pull_cb
237  return true;
238  }
239 };
240 } // namespace std_plugins
241 } // namespace mavros
242 
MAVROS Plugin base class.
Definition: mavros_plugin.h:36
#define ROS_INFO_NAMED(name,...)
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 ...
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 ...
UAS for plugins.
Definition: mavros_uas.h:67
void publish_waypoints() override
publish the updated waypoint list after operation
Definition: rallypoint.cpp:106
std::lock_guard< std::recursive_mutex > lock_guard
bool push_cb(mavros_msgs::WaypointPush::Request &req, mavros_msgs::WaypointPush::Response &res)
Definition: rallypoint.cpp:147
void capabilities_cb(UAS::MAV_CAP capabilities) override
Definition: rallypoint.cpp:73
bool param(const std::string &param_name, T &param_val, const T &default_val) const
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
Subscriptions get_subscriptions() override
Return vector of MAVLink message subscriptions (handlers)
Definition: rallypoint.cpp:51
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
Rallypoint manipulation plugin.
Definition: rallypoint.cpp:24
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.
bool pull_cb(mavros_msgs::WaypointPull::Request &req, mavros_msgs::WaypointPull::Response &res)
Definition: rallypoint.cpp:124
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 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 connection_cb(bool connected) override
Definition: rallypoint.cpp:87
void schedule_pull(const ros::Duration &dt)
void initialize(UAS &uas_) override
Plugin initializer.
Definition: rallypoint.cpp:31
bool hasParam(const std::string &key) const
bool clear_cb(mavros_msgs::WaypointClear::Request &req, mavros_msgs::WaypointClear::Response &res)
Definition: rallypoint.cpp:220
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)


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