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