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 #include <boost/any.hpp>
00022
00023 #include <mavros_msgs/ParamSet.h>
00024 #include <mavros_msgs/ParamGet.h>
00025 #include <mavros_msgs/ParamPull.h>
00026 #include <mavros_msgs/ParamPush.h>
00027
00028 namespace mavplugin {
00034 class Parameter {
00035 public:
00036 typedef boost::any param_t;
00037
00038 std::string param_id;
00039 param_t param_value;
00040 uint16_t param_index;
00041 uint16_t param_count;
00042
00046 static param_t from_param_value(mavlink_param_value_t &pmsg) {
00047 mavlink_param_union_t uv;
00048 uv.param_float = pmsg.param_value;
00049
00050
00051 #define RETURN_TYPE(type) \
00052 { \
00053 type ## _t ret_ ## type = uv.param_ ## type; \
00054 return ret_ ## type; \
00055 }
00056
00057 switch (pmsg.param_type) {
00058 case MAV_PARAM_TYPE_UINT8:
00059 RETURN_TYPE(uint8);
00060 case MAV_PARAM_TYPE_INT8:
00061 RETURN_TYPE(int8);
00062 case MAV_PARAM_TYPE_UINT16:
00063 RETURN_TYPE(uint16);
00064 case MAV_PARAM_TYPE_INT16:
00065 RETURN_TYPE(int16);
00066 case MAV_PARAM_TYPE_UINT32:
00067 RETURN_TYPE(uint32);
00068 case MAV_PARAM_TYPE_INT32:
00069 RETURN_TYPE(int32);
00070 case MAV_PARAM_TYPE_REAL32: {
00071 float ret_float = uv.param_float;
00072 return ret_float;
00073 }
00074
00075 #undef RETURN_TYPE
00076
00077 default:
00078 case MAV_PARAM_TYPE_UINT64:
00079 case MAV_PARAM_TYPE_INT64:
00080 case MAV_PARAM_TYPE_REAL64:
00081 ROS_WARN_NAMED("param", "Unsupported param '%.16s' type: %d, index: %d of %d",
00082 pmsg.param_id, pmsg.param_type,
00083 pmsg.param_index, pmsg.param_count);
00084 return param_t();
00085 };
00086 }
00087
00091 static param_t from_param_value_apm_quirk(mavlink_param_value_t &pmsg) {
00092 switch (pmsg.param_type) {
00093 case MAV_PARAM_TYPE_UINT8:
00094 return (uint8_t) pmsg.param_value;
00095 case MAV_PARAM_TYPE_INT8:
00096 return (int8_t) pmsg.param_value;
00097 case MAV_PARAM_TYPE_UINT16:
00098 return (uint16_t) pmsg.param_value;
00099 case MAV_PARAM_TYPE_INT16:
00100 return (int16_t) pmsg.param_value;
00101 case MAV_PARAM_TYPE_UINT32:
00102 return (uint32_t) pmsg.param_value;
00103 case MAV_PARAM_TYPE_INT32:
00104 return (int32_t) pmsg.param_value;
00105 case MAV_PARAM_TYPE_REAL32:
00106 return (float) pmsg.param_value;
00107
00108 default:
00109 case MAV_PARAM_TYPE_UINT64:
00110 case MAV_PARAM_TYPE_INT64:
00111 case MAV_PARAM_TYPE_REAL64:
00112 ROS_WARN_NAMED("param", "Unsupported param '%.16s' type: %d, index: %d of %d",
00113 pmsg.param_id, pmsg.param_type,
00114 pmsg.param_index, pmsg.param_count);
00115 return param_t();
00116 };
00117 }
00118
00122 static std::string to_string_vt(param_t p) {
00123 std::ostringstream sout;
00124
00125 if (p.type() == typeid(uint8_t))
00126 sout << (unsigned) boost::any_cast<uint8_t>(p) << " ubyte";
00127 else if (p.type() == typeid(int8_t))
00128 sout << (int) boost::any_cast<int8_t>(p) << " byte";
00129 else if (p.type() == typeid(uint16_t))
00130 sout << boost::any_cast<uint16_t>(p) << " ushort";
00131 else if (p.type() == typeid(int16_t))
00132 sout << boost::any_cast<int16_t>(p) << " short";
00133 else if (p.type() == typeid(uint32_t))
00134 sout << boost::any_cast<uint32_t>(p) << " uint";
00135 else if (p.type() == typeid(int32_t))
00136 sout << boost::any_cast<int32_t>(p) << " int";
00137 else if (p.type() == typeid(float))
00138 sout << boost::any_cast<float>(p) << " float";
00139 else {
00140 ROS_FATAL_STREAM_NAMED("param", "Wrong param_t type: " << p.type().name());
00141 sout << "UNK " << p.type().name();
00142 }
00143
00144 return sout.str();
00145 };
00146
00150 static mavlink_param_union_t to_param_union(param_t p) {
00151 mavlink_param_union_t ret;
00152
00153 if (p.type() == typeid(uint8_t)) {
00154 ret.param_uint8 = boost::any_cast<uint8_t>(p);
00155 ret.type = MAV_PARAM_TYPE_UINT8;
00156 }
00157 else if (p.type() == typeid(int8_t)) {
00158 ret.param_int8 = boost::any_cast<int8_t>(p);
00159 ret.type = MAV_PARAM_TYPE_INT8;
00160 }
00161 else if (p.type() == typeid(uint16_t)) {
00162 ret.param_uint16 = boost::any_cast<uint16_t>(p);
00163 ret.type = MAV_PARAM_TYPE_UINT16;
00164 }
00165 else if (p.type() == typeid(int16_t)) {
00166 ret.param_int16 = boost::any_cast<int16_t>(p);
00167 ret.type = MAV_PARAM_TYPE_INT16;
00168 }
00169 else if (p.type() == typeid(uint32_t)) {
00170 ret.param_uint32 = boost::any_cast<uint32_t>(p);
00171 ret.type = MAV_PARAM_TYPE_UINT32;
00172 }
00173 else if (p.type() == typeid(int32_t)) {
00174 ret.param_int32 = boost::any_cast<int32_t>(p);
00175 ret.type = MAV_PARAM_TYPE_INT32;
00176 }
00177 else if (p.type() == typeid(float)) {
00178 ret.param_float = boost::any_cast<float>(p);
00179 ret.type = MAV_PARAM_TYPE_REAL32;
00180 }
00181 else {
00182 ROS_FATAL_STREAM_NAMED("param", "Wrong param_t type: " << p.type().name());
00183 ret.param_float = 0.0;
00184 ret.type = 255;
00185 }
00186
00187 return ret;
00188 };
00189
00193 static mavlink_param_union_t to_param_union_apm_quirk(param_t p) {
00194 mavlink_param_union_t ret;
00195
00196 if (p.type() == typeid(uint8_t)) {
00197 ret.param_float = boost::any_cast<uint8_t>(p);
00198 ret.type = MAV_PARAM_TYPE_UINT8;
00199 }
00200 else if (p.type() == typeid(int8_t)) {
00201 ret.param_float = boost::any_cast<int8_t>(p);
00202 ret.type = MAV_PARAM_TYPE_INT8;
00203 }
00204 else if (p.type() == typeid(uint16_t)) {
00205 ret.param_float = boost::any_cast<uint16_t>(p);
00206 ret.type = MAV_PARAM_TYPE_UINT16;
00207 }
00208 else if (p.type() == typeid(int16_t)) {
00209 ret.param_float = boost::any_cast<int16_t>(p);
00210 ret.type = MAV_PARAM_TYPE_INT16;
00211 }
00212 else if (p.type() == typeid(uint32_t)) {
00213 ret.param_float = boost::any_cast<uint32_t>(p);
00214 ret.type = MAV_PARAM_TYPE_UINT32;
00215 }
00216 else if (p.type() == typeid(int32_t)) {
00217 ret.param_float = boost::any_cast<int32_t>(p);
00218 ret.type = MAV_PARAM_TYPE_INT32;
00219 }
00220 else if (p.type() == typeid(float)) {
00221 ret.param_float = boost::any_cast<float>(p);
00222 ret.type = MAV_PARAM_TYPE_REAL32;
00223 }
00224 else {
00225 ROS_FATAL_STREAM_NAMED("param", "Wrong param_t type: " << p.type().name());
00226 ret.param_float = 0.0;
00227 ret.type = 255;
00228 }
00229
00230 return ret;
00231 };
00232
00236 static int64_t to_integer(param_t &p) {
00237 if (p.type() == typeid(uint8_t))
00238 return boost::any_cast<uint8_t>(p);
00239 else if (p.type() == typeid(int8_t))
00240 return boost::any_cast<int8_t>(p);
00241 else if (p.type() == typeid(uint16_t))
00242 return boost::any_cast<uint16_t>(p);
00243 else if (p.type() == typeid(int16_t))
00244 return boost::any_cast<int16_t>(p);
00245 else if (p.type() == typeid(uint32_t))
00246 return boost::any_cast<uint32_t>(p);
00247 else if (p.type() == typeid(int32_t))
00248 return boost::any_cast<int32_t>(p);
00249 else
00250 return 0;
00251 };
00252
00253 static double to_real(param_t &p) {
00254 if (p.type() == typeid(float))
00255 return boost::any_cast<float>(p);
00256 else
00257 return 0.0;
00258 };
00259
00263 static XmlRpc::XmlRpcValue to_xmlrpc_value(param_t &p) {
00264 if (p.type() == typeid(uint8_t))
00265 return (int) boost::any_cast<uint8_t>(p);
00266 else if (p.type() == typeid(int8_t))
00267 return (int) boost::any_cast<int8_t>(p);
00268 else if (p.type() == typeid(uint16_t))
00269 return (int) boost::any_cast<uint16_t>(p);
00270 else if (p.type() == typeid(int16_t))
00271 return (int) boost::any_cast<int16_t>(p);
00272 else if (p.type() == typeid(uint32_t))
00273 return (int) boost::any_cast<uint32_t>(p);
00274 else if (p.type() == typeid(int32_t))
00275 return (int) boost::any_cast<int32_t>(p);
00276 else if (p.type() == typeid(float))
00277 return (double) boost::any_cast<float>(p);
00278 else {
00279 ROS_FATAL_STREAM_NAMED("param", "Wrong param_t type: " << p.type().name());
00280 return XmlRpc::XmlRpcValue();
00281 }
00282 };
00283
00287 static param_t from_xmlrpc_value(XmlRpc::XmlRpcValue &xml) {
00288 switch (xml.getType()) {
00289 case XmlRpc::XmlRpcValue::TypeBoolean:
00290 return (uint8_t) static_cast<bool>(xml);
00291 case XmlRpc::XmlRpcValue::TypeInt:
00292 return static_cast<int32_t>(xml);
00293 case XmlRpc::XmlRpcValue::TypeDouble:
00294 return (float) static_cast<double>(xml);
00295
00296 default:
00297 ROS_FATAL_NAMED("param", "Unsupported XmlRpcValye type: %d", xml.getType());
00298 return param_t();
00299 };
00300 };
00301
00305 static bool check_exclude_param_id(std::string param_id) {
00306 return param_id == "SYSID_SW_MREV" ||
00307 param_id == "SYS_NUM_RESETS" ||
00308 param_id == "ARSPD_OFFSET" ||
00309 param_id == "GND_ABS_PRESS" ||
00310 param_id == "GND_TEMP" ||
00311 param_id == "CMD_TOTAL" ||
00312 param_id == "CMD_INDEX" ||
00313 param_id == "LOG_LASTFILE" ||
00314 param_id == "FENCE_TOTAL" ||
00315 param_id == "FORMAT_VERSION";
00316 }
00317 };
00318
00319
00323 class ParamSetOpt {
00324 public:
00325 ParamSetOpt(Parameter &_p, size_t _rem) :
00326 param(_p),
00327 retries_remaining(_rem),
00328 is_timedout(false)
00329 { };
00330
00331 Parameter param;
00332 size_t retries_remaining;
00333 bool is_timedout;
00334 std::mutex cond_mutex;
00335 std::condition_variable ack;
00336 };
00337
00338
00342 class ParamPlugin : public MavRosPlugin {
00343 public:
00344 ParamPlugin() :
00345 param_nh("~param"),
00346 uas(nullptr),
00347 param_count(-1),
00348 param_state(PR_IDLE),
00349 is_timedout(false),
00350 param_rx_retries(RETRIES_COUNT),
00351 BOOTUP_TIME_DT(BOOTUP_TIME_MS / 1000.0),
00352 LIST_TIMEOUT_DT(LIST_TIMEOUT_MS / 1000.0),
00353 PARAM_TIMEOUT_DT(PARAM_TIMEOUT_MS / 1000.0)
00354 { };
00355
00356 void initialize(UAS &uas_)
00357 {
00358 uas = &uas_;
00359
00360 pull_srv = param_nh.advertiseService("pull", &ParamPlugin::pull_cb, this);
00361 push_srv = param_nh.advertiseService("push", &ParamPlugin::push_cb, this);
00362 set_srv = param_nh.advertiseService("set", &ParamPlugin::set_cb, this);
00363 get_srv = param_nh.advertiseService("get", &ParamPlugin::get_cb, this);
00364
00365 shedule_timer = param_nh.createTimer(BOOTUP_TIME_DT, &ParamPlugin::shedule_cb, this, true);
00366 shedule_timer.stop();
00367 timeout_timer = param_nh.createTimer(PARAM_TIMEOUT_DT, &ParamPlugin::timeout_cb, this, true);
00368 timeout_timer.stop();
00369 uas->sig_connection_changed.connect(boost::bind(&ParamPlugin::connection_cb, this, _1));
00370 }
00371
00372 const message_map get_rx_handlers() {
00373 return {
00374 MESSAGE_HANDLER(MAVLINK_MSG_ID_PARAM_VALUE, &ParamPlugin::handle_param_value)
00375 };
00376 }
00377
00378 private:
00379 std::recursive_mutex mutex;
00380 ros::NodeHandle param_nh;
00381 UAS *uas;
00382
00383 ros::ServiceServer pull_srv;
00384 ros::ServiceServer push_srv;
00385 ros::ServiceServer set_srv;
00386 ros::ServiceServer get_srv;
00387
00388 ros::Timer shedule_timer;
00389 ros::Timer timeout_timer;
00390
00391 static constexpr int BOOTUP_TIME_MS = 10000;
00392 static constexpr int PARAM_TIMEOUT_MS = 1000;
00393 static constexpr int LIST_TIMEOUT_MS = 30000;
00394 static constexpr int RETRIES_COUNT = 3;
00395
00396 const ros::Duration BOOTUP_TIME_DT;
00397 const ros::Duration LIST_TIMEOUT_DT;
00398 const ros::Duration PARAM_TIMEOUT_DT;
00399
00400 std::map<std::string, Parameter> parameters;
00401 std::list<uint16_t> parameters_missing_idx;
00402 std::map<std::string, ParamSetOpt*> set_parameters;
00403 ssize_t param_count;
00404 enum {
00405 PR_IDLE,
00406 PR_RXLIST,
00407 PR_RXPARAM,
00408 PR_TXPARAM
00409 } param_state;
00410
00411 size_t param_rx_retries;
00412 bool is_timedout;
00413 std::mutex list_cond_mutex;
00414 std::condition_variable list_receiving;
00415
00416 inline Parameter::param_t from_param_value(mavlink_param_value_t &msg) {
00417 if (uas->is_ardupilotmega())
00418 return Parameter::from_param_value_apm_quirk(msg);
00419 else
00420 return Parameter::from_param_value(msg);
00421 }
00422
00423 inline mavlink_param_union_t to_param_union(Parameter::param_t p) {
00424 if (uas->is_ardupilotmega())
00425 return Parameter::to_param_union_apm_quirk(p);
00426 else
00427 return Parameter::to_param_union(p);
00428 }
00429
00430
00431
00432 void handle_param_value(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00433 mavlink_param_value_t pmsg;
00434 mavlink_msg_param_value_decode(msg, &pmsg);
00435
00436 std::string param_id(pmsg.param_id,
00437 strnlen(pmsg.param_id, sizeof(pmsg.param_id)));
00438
00439 lock_guard lock(mutex);
00440
00441 auto param_it = parameters.find(param_id);
00442 if (param_it != parameters.end()) {
00443
00444 Parameter *p = ¶m_it->second;
00445 p->param_value = from_param_value(pmsg);
00446
00447
00448 auto set_it = set_parameters.find(param_id);
00449 if (set_it != set_parameters.end()) {
00450 set_it->second->ack.notify_all();
00451 }
00452
00453 ROS_WARN_STREAM_COND_NAMED(((p->param_index != pmsg.param_index &&
00454 pmsg.param_index != UINT16_MAX) ||
00455 p->param_count != pmsg.param_count),
00456 "param",
00457 "PR: Param " << param_id << " index(" << p->param_index <<
00458 "->" << pmsg.param_index << ")/count(" << p->param_count <<
00459 "->" << pmsg.param_count << ") changed! FCU changed?");
00460 ROS_DEBUG_STREAM_NAMED("param", "PR: Update param " << param_id <<
00461 " (" << p->param_index << "/" << p->param_count <<
00462 ") value: " << Parameter::to_string_vt(p->param_value));
00463 }
00464 else {
00465
00466 Parameter p;
00467 p.param_id = param_id;
00468 p.param_index = pmsg.param_index;
00469 p.param_count = pmsg.param_count;
00470 p.param_value = from_param_value(pmsg);
00471
00472 parameters[param_id] = p;
00473
00474 ROS_DEBUG_STREAM_NAMED("param", "PR: New param " << param_id <<
00475 " (" << p.param_index << "/" << p.param_count <<
00476 ") value: " << Parameter::to_string_vt(p.param_value));
00477 }
00478
00479 if (param_state == PR_RXLIST || param_state == PR_RXPARAM) {
00480
00481 if (param_state == PR_RXLIST) {
00482 param_count = pmsg.param_count;
00483 param_state = PR_RXPARAM;
00484
00485 parameters_missing_idx.clear();
00486 if (param_count != UINT16_MAX) {
00487 ROS_DEBUG_NAMED("param", "PR: waiting %zu parameters", param_count);
00488
00489 for (uint16_t idx = 0; idx < param_count; idx++)
00490 parameters_missing_idx.push_back(idx);
00491 }
00492 else
00493 ROS_WARN_NAMED("param", "PR: FCU does not know index for first element! "
00494 "Param list may be truncated.");
00495 }
00496
00497
00498
00499 bool it_is_first_requested = parameters_missing_idx.front() == pmsg.param_index;
00500
00501
00502 parameters_missing_idx.remove(pmsg.param_index);
00503
00504
00505 if (it_is_first_requested)
00506 param_rx_retries = RETRIES_COUNT;
00507
00508 restart_timeout_timer();
00509
00510
00511 if (parameters_missing_idx.empty()) {
00512 ssize_t missed = param_count - parameters.size();
00513 ROS_INFO_COND_NAMED(missed == 0, "param", "PR: parameters list received");
00514 ROS_WARN_COND_NAMED(missed > 0, "param",
00515 "PR: parameters list received, but %zd parametars are missed",
00516 missed);
00517 go_idle();
00518 list_receiving.notify_all();
00519 }
00520 }
00521 }
00522
00523
00524
00525 void param_request_list() {
00526 mavlink_message_t msg;
00527
00528 ROS_DEBUG_NAMED("param", "PR:m: request list");
00529 mavlink_msg_param_request_list_pack_chan(UAS_PACK_CHAN(uas), &msg,
00530 UAS_PACK_TGT(uas)
00531 );
00532 UAS_FCU(uas)->send_message(&msg);
00533 }
00534
00535 void param_request_read(std::string id, int16_t index = -1) {
00536 ROS_ASSERT(index >= -1);
00537
00538 mavlink_message_t msg;
00539 char param_id[sizeof(mavlink_param_request_read_t::param_id)];
00540
00541 ROS_DEBUG_NAMED("param", "PR:m: request '%s', idx %d", id.c_str(), index);
00542 if (index != -1) {
00543
00544
00545 strncpy(param_id, id.c_str(), sizeof(param_id));
00546 }
00547 else
00548 param_id[0] = '\0';
00549
00550 mavlink_msg_param_request_read_pack_chan(UAS_PACK_CHAN(uas), &msg,
00551 UAS_PACK_TGT(uas),
00552 param_id,
00553 index
00554 );
00555 UAS_FCU(uas)->send_message(&msg);
00556 }
00557
00558 void param_set(Parameter ¶m) {
00559 mavlink_param_union_t pu = to_param_union(param.param_value);
00560
00561 mavlink_message_t msg;
00562 char param_id[sizeof(mavlink_param_set_t::param_id)];
00563 strncpy(param_id, param.param_id.c_str(), sizeof(param_id));
00564
00565 ROS_DEBUG_STREAM_NAMED("param", "PR:m: set param " << param.param_id <<
00566 " (" << param.param_index << "/" << param.param_count <<
00567 ") value: " << Parameter::to_string_vt(param.param_value));
00568 mavlink_msg_param_set_pack_chan(UAS_PACK_CHAN(uas), &msg,
00569 UAS_PACK_TGT(uas),
00570 param_id,
00571 pu.param_float,
00572 pu.type
00573 );
00574 UAS_FCU(uas)->send_message(&msg);
00575 }
00576
00577
00578
00579 void connection_cb(bool connected) {
00580 lock_guard lock(mutex);
00581 if (connected) {
00582 shedule_pull(BOOTUP_TIME_DT);
00583 }
00584 else {
00585 shedule_timer.stop();
00586 }
00587 }
00588
00589 void shedule_pull(const ros::Duration &dt) {
00590 shedule_timer.stop();
00591 shedule_timer.setPeriod(dt);
00592 shedule_timer.start();
00593 }
00594
00595 void shedule_cb(const ros::TimerEvent &event) {
00596 lock_guard lock(mutex);
00597 if (param_state != PR_IDLE) {
00598
00599 ROS_DEBUG_NAMED("param", "PR: busy, reshedule pull");
00600 shedule_pull(BOOTUP_TIME_DT);
00601 }
00602
00603 ROS_DEBUG_NAMED("param", "PR: start sheduled pull");
00604 param_state = PR_RXLIST;
00605 param_rx_retries = RETRIES_COUNT;
00606 parameters.clear();
00607
00608 restart_timeout_timer();
00609 param_request_list();
00610 }
00611
00612 void timeout_cb(const ros::TimerEvent &event) {
00613 lock_guard lock(mutex);
00614 if (param_state == PR_RXLIST && param_rx_retries > 0) {
00615 param_rx_retries--;
00616 ROS_WARN_NAMED("param", "PR: request list timeout, retries left %zu", param_rx_retries);
00617
00618 restart_timeout_timer();
00619 param_request_list();
00620 }
00621 else if (param_state == PR_RXPARAM) {
00622 if (parameters_missing_idx.empty()) {
00623 ROS_WARN_NAMED("param", "PR: missing list is clear, but we in RXPARAM state, "
00624 "maybe last rerequest fails. Params missed: %zd",
00625 param_count - parameters.size());
00626 go_idle();
00627 list_receiving.notify_all();
00628 return;
00629 }
00630
00631 uint16_t first_miss_idx = parameters_missing_idx.front();
00632 if (param_rx_retries > 0) {
00633 param_rx_retries--;
00634 ROS_WARN_NAMED("param", "PR: request param #%u timeout, retries left %zu, and %zu params still missing",
00635 first_miss_idx, param_rx_retries, parameters_missing_idx.size());
00636 restart_timeout_timer();
00637 param_request_read("", first_miss_idx);
00638 }
00639 else {
00640 ROS_ERROR_NAMED("param", "PR: request param #%u completely missing.", first_miss_idx);
00641 parameters_missing_idx.pop_front();
00642 restart_timeout_timer();
00643 if (!parameters_missing_idx.empty()) {
00644 param_rx_retries = RETRIES_COUNT;
00645 first_miss_idx = parameters_missing_idx.front();
00646
00647 ROS_WARN_NAMED("param", "PR: %zu params still missing, trying to request next: #%u",
00648 parameters_missing_idx.size(), first_miss_idx);
00649 param_request_read("", first_miss_idx);
00650 }
00651 }
00652 }
00653 else if (param_state == PR_TXPARAM) {
00654 auto it = set_parameters.begin();
00655 if (it == set_parameters.end()) {
00656 ROS_DEBUG_NAMED("param", "PR: send list empty, but state TXPARAM");
00657 go_idle();
00658 return;
00659 }
00660
00661 if (it->second->retries_remaining > 0) {
00662 it->second->retries_remaining--;
00663 ROS_WARN_NAMED("param", "PR: Resend param set for %s, retries left %zu",
00664 it->second->param.param_id.c_str(),
00665 it->second->retries_remaining);
00666 restart_timeout_timer();
00667 param_set(it->second->param);
00668 }
00669 else {
00670 ROS_ERROR_NAMED("param", "PR: Param set for %s timed out.",
00671 it->second->param.param_id.c_str());
00672 it->second->is_timedout = true;
00673 it->second->ack.notify_all();
00674 }
00675 }
00676 else {
00677 ROS_DEBUG_NAMED("param", "PR: timeout in IDLE!");
00678 }
00679 }
00680
00681 void restart_timeout_timer() {
00682 is_timedout = false;
00683 timeout_timer.stop();
00684 timeout_timer.start();
00685 }
00686
00687 void go_idle() {
00688 param_state = PR_IDLE;
00689 timeout_timer.stop();
00690 }
00691
00692 bool wait_fetch_all() {
00693 std::unique_lock<std::mutex> lock(list_cond_mutex);
00694
00695 return list_receiving.wait_for(lock, std::chrono::nanoseconds(LIST_TIMEOUT_DT.toNSec()))
00696 == std::cv_status::no_timeout
00697 && !is_timedout;
00698 }
00699
00700 bool wait_param_set_ack_for(ParamSetOpt *opt) {
00701 std::unique_lock<std::mutex> lock(opt->cond_mutex);
00702
00703 return opt->ack.wait_for(lock, std::chrono::nanoseconds(PARAM_TIMEOUT_DT.toNSec()) * (RETRIES_COUNT + 2))
00704 == std::cv_status::no_timeout
00705 && !opt->is_timedout;
00706 }
00707
00708 bool send_param_set_and_wait(Parameter ¶m) {
00709 unique_lock lock(mutex);
00710
00711
00712 set_parameters[param.param_id] = new ParamSetOpt(param, RETRIES_COUNT);
00713
00714 auto it = set_parameters.find(param.param_id);
00715 if (it == set_parameters.end()) {
00716 ROS_ERROR_STREAM_NAMED("param", "ParamSetOpt not found for " << param.param_id);
00717 return false;
00718 }
00719
00720 param_state = PR_TXPARAM;
00721 restart_timeout_timer();
00722 param_set(param);
00723
00724 lock.unlock();
00725 bool is_not_timeout = wait_param_set_ack_for(it->second);
00726 lock.lock();
00727
00728
00729 delete it->second;
00730 set_parameters.erase(it);
00731
00732 go_idle();
00733 return is_not_timeout;
00734 }
00735
00736
00737
00742 bool pull_cb(mavros_msgs::ParamPull::Request &req,
00743 mavros_msgs::ParamPull::Response &res) {
00744 unique_lock lock(mutex);
00745
00746 if ((param_state == PR_IDLE && parameters.empty())
00747 || req.force_pull) {
00748 if (!req.force_pull)
00749 ROS_DEBUG_NAMED("param", "PR: start pull");
00750 else
00751 ROS_INFO_NAMED("param", "PR: start force pull");
00752
00753 param_state = PR_RXLIST;
00754 param_rx_retries = RETRIES_COUNT;
00755 parameters.clear();
00756
00757 shedule_timer.stop();
00758 restart_timeout_timer();
00759 param_request_list();
00760
00761 lock.unlock();
00762 res.success = wait_fetch_all();
00763 }
00764 else if (param_state == PR_RXLIST || param_state == PR_RXPARAM) {
00765 lock.unlock();
00766 res.success = wait_fetch_all();
00767 }
00768 else {
00769 lock.unlock();
00770 res.success = true;
00771 }
00772
00773 lock.lock();
00774 res.param_received = parameters.size();
00775
00776 for (auto param_it = parameters.begin();
00777 param_it != parameters.end();
00778 param_it++) {
00779 Parameter *p = ¶m_it->second;
00780 auto pv = Parameter::to_xmlrpc_value(p->param_value);
00781
00782 lock.unlock();
00783 param_nh.setParam(p->param_id, pv);
00784 lock.lock();
00785 }
00786
00787 return true;
00788 }
00789
00794 bool push_cb(mavros_msgs::ParamPush::Request &req,
00795 mavros_msgs::ParamPush::Response &res) {
00796 XmlRpc::XmlRpcValue param_dict;
00797 if (!param_nh.getParam("", param_dict))
00798 return true;
00799
00800 ROS_ASSERT(param_dict.getType() == XmlRpc::XmlRpcValue::TypeStruct);
00801
00802 int tx_count = 0;
00803 for (auto param = param_dict.begin();
00804 param != param_dict.end();
00805 param++) {
00806 if (Parameter::check_exclude_param_id(param->first)) {
00807 ROS_DEBUG_STREAM_NAMED("param", "PR: Exclude param: " << param->first);
00808 continue;
00809 }
00810
00811 unique_lock lock(mutex);
00812 auto param_it = parameters.find(param->first);
00813 if (param_it != parameters.end()) {
00814 Parameter *p = ¶m_it->second;
00815 Parameter to_send = *p;
00816
00817 to_send.param_value = Parameter::from_xmlrpc_value(param->second);
00818
00819 lock.unlock();
00820 bool set_res = send_param_set_and_wait(to_send);
00821 lock.lock();
00822
00823 if (set_res)
00824 tx_count++;
00825 }
00826 else {
00827 ROS_WARN_STREAM_NAMED("param", "PR: Unknown rosparam: " << param->first);
00828 }
00829 }
00830
00831 res.success = true;
00832 res.param_transfered = tx_count;
00833
00834 return true;
00835 }
00836
00841 bool set_cb(mavros_msgs::ParamSet::Request &req,
00842 mavros_msgs::ParamSet::Response &res) {
00843 unique_lock lock(mutex);
00844
00845 if (param_state == PR_RXLIST || param_state == PR_RXPARAM) {
00846 ROS_ERROR_NAMED("param", "PR: receiving not complete");
00847 return false;
00848 }
00849
00850 auto param_it = parameters.find(req.param_id);
00851 if (param_it != parameters.end()) {
00852 Parameter *p = ¶m_it->second;
00853 Parameter to_send = *p;
00854
00855
00856 if (req.value.integer > 0)
00857 to_send.param_value = (uint32_t) req.value.integer;
00858 else if (req.value.integer < 0)
00859 to_send.param_value = (int32_t) req.value.integer;
00860 else if (req.value.real != 0.0)
00861 to_send.param_value = (float) req.value.real;
00862 else
00863 to_send.param_value = (uint32_t) 0;
00864
00865 lock.unlock();
00866 res.success = send_param_set_and_wait(to_send);
00867 lock.lock();
00868
00869 res.value.integer = Parameter::to_integer(p->param_value);
00870 res.value.real = Parameter::to_real(p->param_value);
00871
00872 auto pv = Parameter::to_xmlrpc_value(p->param_value);
00873 lock.unlock();
00874
00875 param_nh.setParam(p->param_id, pv);
00876 }
00877 else {
00878 ROS_ERROR_STREAM_NAMED("param", "PR: Unknown parameter to set: " << req.param_id);
00879 res.success = false;
00880 }
00881
00882 return true;
00883 }
00884
00889 bool get_cb(mavros_msgs::ParamGet::Request &req,
00890 mavros_msgs::ParamGet::Response &res) {
00891 lock_guard lock(mutex);
00892
00893 auto param_it = parameters.find(req.param_id);
00894 if (param_it != parameters.end()) {
00895 Parameter *p = ¶m_it->second;
00896
00897 res.success = true;
00898 res.value.integer = Parameter::to_integer(p->param_value);
00899 res.value.real = Parameter::to_real(p->param_value);
00900 }
00901 else {
00902 ROS_ERROR_STREAM_NAMED("param", "PR: Unknown parameter to get: " << req.param_id);
00903 res.success = false;
00904 }
00905
00906 return true;
00907 }
00908 };
00909 };
00910
00911 PLUGINLIB_EXPORT_CLASS(mavplugin::ParamPlugin, mavplugin::MavRosPlugin)
00912