18 #include <condition_variable> 21 #include <mavros_msgs/CommandAck.h> 22 #include <mavros_msgs/CommandLong.h> 23 #include <mavros_msgs/CommandInt.h> 24 #include <mavros_msgs/CommandBool.h> 25 #include <mavros_msgs/CommandHome.h> 26 #include <mavros_msgs/CommandTOL.h> 27 #include <mavros_msgs/CommandTriggerControl.h> 28 #include <mavros_msgs/CommandTriggerInterval.h> 29 #include <mavros_msgs/CommandVtolTransition.h> 32 namespace std_plugins {
41 std::condition_variable
ack;
47 expected_command(command),
62 use_comp_id_system_control(false)
67 PluginBase::initialize(uas_);
69 double command_ack_timeout;
71 cmd_nh.param(
"command_ack_timeout", command_ack_timeout, ACK_TIMEOUT_DEFAULT);
72 cmd_nh.param(
"use_comp_id_system_control", use_comp_id_system_control,
false);
125 for (
auto &tr : ack_waiting_list) {
126 if (tr.expected_command == ack.command) {
127 tr.result = ack.result;
134 ack.command, ack.result);
141 if (tr.
ack.wait_for(lock, std::chrono::nanoseconds(command_ack_timeout_dt.
toNSec())) == std::cv_status::timeout) {
155 uint16_t command, uint8_t confirmation,
156 float param1,
float param2,
157 float param3,
float param4,
158 float param5,
float param6,
160 unsigned char &success, uint8_t &
result)
162 using mavlink::common::MAV_RESULT;
166 L_CommandTransaction::iterator ack_it;
169 for (
const auto &tr : ack_waiting_list) {
170 if (tr.expected_command == command) {
180 bool is_ack_required = (confirmation != 0 || m_uas->is_ardupilotmega() || m_uas->is_px4()) && !broadcast;
182 ack_it = ack_waiting_list.emplace(ack_waiting_list.end(), command);
184 command_long(broadcast,
185 command, confirmation,
191 if (is_ack_required) {
193 bool is_not_timeout = wait_ack_for(*ack_it);
196 success = is_not_timeout && ack_it->result ==
enum_value(MAV_RESULT::ACCEPTED);
197 result = ack_it->result;
199 ack_waiting_list.erase(ack_it);
213 uint8_t frame, uint16_t command,
214 uint8_t current, uint8_t autocontinue,
215 float param1,
float param2,
216 float param3,
float param4,
217 int32_t x, int32_t y,
219 unsigned char &success)
224 command_int(broadcast,
225 frame, command, current, autocontinue,
234 bool send_command_ack( uint16_t command, uint8_t req_result, uint8_t progress, int32_t result_param2,
235 unsigned char &success, uint8_t &res_result)
237 using mavlink::common::MAV_RESULT;
240 req_result, progress,
245 res_result =
enum_value(MAV_RESULT::ACCEPTED);
253 template<
typename MsgT>
256 using mavlink::minimal::MAV_COMPONENT;
258 const uint8_t tgt_sys_id = (broadcast) ? 0 : m_uas->get_tgt_system();
259 const uint8_t tgt_comp_id = (broadcast) ? 0 :
260 (use_comp_id_system_control) ?
261 enum_value(MAV_COMPONENT::COMP_ID_SYSTEM_CONTROL) : m_uas->get_tgt_component();
263 cmd.target_system = tgt_sys_id;
264 cmd.target_component = tgt_comp_id;
268 uint16_t command, uint8_t confirmation,
269 float param1,
float param2,
270 float param3,
float param4,
271 float param5,
float param6,
274 const uint8_t confirmation_fixed = (broadcast) ? 0 : confirmation;
276 mavlink::common::msg::COMMAND_LONG
cmd {};
277 set_target(
cmd, broadcast);
279 cmd.command = command;
280 cmd.confirmation = confirmation_fixed;
289 UAS_FCU(m_uas)->send_message_ignore_drop(
cmd);
293 uint8_t frame, uint16_t command,
294 uint8_t current, uint8_t autocontinue,
295 float param1,
float param2,
296 float param3,
float param4,
297 int32_t x, int32_t y,
300 mavlink::common::msg::COMMAND_INT
cmd {};
301 set_target(
cmd, broadcast);
304 cmd.command = command;
305 cmd.current = current;
306 cmd.autocontinue = autocontinue;
315 UAS_FCU(m_uas)->send_message_ignore_drop(
cmd);
319 uint8_t progress, int32_t result_param2)
321 mavlink::common::msg::COMMAND_ACK
cmd {};
322 set_target(
cmd,
false);
324 cmd.command = command;
326 cmd.progress = progress;
327 cmd.result_param2 = result_param2;
329 UAS_FCU(m_uas)->send_message_ignore_drop(
cmd);
336 mavros_msgs::CommandLong::Response &res)
338 return send_command_long_and_wait(req.broadcast,
339 req.command, req.confirmation,
340 req.param1, req.param2,
341 req.param3, req.param4,
342 req.param5, req.param6,
344 res.success, res.result);
348 mavros_msgs::CommandInt::Response &res)
350 return send_command_int(req.broadcast,
351 req.frame, req.command,
352 req.current, req.autocontinue,
353 req.param1, req.param2,
354 req.param3, req.param4,
360 mavros_msgs::CommandAck::Response &res)
362 return send_command_ack(req.command, req.result,
363 req.progress, req.result_param2,
364 res.success, res.result);
369 mavros_msgs::CommandBool::Response &res)
371 using mavlink::common::MAV_CMD;
372 return send_command_long_and_wait(
false,
374 (req.value) ? 1.0 : 0.0,
376 res.success, res.result);
380 mavros_msgs::CommandHome::Response &res)
382 using mavlink::common::MAV_CMD;
383 return send_command_long_and_wait(
false,
385 (req.current_gps) ? 1.0 : 0.0,
386 0, 0, req.yaw, req.latitude, req.longitude, req.altitude,
387 res.success, res.result);
391 mavros_msgs::CommandTOL::Response &res)
393 using mavlink::common::MAV_CMD;
394 return send_command_long_and_wait(
false,
399 req.latitude, req.longitude, req.altitude,
400 res.success, res.result);
403 bool land_cb(mavros_msgs::CommandTOL::Request &req,
404 mavros_msgs::CommandTOL::Response &res)
406 using mavlink::common::MAV_CMD;
407 return send_command_long_and_wait(
false,
411 req.latitude, req.longitude, req.altitude,
412 res.success, res.result);
416 mavros_msgs::CommandTriggerControl::Response &res)
418 using mavlink::common::MAV_CMD;
419 return send_command_long_and_wait(
false,
421 (req.trigger_enable) ? 1.0 : 0.0,
422 (req.sequence_reset) ? 1.0 : 0.0,
423 (req.trigger_pause) ? 1.0 : 0.0,
425 res.success, res.result);
429 mavros_msgs::CommandTriggerInterval::Response &res)
431 using mavlink::common::MAV_CMD;
434 return send_command_long_and_wait(
false,
435 enum_value(MAV_CMD::DO_SET_CAM_TRIGG_INTERVAL), 1,
437 req.integration_time,
439 res.success, res.result);
443 mavros_msgs::CommandVtolTransition::Response &res)
445 using mavlink::common::MAV_CMD;
446 return send_command_long_and_wait(
false,
447 enum_value(MAV_CMD::DO_VTOL_TRANSITION),
false,
450 res.success, res.result);
MAVROS Plugin base class.
#define ROS_WARN_THROTTLE_NAMED(period, name,...)
#define ROS_WARN_NAMED(name,...)
void set_target(MsgT &cmd, bool broadcast)
void initialize(UAS &uas_) override
Plugin initializer.
ros::ServiceServer trigger_interval_srv
bool send_command_ack(uint16_t command, uint8_t req_result, uint8_t progress, int32_t result_param2, unsigned char &success, uint8_t &res_result)
ros::ServiceServer arming_srv
bool command_ack_cb(mavros_msgs::CommandAck::Request &req, mavros_msgs::CommandAck::Response &res)
static constexpr double ACK_TIMEOUT_DEFAULT
std::lock_guard< std::mutex > lock_guard
ros::ServiceServer command_ack_srv
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)
bool trigger_control_cb(mavros_msgs::CommandTriggerControl::Request &req, mavros_msgs::CommandTriggerControl::Response &res)
std::list< CommandTransaction > L_CommandTransaction
bool command_long_cb(mavros_msgs::CommandLong::Request &req, mavros_msgs::CommandLong::Response &res)
ros::ServiceServer command_long_srv
ros::ServiceServer vtol_transition_srv
void handle_command_ack(const mavlink::mavlink_message_t *msg, mavlink::common::msg::COMMAND_ACK &ack)
ros::ServiceServer trigger_control_srv
uint16_t expected_command
bool arming_cb(mavros_msgs::CommandBool::Request &req, mavros_msgs::CommandBool::Response &res)
#define UAS_FCU(uasobjptr)
helper accessor to FCU link interface
Subscriptions get_subscriptions() override
Return vector of MAVLink message subscriptions (handlers)
bool vtol_transition_cb(mavros_msgs::CommandVtolTransition::Request &req, mavros_msgs::CommandVtolTransition::Response &res)
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)
CommandTransaction(uint16_t command)
bool trigger_interval_cb(mavros_msgs::CommandTriggerInterval::Request &req, mavros_msgs::CommandTriggerInterval::Response &res)
bool set_home_cb(mavros_msgs::CommandHome::Request &req, mavros_msgs::CommandHome::Response &res)
bool takeoff_cb(mavros_msgs::CommandTOL::Request &req, mavros_msgs::CommandTOL::Response &res)
std::vector< HandlerInfo > Subscriptions
Subscriptions vector.
std::condition_variable ack
bool use_comp_id_system_control
L_CommandTransaction ack_waiting_list
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)
bool command_int_cb(mavros_msgs::CommandInt::Request &req, mavros_msgs::CommandInt::Response &res)
ros::ServiceServer command_int_srv
std::unique_lock< std::mutex > unique_lock
bool land_cb(mavros_msgs::CommandTOL::Request &req, mavros_msgs::CommandTOL::Response &res)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
ros::ServiceServer takeoff_srv
ros::Duration command_ack_timeout_dt
bool wait_ack_for(CommandTransaction &tr)
ros::ServiceServer land_srv
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)
ros::ServiceServer set_home_srv
constexpr std::underlying_type< _T >::type enum_value(_T e)
std::recursive_mutex mutex
void command_ack(uint16_t command, uint8_t result, uint8_t progress, int32_t result_param2)