command.cpp
Go to the documentation of this file.
00001 
00009 /*
00010  * Copyright 2014 Vladimir Ermakov.
00011  *
00012  * This file is part of the mavros package and subject to the license terms
00013  * in the top-level LICENSE file of the mavros repository.
00014  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
00015  */
00016 
00017 #include <chrono>
00018 #include <condition_variable>
00019 #include <mavros/mavros_plugin.h>
00020 #include <pluginlib/class_list_macros.h>
00021 
00022 #include <mavros_msgs/CommandLong.h>
00023 #include <mavros_msgs/CommandInt.h>
00024 #include <mavros_msgs/CommandBool.h>
00025 #include <mavros_msgs/CommandHome.h>
00026 #include <mavros_msgs/CommandTOL.h>
00027 #include <mavros_msgs/CommandTriggerControl.h>
00028 
00029 namespace mavplugin {
00030 class CommandTransaction {
00031 public:
00032         std::mutex cond_mutex;
00033         std::condition_variable ack;
00034         uint16_t expected_command;
00035         uint8_t result;
00036 
00037         explicit CommandTransaction(uint16_t command) :
00038                 ack(),
00039                 expected_command(command),
00040                 result(MAV_RESULT_FAILED)
00041         { }
00042 };
00043 
00049 class CommandPlugin : public MavRosPlugin {
00050 public:
00051         CommandPlugin() :
00052                 uas(nullptr),
00053                 cmd_nh("~cmd"),
00054                 use_comp_id_system_control(false),
00055                 ACK_TIMEOUT_DT(ACK_TIMEOUT_MS / 1000.0)
00056         { };
00057 
00058         void initialize(UAS &uas_)
00059         {
00060                 uas = &uas_;
00061 
00062                 cmd_nh.param("use_comp_id_system_control", use_comp_id_system_control, false);
00063 
00064                 command_long_srv = cmd_nh.advertiseService("command", &CommandPlugin::command_long_cb, this);
00065                 command_int_srv = cmd_nh.advertiseService("command_int", &CommandPlugin::command_int_cb, this);
00066                 arming_srv = cmd_nh.advertiseService("arming", &CommandPlugin::arming_cb, this);
00067                 set_home_srv = cmd_nh.advertiseService("set_home", &CommandPlugin::set_home_cb, this);
00068                 takeoff_srv = cmd_nh.advertiseService("takeoff", &CommandPlugin::takeoff_cb, this);
00069                 land_srv = cmd_nh.advertiseService("land", &CommandPlugin::land_cb, this);
00070                 trigger_srv = cmd_nh.advertiseService("trigger_control", &CommandPlugin::trigger_control_cb, this);
00071         }
00072 
00073         const message_map get_rx_handlers() {
00074                 return {
00075                                MESSAGE_HANDLER(MAVLINK_MSG_ID_COMMAND_ACK, &CommandPlugin::handle_command_ack)
00076                 };
00077         }
00078 
00079 private:
00080         std::recursive_mutex mutex;
00081         UAS *uas;
00082 
00083         ros::NodeHandle cmd_nh;
00084         ros::ServiceServer command_long_srv;
00085         ros::ServiceServer command_int_srv;
00086         ros::ServiceServer arming_srv;
00087         ros::ServiceServer set_home_srv;
00088         ros::ServiceServer takeoff_srv;
00089         ros::ServiceServer land_srv;
00090         ros::ServiceServer trigger_srv;
00091 
00092         bool use_comp_id_system_control;
00093 
00094         std::list<CommandTransaction *> ack_waiting_list;
00095         static constexpr int ACK_TIMEOUT_MS = 5000;
00096 
00097         const ros::Duration ACK_TIMEOUT_DT;
00098 
00099         /* -*- message handlers -*- */
00100 
00101         void handle_command_ack(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00102                 mavlink_command_ack_t ack;
00103                 mavlink_msg_command_ack_decode(msg, &ack);
00104 
00105                 lock_guard lock(mutex);
00106                 for (auto it = ack_waiting_list.cbegin();
00107                                 it != ack_waiting_list.cend(); it++)
00108                         if ((*it)->expected_command == ack.command) {
00109                                 (*it)->result = ack.result;
00110                                 (*it)->ack.notify_all();
00111                                 return;
00112                         }
00113 
00114                 ROS_WARN_THROTTLE_NAMED(10, "cmd", "CMD: Unexpected command %u, result %u",
00115                                 ack.command, ack.result);
00116         }
00117 
00118         /* -*- mid-level functions -*- */
00119 
00120         bool wait_ack_for(CommandTransaction *tr) {
00121                 std::unique_lock<std::mutex> lock(tr->cond_mutex);
00122 
00123                 return tr->ack.wait_for(lock, std::chrono::nanoseconds(ACK_TIMEOUT_DT.toNSec()))
00124                        == std::cv_status::no_timeout;
00125         }
00126 
00132         bool send_command_long_and_wait(bool broadcast,
00133                         uint16_t command, uint8_t confirmation,
00134                         float param1, float param2,
00135                         float param3, float param4,
00136                         float param5, float param6,
00137                         float param7,
00138                         unsigned char &success, uint8_t &result)
00139         {
00140                 unique_lock lock(mutex);
00141 
00142                 /* check transactions */
00143                 for (auto it = ack_waiting_list.cbegin();
00144                                 it != ack_waiting_list.cend(); it++)
00145                         if ((*it)->expected_command == command) {
00146                                 ROS_WARN_THROTTLE_NAMED(10, "cmd", "CMD: Command %u alredy in progress", command);
00147                                 return false;
00148                         }
00149 
00154                 bool is_ack_required = (confirmation != 0 || uas->is_ardupilotmega()) && !uas->is_px4();
00155                 if (is_ack_required && !broadcast)
00156                         ack_waiting_list.push_back(new CommandTransaction(command));
00157 
00158                 command_long(broadcast,
00159                                 command, confirmation,
00160                                 param1, param2,
00161                                 param3, param4,
00162                                 param5, param6,
00163                                 param7);
00164 
00165                 if (is_ack_required && !broadcast) {
00166                         auto it = ack_waiting_list.begin();
00167                         for (; it != ack_waiting_list.end(); it++)
00168                                 if ((*it)->expected_command == command)
00169                                         break;
00170 
00171                         if (it == ack_waiting_list.end()) {
00172                                 ROS_ERROR_NAMED("cmd", "CMD: CommandTransaction not found for %u", command);
00173                                 return false;
00174                         }
00175 
00176                         lock.unlock();
00177                         bool is_not_timeout = wait_ack_for(*it);
00178                         lock.lock();
00179 
00180                         success = is_not_timeout && (*it)->result == MAV_RESULT_ACCEPTED;
00181                         result = (*it)->result;
00182 
00183                         delete *it;
00184                         ack_waiting_list.erase(it);
00185                 }
00186                 else {
00187                         success = true;
00188                         result = MAV_RESULT_ACCEPTED;
00189                 }
00190 
00191                 return true;
00192         }
00193 
00197         bool send_command_int(bool broadcast,
00198                         uint8_t frame, uint16_t command,
00199                         uint8_t current, uint8_t autocontinue,
00200                         float param1, float param2,
00201                         float param3, float param4,
00202                         int32_t x, int32_t y,
00203                         float z,
00204                         unsigned char &success) {
00205                 /* Note: seems that COMMAND_INT don't produce COMMAND_ACK
00206                  * so wait don't needed.
00207                  */
00208                 command_int(broadcast,
00209                                 frame, command, current, autocontinue,
00210                                 param1, param2,
00211                                 param3, param4,
00212                                 x, y, z);
00213 
00214                 success = true;
00215                 return true;
00216         }
00217 
00218         /* -*- low-level send -*- */
00219 
00220         void command_long(bool broadcast,
00221                         uint16_t command, uint8_t confirmation,
00222                         float param1, float param2,
00223                         float param3, float param4,
00224                         float param5, float param6,
00225                         float param7)
00226         {
00227                 mavlink_message_t msg;
00228                 const uint8_t tgt_sys_id = (broadcast) ? 0 : uas->get_tgt_system();
00229                 const uint8_t tgt_comp_id = (broadcast) ? 0 :
00230                         (use_comp_id_system_control) ?
00231                                 MAV_COMP_ID_SYSTEM_CONTROL : uas->get_tgt_component();
00232                 const uint8_t confirmation_fixed = (broadcast) ? 0 : confirmation;
00233 
00234                 mavlink_msg_command_long_pack_chan(UAS_PACK_CHAN(uas), &msg,
00235                                 tgt_sys_id,
00236                                 tgt_comp_id,
00237                                 command,
00238                                 confirmation_fixed,
00239                                 param1, param2,
00240                                 param3, param4,
00241                                 param5, param6,
00242                                 param7);
00243                 UAS_FCU(uas)->send_message(&msg);
00244         }
00245 
00246         void command_int(bool broadcast,
00247                         uint8_t frame, uint16_t command,
00248                         uint8_t current, uint8_t autocontinue,
00249                         float param1, float param2,
00250                         float param3, float param4,
00251                         int32_t x, int32_t y,
00252                         float z)
00253         {
00254                 mavlink_message_t msg;
00255                 const uint8_t tgt_sys_id = (broadcast) ? 0 : uas->get_tgt_system();
00256                 const uint8_t tgt_comp_id = (broadcast) ? 0 :
00257                         (use_comp_id_system_control) ?
00258                                 MAV_COMP_ID_SYSTEM_CONTROL : uas->get_tgt_component();
00259 
00260                 mavlink_msg_command_int_pack_chan(UAS_PACK_CHAN(uas), &msg,
00261                                 tgt_sys_id,
00262                                 tgt_comp_id,
00263                                 frame,
00264                                 command,
00265                                 current,
00266                                 autocontinue,
00267                                 param1, param2,
00268                                 param3, param4,
00269                                 x, y, z);
00270                 UAS_FCU(uas)->send_message(&msg);
00271         }
00272 
00273         /* -*- callbacks -*- */
00274 
00275         bool command_long_cb(mavros_msgs::CommandLong::Request &req,
00276                         mavros_msgs::CommandLong::Response &res) {
00277                 return send_command_long_and_wait(req.broadcast,
00278                                 req.command, req.confirmation,
00279                                 req.param1, req.param2,
00280                                 req.param3, req.param4,
00281                                 req.param5, req.param6,
00282                                 req.param7,
00283                                 res.success, res.result);
00284         }
00285 
00286         bool command_int_cb(mavros_msgs::CommandInt::Request &req,
00287                         mavros_msgs::CommandInt::Response &res) {
00288                 return send_command_int(req.broadcast,
00289                                 req.frame, req.command,
00290                                 req.current, req.autocontinue,
00291                                 req.param1, req.param2,
00292                                 req.param3, req.param4,
00293                                 req.x, req.y, req.z,
00294                                 res.success);
00295         }
00296 
00297         bool arming_cb(mavros_msgs::CommandBool::Request &req,
00298                         mavros_msgs::CommandBool::Response &res) {
00299                 return send_command_long_and_wait(false,
00300                                 MAV_CMD_COMPONENT_ARM_DISARM, 1,
00301                                 (req.value) ? 1.0 : 0.0,
00302                                 0, 0, 0, 0, 0, 0,
00303                                 res.success, res.result);
00304         }
00305 
00306         bool set_home_cb(mavros_msgs::CommandHome::Request &req,
00307                         mavros_msgs::CommandHome::Response &res) {
00308                 return send_command_long_and_wait(false,
00309                                 MAV_CMD_DO_SET_HOME, 1,
00310                                 (req.current_gps) ? 1.0 : 0.0,
00311                                 0, 0, 0, req.latitude, req.longitude, req.altitude,
00312                                 res.success, res.result);
00313         }
00314 
00315         bool takeoff_cb(mavros_msgs::CommandTOL::Request &req,
00316                         mavros_msgs::CommandTOL::Response &res) {
00317                 return send_command_long_and_wait(false,
00318                                 MAV_CMD_NAV_TAKEOFF, 1,
00319                                 req.min_pitch,
00320                                 0, 0,
00321                                 req.yaw,
00322                                 req.latitude, req.longitude, req.altitude,
00323                                 res.success, res.result);
00324         }
00325 
00326         bool land_cb(mavros_msgs::CommandTOL::Request &req,
00327                         mavros_msgs::CommandTOL::Response &res) {
00328                 return send_command_long_and_wait(false,
00329                                 MAV_CMD_NAV_LAND, 1,
00330                                 0, 0, 0,
00331                                 req.yaw,
00332                                 req.latitude, req.longitude, req.altitude,
00333                                 res.success, res.result);
00334         }
00335 
00336         bool trigger_control_cb(mavros_msgs::CommandTriggerControl::Request &req,
00337                         mavros_msgs::CommandTriggerControl::Response &res) {
00338                 return send_command_long_and_wait(false,
00339                                 MAV_CMD_DO_TRIGGER_CONTROL, 1,
00340                                 (req.trigger_enable)? 1.0 : 0.0,
00341                                 req.integration_time,
00342                                 0, 0, 0, 0, 0,
00343                                 res.success, res.result);
00344         }
00345 };
00346 };      // namespace mavplugin
00347 
00348 PLUGINLIB_EXPORT_CLASS(mavplugin::CommandPlugin, mavplugin::MavRosPlugin)
00349 


mavros
Author(s): Vladimir Ermakov
autogenerated on Thu Feb 9 2017 04:00:17