00001
00009
00010
00011
00012
00013
00014
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;
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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();
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
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();
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();
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();
00786 return true;
00787 }
00788 };
00789 };
00790
00791 PLUGINLIB_EXPORT_CLASS(mavplugin::WaypointPlugin, mavplugin::MavRosPlugin)
00792