command.cpp
Go to the documentation of this file.
1 
9 /*
10  * Copyright 2014,2016 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 
17 #include <chrono>
18 #include <condition_variable>
19 #include <mavros/mavros_plugin.h>
20 
21 #include <mavros_msgs/CommandLong.h>
22 #include <mavros_msgs/CommandInt.h>
23 #include <mavros_msgs/CommandBool.h>
24 #include <mavros_msgs/CommandHome.h>
25 #include <mavros_msgs/CommandTOL.h>
26 #include <mavros_msgs/CommandTriggerControl.h>
27 #include <mavros_msgs/CommandTriggerInterval.h>
28 #include <mavros_msgs/CommandVtolTransition.h>
29 
30 namespace mavros {
31 namespace std_plugins {
32 static constexpr double ACK_TIMEOUT_DEFAULT = 5.0;
33 using utils::enum_value;
34 using lock_guard = std::lock_guard<std::mutex>;
35 using unique_lock = std::unique_lock<std::mutex>;
36 
38 public:
40  std::condition_variable ack;
41  uint16_t expected_command;
42  uint8_t result;
43 
44  explicit CommandTransaction(uint16_t command) :
45  ack(),
46  expected_command(command),
47  // Default result if wait ack timeout
48  result(enum_value(mavlink::common::MAV_RESULT::FAILED))
49  { }
50 };
51 
58 public:
59  CommandPlugin() : PluginBase(),
60  cmd_nh("~cmd"),
61  use_comp_id_system_control(false)
62  { }
63 
64  void initialize(UAS &uas_) override
65  {
66  PluginBase::initialize(uas_);
67 
68  double command_ack_timeout;
69 
70  cmd_nh.param("command_ack_timeout", command_ack_timeout, ACK_TIMEOUT_DEFAULT);
71  cmd_nh.param("use_comp_id_system_control", use_comp_id_system_control, false);
72 
73  command_ack_timeout_dt = ros::Duration(command_ack_timeout);
74 
75  command_long_srv = cmd_nh.advertiseService("command", &CommandPlugin::command_long_cb, this);
76  command_int_srv = cmd_nh.advertiseService("command_int", &CommandPlugin::command_int_cb, this);
77  arming_srv = cmd_nh.advertiseService("arming", &CommandPlugin::arming_cb, this);
78  set_home_srv = cmd_nh.advertiseService("set_home", &CommandPlugin::set_home_cb, this);
79  takeoff_srv = cmd_nh.advertiseService("takeoff", &CommandPlugin::takeoff_cb, this);
80  land_srv = cmd_nh.advertiseService("land", &CommandPlugin::land_cb, this);
81  trigger_control_srv = cmd_nh.advertiseService("trigger_control", &CommandPlugin::trigger_control_cb, this);
82  trigger_interval_srv = cmd_nh.advertiseService("trigger_interval", &CommandPlugin::trigger_interval_cb, this);
83  vtol_transition_srv = cmd_nh.advertiseService("vtol_transition", &CommandPlugin::vtol_transition_cb, this);
84  }
85 
87  {
88  return {
90  };
91  }
92 
93 private:
94  using L_CommandTransaction = std::list<CommandTransaction>;
95 
97 
108 
110 
113 
114  /* -*- message handlers -*- */
115 
116  void handle_command_ack(const mavlink::mavlink_message_t *msg, mavlink::common::msg::COMMAND_ACK &ack)
117  {
118  lock_guard lock(mutex);
119 
120  // XXX(vooon): place here source ids check
121 
122  for (auto &tr : ack_waiting_list) {
123  if (tr.expected_command == ack.command) {
124  tr.result = ack.result;
125  tr.ack.notify_all();
126  return;
127  }
128  }
129 
130  ROS_WARN_THROTTLE_NAMED(10, "cmd", "CMD: Unexpected command %u, result %u",
131  ack.command, ack.result);
132  }
133 
134  /* -*- mid-level functions -*- */
135 
137  unique_lock lock(tr.cond_mutex);
138  if (tr.ack.wait_for(lock, std::chrono::nanoseconds(command_ack_timeout_dt.toNSec())) == std::cv_status::timeout) {
139  ROS_WARN_NAMED("cmd", "CMD: Command %u -- wait ack timeout", tr.expected_command);
140  return false;
141  } else {
142  return true;
143  }
144  }
145 
151  bool send_command_long_and_wait(bool broadcast,
152  uint16_t command, uint8_t confirmation,
153  float param1, float param2,
154  float param3, float param4,
155  float param5, float param6,
156  float param7,
157  unsigned char &success, uint8_t &result)
158  {
159  using mavlink::common::MAV_RESULT;
160 
161  unique_lock lock(mutex);
162 
163  L_CommandTransaction::iterator ack_it;
164 
165  /* check transactions */
166  for (const auto &tr : ack_waiting_list) {
167  if (tr.expected_command == command) {
168  ROS_WARN_THROTTLE_NAMED(10, "cmd", "CMD: Command %u already in progress", command);
169  return false;
170  }
171  }
172 
177  bool is_ack_required = (confirmation != 0 || m_uas->is_ardupilotmega() || m_uas->is_px4()) && !broadcast;
178  if (is_ack_required)
179  ack_it = ack_waiting_list.emplace(ack_waiting_list.end(), command);
180 
181  command_long(broadcast,
182  command, confirmation,
183  param1, param2,
184  param3, param4,
185  param5, param6,
186  param7);
187 
188  if (is_ack_required) {
189  lock.unlock();
190  bool is_not_timeout = wait_ack_for(*ack_it);
191  lock.lock();
192 
193  success = is_not_timeout && ack_it->result == enum_value(MAV_RESULT::ACCEPTED);
194  result = ack_it->result;
195 
196  ack_waiting_list.erase(ack_it);
197  }
198  else {
199  success = true;
200  result = enum_value(MAV_RESULT::ACCEPTED);
201  }
202 
203  return true;
204  }
205 
209  bool send_command_int(bool broadcast,
210  uint8_t frame, uint16_t command,
211  uint8_t current, uint8_t autocontinue,
212  float param1, float param2,
213  float param3, float param4,
214  int32_t x, int32_t y,
215  float z,
216  unsigned char &success)
217  {
218  /* Note: seems that COMMAND_INT don't produce COMMAND_ACK
219  * so wait don't needed.
220  */
221  command_int(broadcast,
222  frame, command, current, autocontinue,
223  param1, param2,
224  param3, param4,
225  x, y, z);
226 
227  success = true;
228  return true;
229  }
230 
231  /* -*- low-level send -*- */
232 
233  template<typename MsgT>
234  inline void set_target(MsgT &cmd, bool broadcast)
235  {
236  using mavlink::minimal::MAV_COMPONENT;
237 
238  const uint8_t tgt_sys_id = (broadcast) ? 0 : m_uas->get_tgt_system();
239  const uint8_t tgt_comp_id = (broadcast) ? 0 :
240  (use_comp_id_system_control) ?
241  enum_value(MAV_COMPONENT::COMP_ID_SYSTEM_CONTROL) : m_uas->get_tgt_component();
242 
243  cmd.target_system = tgt_sys_id;
244  cmd.target_component = tgt_comp_id;
245  }
246 
247  void command_long(bool broadcast,
248  uint16_t command, uint8_t confirmation,
249  float param1, float param2,
250  float param3, float param4,
251  float param5, float param6,
252  float param7)
253  {
254  const uint8_t confirmation_fixed = (broadcast) ? 0 : confirmation;
255 
256  mavlink::common::msg::COMMAND_LONG cmd {};
257  set_target(cmd, broadcast);
258 
259  cmd.command = command;
260  cmd.confirmation = confirmation_fixed;
261  cmd.param1 = param1;
262  cmd.param2 = param2;
263  cmd.param3 = param3;
264  cmd.param4 = param4;
265  cmd.param5 = param5;
266  cmd.param6 = param6;
267  cmd.param7 = param7;
268 
269  UAS_FCU(m_uas)->send_message_ignore_drop(cmd);
270  }
271 
272  void command_int(bool broadcast,
273  uint8_t frame, uint16_t command,
274  uint8_t current, uint8_t autocontinue,
275  float param1, float param2,
276  float param3, float param4,
277  int32_t x, int32_t y,
278  float z)
279  {
280  mavlink::common::msg::COMMAND_INT cmd {};
281  set_target(cmd, broadcast);
282 
283  cmd.frame = frame;
284  cmd.command = command;
285  cmd.current = current;
286  cmd.autocontinue = autocontinue;
287  cmd.param1 = param1;
288  cmd.param2 = param2;
289  cmd.param3 = param3;
290  cmd.param4 = param4;
291  cmd.x = x;
292  cmd.y = y;
293  cmd.z = z;
294 
295  UAS_FCU(m_uas)->send_message_ignore_drop(cmd);
296  }
297 
298  /* -*- callbacks -*- */
299 
300  bool command_long_cb(mavros_msgs::CommandLong::Request &req,
301  mavros_msgs::CommandLong::Response &res)
302  {
303  return send_command_long_and_wait(req.broadcast,
304  req.command, req.confirmation,
305  req.param1, req.param2,
306  req.param3, req.param4,
307  req.param5, req.param6,
308  req.param7,
309  res.success, res.result);
310  }
311 
312  bool command_int_cb(mavros_msgs::CommandInt::Request &req,
313  mavros_msgs::CommandInt::Response &res)
314  {
315  return send_command_int(req.broadcast,
316  req.frame, req.command,
317  req.current, req.autocontinue,
318  req.param1, req.param2,
319  req.param3, req.param4,
320  req.x, req.y, req.z,
321  res.success);
322  }
323 
324  bool arming_cb(mavros_msgs::CommandBool::Request &req,
325  mavros_msgs::CommandBool::Response &res)
326  {
327  using mavlink::common::MAV_CMD;
328  return send_command_long_and_wait(false,
329  enum_value(MAV_CMD::COMPONENT_ARM_DISARM), 1,
330  (req.value) ? 1.0 : 0.0,
331  0, 0, 0, 0, 0, 0,
332  res.success, res.result);
333  }
334 
335  bool set_home_cb(mavros_msgs::CommandHome::Request &req,
336  mavros_msgs::CommandHome::Response &res)
337  {
338  using mavlink::common::MAV_CMD;
339  return send_command_long_and_wait(false,
340  enum_value(MAV_CMD::DO_SET_HOME), 1,
341  (req.current_gps) ? 1.0 : 0.0,
342  0, 0, req.yaw, req.latitude, req.longitude, req.altitude,
343  res.success, res.result);
344  }
345 
346  bool takeoff_cb(mavros_msgs::CommandTOL::Request &req,
347  mavros_msgs::CommandTOL::Response &res)
348  {
349  using mavlink::common::MAV_CMD;
350  return send_command_long_and_wait(false,
351  enum_value(MAV_CMD::NAV_TAKEOFF), 1,
352  req.min_pitch,
353  0, 0,
354  req.yaw,
355  req.latitude, req.longitude, req.altitude,
356  res.success, res.result);
357  }
358 
359  bool land_cb(mavros_msgs::CommandTOL::Request &req,
360  mavros_msgs::CommandTOL::Response &res)
361  {
362  using mavlink::common::MAV_CMD;
363  return send_command_long_and_wait(false,
364  enum_value(MAV_CMD::NAV_LAND), 1,
365  0, 0, 0,
366  req.yaw,
367  req.latitude, req.longitude, req.altitude,
368  res.success, res.result);
369  }
370 
371  bool trigger_control_cb(mavros_msgs::CommandTriggerControl::Request &req,
372  mavros_msgs::CommandTriggerControl::Response &res)
373  {
374  using mavlink::common::MAV_CMD;
375  return send_command_long_and_wait(false,
376  enum_value(MAV_CMD::DO_TRIGGER_CONTROL), 1,
377  (req.trigger_enable) ? 1.0 : 0.0,
378  (req.sequence_reset) ? 1.0 : 0.0,
379  (req.trigger_pause) ? 1.0 : 0.0,
380  0, 0, 0, 0,
381  res.success, res.result);
382  }
383 
384  bool trigger_interval_cb(mavros_msgs::CommandTriggerInterval::Request &req,
385  mavros_msgs::CommandTriggerInterval::Response &res)
386  {
387  using mavlink::common::MAV_CMD;
388 
389  // trigger interval can only be set when triggering is disabled
390  return send_command_long_and_wait(false,
391  enum_value(MAV_CMD::DO_SET_CAM_TRIGG_INTERVAL), 1,
392  req.cycle_time,
393  req.integration_time,
394  0, 0, 0, 0, 0,
395  res.success, res.result);
396  }
397 
398  bool vtol_transition_cb(mavros_msgs::CommandVtolTransition::Request &req,
399  mavros_msgs::CommandVtolTransition::Response &res)
400  {
401  using mavlink::common::MAV_CMD;
402  return send_command_long_and_wait(false,
403  enum_value(MAV_CMD::DO_VTOL_TRANSITION), false,
404  req.state,
405  0, 0, 0, 0, 0, 0,
406  res.success, res.result);
407  }
408 };
409 } // namespace std_plugins
410 } // namespace mavros
411 
MAVROS Plugin base class.
Definition: mavros_plugin.h:36
#define ROS_WARN_THROTTLE_NAMED(period, name,...)
#define ROS_WARN_NAMED(name,...)
void set_target(MsgT &cmd, bool broadcast)
Definition: command.cpp:234
void initialize(UAS &uas_) override
Plugin initializer.
Definition: command.cpp:64
MAVROS Plugin base.
string cmd
ros::ServiceServer trigger_interval_srv
Definition: command.cpp:106
ros::ServiceServer arming_srv
Definition: command.cpp:101
static constexpr double ACK_TIMEOUT_DEFAULT
Definition: command.cpp:32
std::lock_guard< std::mutex > lock_guard
Definition: command.cpp:34
void command_long(bool broadcast, uint16_t command, uint8_t confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7)
Definition: command.cpp:247
bool trigger_control_cb(mavros_msgs::CommandTriggerControl::Request &req, mavros_msgs::CommandTriggerControl::Response &res)
Definition: command.cpp:371
std::list< CommandTransaction > L_CommandTransaction
Definition: command.cpp:94
bool command_long_cb(mavros_msgs::CommandLong::Request &req, mavros_msgs::CommandLong::Response &res)
Definition: command.cpp:300
ros::ServiceServer command_long_srv
Definition: command.cpp:99
ros::ServiceServer vtol_transition_srv
Definition: command.cpp:107
void handle_command_ack(const mavlink::mavlink_message_t *msg, mavlink::common::msg::COMMAND_ACK &ack)
Definition: command.cpp:116
ros::ServiceServer trigger_control_srv
Definition: command.cpp:105
UAS for plugins.
Definition: mavros_uas.h:67
bool arming_cb(mavros_msgs::CommandBool::Request &req, mavros_msgs::CommandBool::Response &res)
Definition: command.cpp:324
#define UAS_FCU(uasobjptr)
helper accessor to FCU link interface
Definition: mavros_uas.h:42
Subscriptions get_subscriptions() override
Return vector of MAVLink message subscriptions (handlers)
Definition: command.cpp:86
bool vtol_transition_cb(mavros_msgs::CommandVtolTransition::Request &req, mavros_msgs::CommandVtolTransition::Response &res)
Definition: command.cpp:398
bool send_command_long_and_wait(bool broadcast, uint16_t command, uint8_t confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, unsigned char &success, uint8_t &result)
Definition: command.cpp:151
bool trigger_interval_cb(mavros_msgs::CommandTriggerInterval::Request &req, mavros_msgs::CommandTriggerInterval::Response &res)
Definition: command.cpp:384
bool set_home_cb(mavros_msgs::CommandHome::Request &req, mavros_msgs::CommandHome::Response &res)
Definition: command.cpp:335
bool takeoff_cb(mavros_msgs::CommandTOL::Request &req, mavros_msgs::CommandTOL::Response &res)
Definition: command.cpp:346
std::vector< HandlerInfo > Subscriptions
Subscriptions vector.
Definition: mavros_plugin.h:47
std::condition_variable ack
Definition: command.cpp:40
int64_t toNSec() const
L_CommandTransaction ack_waiting_list
Definition: command.cpp:111
bool send_command_int(bool broadcast, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z, unsigned char &success)
Definition: command.cpp:209
bool command_int_cb(mavros_msgs::CommandInt::Request &req, mavros_msgs::CommandInt::Response &res)
Definition: command.cpp:312
ros::ServiceServer command_int_srv
Definition: command.cpp:100
std::unique_lock< std::mutex > unique_lock
Definition: command.cpp:35
bool land_cb(mavros_msgs::CommandTOL::Request &req, mavros_msgs::CommandTOL::Response &res)
Definition: command.cpp:359
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
ros::ServiceServer takeoff_srv
Definition: command.cpp:103
bool wait_ack_for(CommandTransaction &tr)
Definition: command.cpp:136
void command_int(bool broadcast, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z)
Definition: command.cpp:272
ros::ServiceServer set_home_srv
Definition: command.cpp:102
constexpr std::underlying_type< _T >::type enum_value(_T e)
Definition: utils.h:55
std::recursive_mutex mutex


mavros
Author(s): Vladimir Ermakov
autogenerated on Fri May 7 2021 02:21:15