param.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 #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                 // Fix for #170 by copying to temporary var.
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         /* -*- message handlers -*- */
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                 // search
00441                 auto param_it = parameters.find(param_id);
00442                 if (param_it != parameters.end()) {
00443                         // parameter exists
00444                         Parameter *p = &param_it->second;
00445                         p->param_value = from_param_value(pmsg);
00446 
00447                         // check that ack required
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                         // insert new element
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                         // we received first param. setup list timeout
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                                         // declare that all parameters are missing
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                         // trying to avoid endless rerequest loop
00498                         // Issue #276
00499                         bool it_is_first_requested = parameters_missing_idx.front() == pmsg.param_index;
00500 
00501                         // remove idx for that message
00502                         parameters_missing_idx.remove(pmsg.param_index);
00503 
00504                         // in receiving mode we use param_rx_retries for LIST and PARAM
00505                         if (it_is_first_requested)
00506                                 param_rx_retries = RETRIES_COUNT;
00507 
00508                         restart_timeout_timer();
00509 
00510                         /* index starting from 0, receivig done */
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         /* -*- low-level send function -*- */
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                         // by specs if len < 16: place null termination
00544                         // else if len == 16: don't
00545                         strncpy(param_id, id.c_str(), sizeof(param_id));
00546                 }
00547                 else
00548                         param_id[0] = '\0';     // force NULL termination
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 &param) {
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         /* -*- mid-level functions -*- */
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                         // try later
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 &param) {
00709                 unique_lock lock(mutex);
00710 
00711                 // add to waiting list
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                 // free opt data
00729                 delete it->second;
00730                 set_parameters.erase(it);
00731 
00732                 go_idle();
00733                 return is_not_timeout;
00734         }
00735 
00736         /* -*- ROS callbacks -*- */
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 = &param_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 = &param_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 = &param_it->second;
00853                         Parameter to_send = *p;
00854 
00855                         // according to ParamValue description
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 = &param_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 };      // namespace mavplugin
00910 
00911 PLUGINLIB_EXPORT_CLASS(mavplugin::ParamPlugin, mavplugin::MavRosPlugin)
00912 


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