waypoint.cpp
Go to the documentation of this file.
00001 
00009 /*
00010  * Copyright 2014,2015 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/WaypointList.h>
00023 #include <mavros_msgs/WaypointSetCurrent.h>
00024 #include <mavros_msgs/WaypointClear.h>
00025 #include <mavros_msgs/WaypointPull.h>
00026 #include <mavros_msgs/WaypointPush.h>
00027 
00028 namespace mavplugin {
00029 class WaypointItem {
00030 public:
00031         uint16_t seq;
00032         enum MAV_FRAME frame;
00033         enum MAV_CMD command;
00034         uint8_t current;/* APM use some magical numbers */
00035         bool autocontinue;
00036         float param1;
00037         float param2;
00038         float param3;
00039         float param4;
00040         double x_lat;
00041         double y_long;
00042         double z_alt;
00043 
00044         static mavros_msgs::Waypoint to_msg(WaypointItem &wp) {
00045                 mavros_msgs::Waypoint ret;
00046 
00047                 ret.frame = static_cast<uint8_t>(wp.frame);
00048                 ret.command = static_cast<uint16_t>(wp.command);
00049                 ret.is_current = !!wp.current;
00050                 ret.autocontinue = wp.autocontinue;
00051                 ret.param1 = wp.param1;
00052                 ret.param2 = wp.param2;
00053                 ret.param3 = wp.param3;
00054                 ret.param4 = wp.param4;
00055                 ret.x_lat = wp.x_lat;
00056                 ret.y_long = wp.y_long;
00057                 ret.z_alt = wp.z_alt;
00058 
00059                 return ret;
00060         }
00061 
00062         static WaypointItem from_msg(mavros_msgs::Waypoint &wp, uint16_t seq) {
00063                 WaypointItem ret;
00064 
00065                 ret.seq = seq;
00066                 ret.frame = static_cast<enum MAV_FRAME>(wp.frame);
00067                 ret.command = static_cast<enum MAV_CMD>(wp.command);
00068                 ret.current = wp.is_current;
00069                 ret.autocontinue = wp.autocontinue;
00070                 ret.param1 = wp.param1;
00071                 ret.param2 = wp.param2;
00072                 ret.param3 = wp.param3;
00073                 ret.param4 = wp.param4;
00074                 ret.x_lat = wp.x_lat;
00075                 ret.y_long = wp.y_long;
00076                 ret.z_alt = wp.z_alt;
00077 
00078                 return ret;
00079         }
00080 
00081         static WaypointItem from_mission_item(mavlink_mission_item_t &mit) {
00082                 WaypointItem ret;
00083 
00084                 ret.seq = mit.seq;
00085                 ret.frame = static_cast<enum MAV_FRAME>(mit.frame);
00086                 ret.command = static_cast<enum MAV_CMD>(mit.command);
00087                 ret.current = mit.current;
00088                 ret.autocontinue = mit.autocontinue;
00089                 ret.param1 = mit.param1;
00090                 ret.param2 = mit.param2;
00091                 ret.param3 = mit.param3;
00092                 ret.param4 = mit.param4;
00093                 ret.x_lat = mit.x;
00094                 ret.y_long = mit.y;
00095                 ret.z_alt = mit.z;
00096 
00097                 return ret;
00098         }
00099 
00100         static std::string to_string_frame(WaypointItem &wpi) {
00101                 switch (wpi.frame) {
00102                 case MAV_FRAME_GLOBAL:          return "GAA";
00103                 case MAV_FRAME_LOCAL_NED:       return "LNED";
00104                 case MAV_FRAME_MISSION:         return "MIS";
00105                 case MAV_FRAME_GLOBAL_RELATIVE_ALT:     return "GRA";
00106                 case MAV_FRAME_LOCAL_ENU:       return "LENU";
00107                 default:
00108                         std::ostringstream unk;
00109                         unk << "UNK " << (int)wpi.frame;
00110                         return unk.str();
00111                 }
00112         }
00113 
00114         static std::string to_string_command(WaypointItem &wpi) {
00115                 switch (wpi.command) {
00116                 case MAV_CMD_NAV_WAYPOINT:      return "WAYPOINT";
00117                 case MAV_CMD_NAV_LOITER_UNLIM:  return "LOITER UNLIM";
00118                 case MAV_CMD_NAV_LOITER_TURNS:  return "LOITER TURNS";
00119                 case MAV_CMD_NAV_LOITER_TIME:   return "LOITER TIME";
00120                 case MAV_CMD_NAV_RETURN_TO_LAUNCH:      return "RTL";
00121                 case MAV_CMD_NAV_LAND:          return "LAND";
00122                 case MAV_CMD_NAV_TAKEOFF:       return "TAKEOFF";
00123                 case MAV_CMD_NAV_ROI:           return "ROI";
00124                 case MAV_CMD_NAV_PATHPLANNING:  return "PATH PLANNING";
00125                 default:
00126                         std::ostringstream unk;
00127                         unk << "UNK " << (int)wpi.command;
00128                         return unk.str();
00129                 }
00130         }
00131 };
00132 
00133 
00137 class WaypointPlugin : public MavRosPlugin {
00138 public:
00139         WaypointPlugin() :
00140                 wp_nh("~mission"),
00141                 uas(nullptr),
00142                 wp_state(WP_IDLE),
00143                 wp_count(0),
00144                 wp_retries(RETRIES_COUNT),
00145                 wp_cur_id(0),
00146                 wp_cur_active(0),
00147                 wp_set_active(0),
00148                 is_timedout(false),
00149                 do_pull_after_gcs(false),
00150                 reshedule_pull(false),
00151                 BOOTUP_TIME_DT(BOOTUP_TIME_MS / 1000.0),
00152                 LIST_TIMEOUT_DT(LIST_TIMEOUT_MS / 1000.0),
00153                 WP_TIMEOUT_DT(WP_TIMEOUT_MS / 1000.0),
00154                 RESHEDULE_DT(RESHEDULE_MS / 1000.0)
00155         { };
00156 
00157         void initialize(UAS &uas_)
00158         {
00159                 uas = &uas_;
00160                 wp_state = WP_IDLE;
00161 
00162                 wp_nh.param("pull_after_gcs", do_pull_after_gcs, false);
00163 
00164                 wp_list_pub = wp_nh.advertise<mavros_msgs::WaypointList>("waypoints", 2, true);
00165                 pull_srv = wp_nh.advertiseService("pull", &WaypointPlugin::pull_cb, this);
00166                 push_srv = wp_nh.advertiseService("push", &WaypointPlugin::push_cb, this);
00167                 clear_srv = wp_nh.advertiseService("clear", &WaypointPlugin::clear_cb, this);
00168                 set_cur_srv = wp_nh.advertiseService("set_current", &WaypointPlugin::set_cur_cb, this);
00169 
00170                 wp_timer = wp_nh.createTimer(WP_TIMEOUT_DT, &WaypointPlugin::timeout_cb, this, true);
00171                 wp_timer.stop();
00172                 shedule_timer = wp_nh.createTimer(BOOTUP_TIME_DT, &WaypointPlugin::sheduled_pull_cb, this, true);
00173                 shedule_timer.stop();
00174                 uas->sig_connection_changed.connect(boost::bind(&WaypointPlugin::connection_cb, this, _1));
00175         };
00176 
00177         const message_map get_rx_handlers() {
00178                 return {
00179                                MESSAGE_HANDLER(MAVLINK_MSG_ID_MISSION_ITEM, &WaypointPlugin::handle_mission_item),
00180                                MESSAGE_HANDLER(MAVLINK_MSG_ID_MISSION_REQUEST, &WaypointPlugin::handle_mission_request),
00181                                MESSAGE_HANDLER(MAVLINK_MSG_ID_MISSION_CURRENT, &WaypointPlugin::handle_mission_current),
00182                                MESSAGE_HANDLER(MAVLINK_MSG_ID_MISSION_COUNT, &WaypointPlugin::handle_mission_count),
00183                                MESSAGE_HANDLER(MAVLINK_MSG_ID_MISSION_ITEM_REACHED, &WaypointPlugin::handle_mission_item_reached),
00184                                MESSAGE_HANDLER(MAVLINK_MSG_ID_MISSION_ACK, &WaypointPlugin::handle_mission_ack),
00185                 };
00186         }
00187 
00188 private:
00189         std::recursive_mutex mutex;
00190         ros::NodeHandle wp_nh;
00191         UAS *uas;
00192 
00193         ros::Publisher wp_list_pub;
00194         ros::ServiceServer pull_srv;
00195         ros::ServiceServer push_srv;
00196         ros::ServiceServer clear_srv;
00197         ros::ServiceServer set_cur_srv;
00198 
00199         std::vector<WaypointItem> waypoints;
00200         std::vector<WaypointItem> send_waypoints;
00201         enum {
00202                 WP_IDLE,
00203                 WP_RXLIST,
00204                 WP_RXWP,
00205                 WP_TXLIST,
00206                 WP_TXWP,
00207                 WP_CLEAR,
00208                 WP_SET_CUR
00209         } wp_state;
00210 
00211         size_t wp_count;
00212         size_t wp_cur_id;
00213         size_t wp_cur_active;
00214         size_t wp_set_active;
00215         size_t wp_retries;
00216         bool is_timedout;
00217         std::mutex recv_cond_mutex;
00218         std::mutex send_cond_mutex;
00219         std::condition_variable list_receiving;
00220         std::condition_variable list_sending;
00221 
00222         ros::Timer wp_timer;
00223         ros::Timer shedule_timer;
00224         bool do_pull_after_gcs;
00225         bool reshedule_pull;
00226 
00227         static constexpr int BOOTUP_TIME_MS = 15000;    
00228         static constexpr int LIST_TIMEOUT_MS = 30000;   
00229         static constexpr int WP_TIMEOUT_MS = 1000;
00230         static constexpr int RESHEDULE_MS = 5000;
00231         static constexpr int RETRIES_COUNT = 3;
00232 
00233         const ros::Duration BOOTUP_TIME_DT;
00234         const ros::Duration LIST_TIMEOUT_DT;
00235         const ros::Duration WP_TIMEOUT_DT;
00236         const ros::Duration RESHEDULE_DT;
00237 
00238         /* -*- rx handlers -*- */
00239 
00240         void handle_mission_item(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00241                 mavlink_mission_item_t mit;
00242                 mavlink_msg_mission_item_decode(msg, &mit);
00243                 WaypointItem wpi = WaypointItem::from_mission_item(mit);
00244                 unique_lock lock(mutex);
00245 
00246                 /* receive item only in RX state */
00247                 if (wp_state == WP_RXWP) {
00248                         if (mit.seq != wp_cur_id) {
00249                                 ROS_WARN_NAMED("wp", "WP: Seq mismatch, dropping item (%d != %zu)",
00250                                                 mit.seq, wp_cur_id);
00251                                 return;
00252                         }
00253 
00254                         ROS_INFO_STREAM_NAMED("wp", "WP: item #" << wpi.seq <<
00255                                         " " << WaypointItem::to_string_frame(wpi) <<
00256                                         " " << WaypointItem::to_string_command(wpi) <<
00257                                         ((wpi.current) ? " CUR" : "    ") <<
00258                                         " params: " << wpi.param1 <<
00259                                         ", " << wpi.param2 <<
00260                                         ", " << wpi.param3 <<
00261                                         ", " << wpi.param4 <<
00262                                         " x: " << wpi.x_lat <<
00263                                         " y: " << wpi.y_long <<
00264                                         " z: " << wpi.z_alt);
00265 
00266                         waypoints.push_back(wpi);
00267                         if (++wp_cur_id < wp_count) {
00268                                 restart_timeout_timer();
00269                                 mission_request(wp_cur_id);
00270                         }
00271                         else {
00272                                 request_mission_done();
00273                                 lock.unlock();
00274                                 publish_waypoints();
00275                         }
00276                 }
00277                 else {
00278                         ROS_DEBUG_NAMED("wp", "WP: rejecting item, wrong state %d", wp_state);
00279                         if (do_pull_after_gcs && reshedule_pull) {
00280                                 ROS_DEBUG_NAMED("wp", "WP: reshedule pull");
00281                                 shedule_pull(WP_TIMEOUT_DT);
00282                         }
00283                 }
00284         }
00285 
00286         void handle_mission_request(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00287                 mavlink_mission_request_t mreq;
00288                 mavlink_msg_mission_request_decode(msg, &mreq);
00289                 lock_guard lock(mutex);
00290 
00291                 if ((wp_state == WP_TXLIST && mreq.seq == 0) || (wp_state == WP_TXWP)) {
00292                         if (mreq.seq != wp_cur_id && mreq.seq != wp_cur_id + 1) {
00293                                 ROS_WARN_NAMED("wp", "WP: Seq mismatch, dropping request (%d != %zu)",
00294                                                 mreq.seq, wp_cur_id);
00295                                 return;
00296                         }
00297 
00298                         restart_timeout_timer();
00299                         if (mreq.seq < send_waypoints.size()) {
00300                                 wp_state = WP_TXWP;
00301                                 wp_cur_id = mreq.seq;
00302                                 send_waypoint(wp_cur_id);
00303                         }
00304                         else
00305                                 ROS_ERROR_NAMED("wp", "WP: FCU require seq out of range");
00306                 }
00307                 else
00308                         ROS_DEBUG_NAMED("wp", "WP: rejecting request, wrong state %d", wp_state);
00309         }
00310 
00311         void handle_mission_current(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00312                 mavlink_mission_current_t mcur;
00313                 mavlink_msg_mission_current_decode(msg, &mcur);
00314                 unique_lock lock(mutex);
00315 
00316                 if (wp_state == WP_SET_CUR) {
00317                         /* MISSION_SET_CURRENT ACK */
00318                         ROS_DEBUG_NAMED("wp", "WP: set current #%d done", mcur.seq);
00319                         go_idle();
00320                         wp_cur_active = mcur.seq;
00321                         set_current_waypoint(wp_cur_active);
00322 
00323                         lock.unlock();
00324                         list_sending.notify_all();
00325                         publish_waypoints();
00326                 }
00327                 else if (wp_state == WP_IDLE && wp_cur_active != mcur.seq) {
00328                         /* update active */
00329                         ROS_DEBUG_NAMED("wp", "WP: update current #%d", mcur.seq);
00330                         wp_cur_active = mcur.seq;
00331                         set_current_waypoint(wp_cur_active);
00332 
00333                         lock.unlock();
00334                         publish_waypoints();
00335                 }
00336         }
00337 
00338         void handle_mission_count(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00339                 mavlink_mission_count_t mcnt;
00340                 mavlink_msg_mission_count_decode(msg, &mcnt);
00341                 unique_lock lock(mutex);
00342 
00343                 if (wp_state == WP_RXLIST) {
00344                         /* FCU report of MISSION_REQUEST_LIST */
00345                         ROS_DEBUG_NAMED("wp", "WP: count %d", mcnt.count);
00346 
00347                         wp_count = mcnt.count;
00348                         wp_cur_id = 0;
00349 
00350                         waypoints.clear();
00351                         waypoints.reserve(wp_count);
00352 
00353                         if (wp_count > 0) {
00354                                 wp_state = WP_RXWP;
00355                                 restart_timeout_timer();
00356                                 mission_request(wp_cur_id);
00357                         }
00358                         else {
00359                                 request_mission_done();
00360                                 lock.unlock();
00361                                 publish_waypoints();
00362                         }
00363                 }
00364                 else {
00365                         ROS_INFO_NAMED("wp", "WP: seems GCS requesting mission");
00366                         /* shedule pull after GCS done */
00367                         if (do_pull_after_gcs) {
00368                                 ROS_INFO_NAMED("wp", "WP: sheduling pull after GCS is done");
00369                                 reshedule_pull = true;
00370                                 shedule_pull(RESHEDULE_DT);
00371                         }
00372                 }
00373         }
00374 
00375         void handle_mission_item_reached(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00376                 mavlink_mission_item_reached_t mitr;
00377                 mavlink_msg_mission_item_reached_decode(msg, &mitr);
00378 
00379                 /* in QGC used as informational message */
00380                 ROS_INFO_NAMED("wp", "WP: reached #%d", mitr.seq);
00381         }
00382 
00383         void handle_mission_ack(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00384                 mavlink_mission_ack_t mack;
00385                 mavlink_msg_mission_ack_decode(msg, &mack);
00386                 unique_lock lock(mutex);
00387 
00388                 if ((wp_state == WP_TXLIST || wp_state == WP_TXWP)
00389                                 && (wp_cur_id == send_waypoints.size() - 1)
00390                                 && (mack.type == MAV_MISSION_ACCEPTED)) {
00391                         go_idle();
00392                         waypoints = send_waypoints;
00393                         send_waypoints.clear();
00394 
00395                         lock.unlock();
00396                         list_sending.notify_all();
00397                         publish_waypoints();
00398                         ROS_INFO_NAMED("wp", "WP: mission sended");
00399                 }
00400                 else if (wp_state == WP_TXLIST || wp_state == WP_TXWP) {
00401                         go_idle();
00402                         /* use this flag for failure report */
00403                         is_timedout = true;
00404                         lock.unlock();
00405                         list_sending.notify_all();
00406 
00407                         switch (mack.type) {
00408                         case MAV_MISSION_ERROR:
00409                                 ROS_ERROR_NAMED("wp", "WP: upload failed: general error");
00410                                 break;
00411 
00412                         case MAV_MISSION_UNSUPPORTED_FRAME:
00413                                 ROS_ERROR_NAMED("wp", "WP: upload failed: unsupported frame");
00414                                 break;
00415 
00416                         case MAV_MISSION_UNSUPPORTED:
00417                                 ROS_ERROR_NAMED("wp", "WP: upload failed: command unsupported");
00418                                 break;
00419 
00420                         case MAV_MISSION_NO_SPACE:
00421                                 ROS_ERROR_NAMED("wp", "WP: upload failed: no space left on mission storage");
00422                                 break;
00423 
00424                         case MAV_MISSION_DENIED:
00425                                 ROS_ERROR_NAMED("wp", "WP: upload failed: denied");
00426                                 break;
00427 
00428                         default:
00429                                 ROS_ERROR_NAMED("wp", "WP: upload failed: error #%d", mack.type);
00430                                 break;
00431                         }
00432                 }
00433                 else if (wp_state == WP_CLEAR) {
00434                         go_idle();
00435                         if (mack.type != MAV_MISSION_ACCEPTED) {
00436                                 is_timedout = true;
00437                                 lock.unlock();
00438                                 ROS_ERROR_NAMED("wp", "WP: clear failed: error #%d", mack.type);
00439                         }
00440                         else {
00441                                 waypoints.clear();
00442                                 lock.unlock();
00443                                 publish_waypoints();
00444                                 ROS_INFO_NAMED("wp", "WP: mission cleared");
00445                         }
00446 
00447                         list_sending.notify_all();
00448                 }
00449                 else
00450                         ROS_DEBUG_NAMED("wp", "WP: not planned ACK, type: %d", mack.type);
00451         }
00452 
00453         /* -*- mid-level helpers -*- */
00454 
00455         void timeout_cb(const ros::TimerEvent &event) {
00456                 unique_lock lock(mutex);
00457                 if (wp_retries > 0) {
00458                         wp_retries--;
00459                         ROS_WARN_NAMED("wp", "WP: timeout, retries left %zu", wp_retries);
00460 
00461                         switch (wp_state) {
00462                         case WP_RXLIST:
00463                                 mission_request_list();
00464                                 break;
00465                         case WP_RXWP:
00466                                 mission_request(wp_cur_id);
00467                                 break;
00468                         case WP_TXLIST:
00469                                 mission_count(wp_count);
00470                                 break;
00471                         case WP_TXWP:
00472                                 send_waypoint(wp_cur_id);
00473                                 break;
00474                         case WP_CLEAR:
00475                                 mission_clear_all();
00476                                 break;
00477                         case WP_SET_CUR:
00478                                 mission_set_current(wp_set_active);
00479                                 break;
00480 
00481                         case WP_IDLE:
00482                                 break;
00483                         }
00484 
00485                         restart_timeout_timer_int();
00486                 }
00487                 else {
00488                         ROS_ERROR_NAMED("wp", "WP: timed out.");
00489                         go_idle();
00490                         is_timedout = true;
00491                         /* prevent waiting cond var timeout */
00492                         lock.unlock();
00493                         list_receiving.notify_all();
00494                         list_sending.notify_all();
00495                 }
00496         }
00497 
00498         void connection_cb(bool connected) {
00499                 lock_guard lock(mutex);
00500                 if (connected)
00501                         shedule_pull(BOOTUP_TIME_DT);
00502                 else
00503                         shedule_timer.stop();
00504         }
00505 
00506         void sheduled_pull_cb(const ros::TimerEvent &event) {
00507                 lock_guard lock(mutex);
00508                 if (wp_state != WP_IDLE) {
00509                         /* try later */
00510                         ROS_DEBUG_NAMED("wp", "WP: busy, reshedule pull");
00511                         shedule_pull(RESHEDULE_DT);
00512                         return;
00513                 }
00514 
00515                 ROS_DEBUG_NAMED("wp", "WP: start sheduled pull");
00516                 wp_state = WP_RXLIST;
00517                 wp_count = 0;
00518                 restart_timeout_timer();
00519                 mission_request_list();
00520         }
00521 
00522         void request_mission_done(void) {
00523                 /* possibly not needed if count == 0 (QGC impl) */
00524                 mission_ack(MAV_MISSION_ACCEPTED);
00525 
00526                 go_idle();
00527                 list_receiving.notify_all();
00528                 ROS_INFO_NAMED("wp", "WP: mission received");
00529         }
00530 
00531         void go_idle(void) {
00532                 reshedule_pull = false;
00533                 wp_state = WP_IDLE;
00534                 wp_timer.stop();
00535         }
00536 
00537         void restart_timeout_timer(void) {
00538                 wp_retries = RETRIES_COUNT;
00539                 restart_timeout_timer_int();
00540         }
00541 
00542         void restart_timeout_timer_int(void) {
00543                 is_timedout = false;
00544                 wp_timer.stop();
00545                 wp_timer.start();
00546         }
00547 
00548         void shedule_pull(const ros::Duration &dt) {
00549                 shedule_timer.stop();
00550                 shedule_timer.setPeriod(dt);
00551                 shedule_timer.start();
00552         }
00553 
00554         void send_waypoint(size_t seq) {
00555                 if (seq < send_waypoints.size()) {
00556                         WaypointItem wpi = send_waypoints.at(seq);
00557                         mission_item(wpi);
00558 
00559                         ROS_DEBUG_STREAM_NAMED("wp", "WP: send item #" << wpi.seq <<
00560                                         " " << WaypointItem::to_string_frame(wpi) <<
00561                                         " " << WaypointItem::to_string_command(wpi) <<
00562                                         ((wpi.current) ? " CUR" : "    ") <<
00563                                         " params: " << wpi.param1 <<
00564                                         ", " << wpi.param2 <<
00565                                         ", " << wpi.param3 <<
00566                                         ", " << wpi.param4 <<
00567                                         " x: " << wpi.x_lat <<
00568                                         " y: " << wpi.y_long <<
00569                                         " z: " << wpi.z_alt);
00570                 }
00571         }
00572 
00573         bool wait_fetch_all() {
00574                 std::unique_lock<std::mutex> lock(recv_cond_mutex);
00575 
00576                 return list_receiving.wait_for(lock, std::chrono::nanoseconds(LIST_TIMEOUT_DT.toNSec()))
00577                        == std::cv_status::no_timeout
00578                        && !is_timedout;
00579         }
00580 
00581         bool wait_push_all() {
00582                 std::unique_lock<std::mutex> lock(send_cond_mutex);
00583 
00584                 return list_sending.wait_for(lock, std::chrono::nanoseconds(LIST_TIMEOUT_DT.toNSec()))
00585                        == std::cv_status::no_timeout
00586                        && !is_timedout;
00587         }
00588 
00589         void set_current_waypoint(size_t seq) {
00590                 for (auto &it : waypoints)
00591                         it.current = (it.seq == seq) ? true : false;
00592         }
00593 
00594         void publish_waypoints() {
00595                 auto wpl = boost::make_shared<mavros_msgs::WaypointList>();
00596                 unique_lock lock(mutex);
00597 
00598                 wpl->waypoints.clear();
00599                 wpl->waypoints.reserve(waypoints.size());
00600                 for (auto &it : waypoints) {
00601                         wpl->waypoints.push_back(WaypointItem::to_msg(it));
00602                 }
00603 
00604                 lock.unlock();
00605                 wp_list_pub.publish(wpl);
00606         }
00607 
00608         /* -*- low-level send functions -*- */
00609 
00610         void mission_item(WaypointItem &wp) {
00611                 mavlink_message_t msg;
00612 
00613                 mavlink_msg_mission_item_pack_chan(UAS_PACK_CHAN(uas), &msg,
00614                                 UAS_PACK_TGT(uas),
00615                                 wp.seq,
00616                                 wp.frame,
00617                                 wp.command,
00618                                 wp.current,
00619                                 wp.autocontinue,
00620                                 wp.param1,
00621                                 wp.param2,
00622                                 wp.param3,
00623                                 wp.param4,
00624                                 wp.x_lat,
00625                                 wp.y_long,
00626                                 wp.z_alt
00627                                 );
00628                 UAS_FCU(uas)->send_message(&msg);
00629         }
00630 
00631         void mission_request(uint16_t seq) {
00632                 mavlink_message_t msg;
00633 
00634                 ROS_DEBUG_NAMED("wp", "WP:m: request #%u", seq);
00635                 mavlink_msg_mission_request_pack_chan(UAS_PACK_CHAN(uas), &msg,
00636                                 UAS_PACK_TGT(uas),
00637                                 seq
00638                                 );
00639                 UAS_FCU(uas)->send_message(&msg);
00640         }
00641 
00642         void mission_set_current(uint16_t seq) {
00643                 mavlink_message_t msg;
00644 
00645                 ROS_DEBUG_NAMED("wp", "WP:m: set current #%u", seq);
00646                 mavlink_msg_mission_set_current_pack_chan(UAS_PACK_CHAN(uas), &msg,
00647                                 UAS_PACK_TGT(uas),
00648                                 seq
00649                                 );
00650                 UAS_FCU(uas)->send_message(&msg);
00651         }
00652 
00653         void mission_request_list() {
00654                 mavlink_message_t msg;
00655 
00656                 ROS_DEBUG_NAMED("wp", "WP:m: request list");
00657                 mavlink_msg_mission_request_list_pack_chan(UAS_PACK_CHAN(uas), &msg,
00658                                 UAS_PACK_TGT(uas)
00659                                 );
00660                 UAS_FCU(uas)->send_message(&msg);
00661         }
00662 
00663         void mission_count(uint16_t cnt) {
00664                 mavlink_message_t msg;
00665 
00666                 ROS_DEBUG_NAMED("wp", "WP:m: count %u", cnt);
00667                 mavlink_msg_mission_count_pack_chan(UAS_PACK_CHAN(uas), &msg,
00668                                 UAS_PACK_TGT(uas),
00669                                 cnt
00670                                 );
00671                 UAS_FCU(uas)->send_message(&msg);
00672         }
00673 
00674         void mission_clear_all() {
00675                 mavlink_message_t msg;
00676 
00677                 ROS_DEBUG_NAMED("wp", "WP:m: clear all");
00678                 mavlink_msg_mission_clear_all_pack_chan(UAS_PACK_CHAN(uas), &msg,
00679                                 UAS_PACK_TGT(uas)
00680                                 );
00681                 UAS_FCU(uas)->send_message(&msg);
00682         }
00683 
00684         void mission_ack(enum MAV_MISSION_RESULT type) {
00685                 mavlink_message_t msg;
00686 
00687                 ROS_DEBUG_NAMED("wp", "WP:m: ACK %u", type);
00688                 mavlink_msg_mission_ack_pack_chan(UAS_PACK_CHAN(uas), &msg,
00689                                 UAS_PACK_TGT(uas),
00690                                 type
00691                                 );
00692                 UAS_FCU(uas)->send_message(&msg);
00693         }
00694 
00695         /* -*- ROS callbacks -*- */
00696 
00697         bool pull_cb(mavros_msgs::WaypointPull::Request &req,
00698                         mavros_msgs::WaypointPull::Response &res) {
00699                 unique_lock lock(mutex);
00700 
00701                 if (wp_state != WP_IDLE)
00702                         // Wrong initial state, other operation in progress?
00703                         return false;
00704 
00705                 wp_state = WP_RXLIST;
00706                 wp_count = 0;
00707                 restart_timeout_timer();
00708 
00709                 lock.unlock();
00710                 mission_request_list();
00711                 res.success = wait_fetch_all();
00712                 lock.lock();
00713 
00714                 res.wp_received = waypoints.size();
00715                 go_idle();      // not nessessary, but prevents from blocking
00716                 return true;
00717         }
00718 
00719         bool push_cb(mavros_msgs::WaypointPush::Request &req,
00720                         mavros_msgs::WaypointPush::Response &res) {
00721                 unique_lock lock(mutex);
00722 
00723                 if (wp_state != WP_IDLE)
00724                         // Wrong initial state, other operation in progress?
00725                         return false;
00726 
00727                 wp_state = WP_TXLIST;
00728 
00729                 send_waypoints.clear();
00730                 send_waypoints.reserve(req.waypoints.size());
00731                 uint16_t seq = 0;
00732                 for (auto &it : req.waypoints) {
00733                         send_waypoints.push_back(WaypointItem::from_msg(it, seq++));
00734                 }
00735 
00736                 wp_count = send_waypoints.size();
00737                 wp_cur_id = 0;
00738                 restart_timeout_timer();
00739 
00740                 lock.unlock();
00741                 mission_count(wp_count);
00742                 res.success = wait_push_all();
00743                 lock.lock();
00744 
00745                 res.wp_transfered = wp_cur_id + 1;
00746                 go_idle();      // same as in pull_cb
00747                 return true;
00748         }
00749 
00750         bool clear_cb(mavros_msgs::WaypointClear::Request &req,
00751                         mavros_msgs::WaypointClear::Response &res) {
00752                 unique_lock lock(mutex);
00753 
00754                 if (wp_state != WP_IDLE)
00755                         return false;
00756 
00757                 wp_state = WP_CLEAR;
00758                 restart_timeout_timer();
00759 
00760                 lock.unlock();
00761                 mission_clear_all();
00762                 res.success = wait_push_all();
00763 
00764                 lock.lock();
00765                 go_idle();      // same as in pull_cb
00766                 return true;
00767         }
00768 
00769         bool set_cur_cb(mavros_msgs::WaypointSetCurrent::Request &req,
00770                         mavros_msgs::WaypointSetCurrent::Response &res) {
00771                 unique_lock lock(mutex);
00772 
00773                 if (wp_state != WP_IDLE)
00774                         return false;
00775 
00776                 wp_state = WP_SET_CUR;
00777                 wp_set_active = req.wp_seq;
00778                 restart_timeout_timer();
00779 
00780                 lock.unlock();
00781                 mission_set_current(wp_set_active);
00782                 res.success = wait_push_all();
00783 
00784                 lock.lock();
00785                 go_idle();      // same as in pull_cb
00786                 return true;
00787         }
00788 };
00789 };      // namespace mavplugin
00790 
00791 PLUGINLIB_EXPORT_CLASS(mavplugin::WaypointPlugin, mavplugin::MavRosPlugin)
00792 


mavros
Author(s): Vladimir Ermakov
autogenerated on Sat Jun 8 2019 19:55:19