param.cpp
Go to the documentation of this file.
1 
9 /*
10  * Copyright 2014,2015,2016 Vladimir Ermakov.
11  *
12  * This file is part of the mavros package and subject to the license terms
13  * in the top-level LICENSE file of the mavros repository.
14  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
15  */
16 
17 #include <chrono>
18 #include <condition_variable>
19 #include <mavros/mavros_plugin.h>
20 
21 #include <mavros_msgs/ParamSet.h>
22 #include <mavros_msgs/ParamGet.h>
23 #include <mavros_msgs/ParamPull.h>
24 #include <mavros_msgs/ParamPush.h>
25 #include <mavros_msgs/Param.h>
26 
27 namespace mavros {
28 namespace std_plugins {
29 using utils::enum_value;
30 
49 class Parameter {
50 public:
51  using MT = mavlink::common::MAV_PARAM_TYPE;
52  using PARAM_SET = mavlink::common::msg::PARAM_SET;
54 
55  std::string param_id;
57  uint16_t param_index;
58  uint16_t param_count;
59 
60  void set_value(mavlink::common::msg::PARAM_VALUE &pmsg)
61  {
62  mavlink::mavlink_param_union_t uv;
63  uv.param_float = pmsg.param_value;
64 
65  // #170 - copy union value to itermediate var
66  int int_tmp;
67  float float_tmp;
68 
69  switch (pmsg.param_type) {
70  // [[[cog:
71  // param_types = [ (s, 'float' if s == 'real32' else s) for s in (
72  // 'int8', 'uint8',
73  // 'int16', 'uint16',
74  // 'int32', 'uint32',
75  // 'real32',
76  // )]
77  // unsupported_types = ('int64', 'uint64', 'real64')
78  //
79  // for a, b in param_types:
80  // btype = 'int' if 'int' in b else b
81  // cog.outl("case enum_value(MT::%s):" % a.upper())
82  // cog.outl("\t%s_tmp = uv.param_%s;" % (btype, b))
83  // cog.outl("\tparam_value = %s_tmp;" % btype)
84  // cog.outl("\tbreak;")
85  // ]]]
86  case enum_value(MT::INT8):
87  int_tmp = uv.param_int8;
88  param_value = int_tmp;
89  break;
90  case enum_value(MT::UINT8):
91  int_tmp = uv.param_uint8;
92  param_value = int_tmp;
93  break;
94  case enum_value(MT::INT16):
95  int_tmp = uv.param_int16;
96  param_value = int_tmp;
97  break;
98  case enum_value(MT::UINT16):
99  int_tmp = uv.param_uint16;
100  param_value = int_tmp;
101  break;
102  case enum_value(MT::INT32):
103  int_tmp = uv.param_int32;
104  param_value = int_tmp;
105  break;
106  case enum_value(MT::UINT32):
107  int_tmp = uv.param_uint32;
108  param_value = int_tmp;
109  break;
110  case enum_value(MT::REAL32):
111  float_tmp = uv.param_float;
112  param_value = float_tmp;
113  break;
114  // [[[end]]] (checksum: 5950e4ee032d4aa198b953f56909e129)
115 
116  default:
117  ROS_WARN_NAMED("param", "PM: Unsupported param %.16s (%u/%u) type: %u",
118  pmsg.param_id.data(), pmsg.param_index, pmsg.param_count, pmsg.param_type);
119  param_value = 0;
120  };
121  }
122 
126  void set_value_apm_quirk(mavlink::common::msg::PARAM_VALUE &pmsg)
127  {
128  int32_t int_tmp;
129  float float_tmp;
130 
131  switch (pmsg.param_type) {
132  // [[[cog:
133  // for a, b in param_types:
134  // btype = 'int' if 'int' in b else b
135  // cog.outl("case enum_value(MT::%s):" % a.upper())
136  // cog.outl("\t%s_tmp = pmsg.param_value;" % btype)
137  // cog.outl("\tparam_value = %s_tmp;" % btype)
138  // cog.outl("\tbreak;")
139  // ]]]
140  case enum_value(MT::INT8):
141  int_tmp = pmsg.param_value;
142  param_value = int_tmp;
143  break;
144  case enum_value(MT::UINT8):
145  int_tmp = pmsg.param_value;
146  param_value = int_tmp;
147  break;
148  case enum_value(MT::INT16):
149  int_tmp = pmsg.param_value;
150  param_value = int_tmp;
151  break;
152  case enum_value(MT::UINT16):
153  int_tmp = pmsg.param_value;
154  param_value = int_tmp;
155  break;
156  case enum_value(MT::INT32):
157  int_tmp = pmsg.param_value;
158  param_value = int_tmp;
159  break;
160  case enum_value(MT::UINT32):
161  int_tmp = pmsg.param_value;
162  param_value = int_tmp;
163  break;
164  case enum_value(MT::REAL32):
165  float_tmp = pmsg.param_value;
166  param_value = float_tmp;
167  break;
168  // [[[end]]] (checksum: c30ee34dd84213471690612ab49f1f73)
169 
170  default:
171  ROS_WARN_NAMED("param", "PM: Unsupported param %.16s (%u/%u) type: %u",
172  pmsg.param_id.data(), pmsg.param_index, pmsg.param_count, pmsg.param_type);
173  param_value = 0;
174  }
175  }
176 
179  {
180  // Note: XmlRpcValue does not have const cast operators.
181  // This method can't be const.
182 
183  mavlink::mavlink_param_union_t uv;
184  PARAM_SET ret{};
185 
186  mavlink::set_string(ret.param_id, param_id);
187 
188  switch (param_value.getType()) {
189  // [[[cog:
190  // xmlrpc_types = (
191  // ('TypeBoolean', 'uint8', 'bool'),
192  // ('TypeInt', 'int32', 'int32_t'),
193  // ('TypeDouble', 'real32', 'double'),
194  // )
195  //
196  // for a, b, c in xmlrpc_types:
197  // uvb = 'float' if 'real32' == b else b
198  // cog.outl("case XmlRpcValue::%s:" % a)
199  // cog.outl("\tuv.param_%s = static_cast<%s>(param_value);" % (uvb, c))
200  // cog.outl("\tret.param_type = enum_value(MT::%s);" % b.upper())
201  // cog.outl("\tbreak;")
202  // ]]]
204  uv.param_uint8 = static_cast<bool>(param_value);
205  ret.param_type = enum_value(MT::UINT8);
206  break;
208  uv.param_int32 = static_cast<int32_t>(param_value);
209  ret.param_type = enum_value(MT::INT32);
210  break;
212  uv.param_float = static_cast<double>(param_value);
213  ret.param_type = enum_value(MT::REAL32);
214  break;
215  // [[[end]]] (checksum: c414a3950fba234cbbe694a2576ae022)
216 
217  default:
218  ROS_WARN_NAMED("param", "PR: Unsupported XmlRpcValue type: %u", param_value.getType());
219  }
220 
221  ret.param_value = uv.param_float;
222  return ret;
223  }
224 
227  {
228  PARAM_SET ret{};
229 
230  mavlink::set_string(ret.param_id, param_id);
231 
232  switch (param_value.getType()) {
233  // [[[cog:
234  // for a, b, c in xmlrpc_types:
235  // cog.outl("case XmlRpcValue::%s:" % a)
236  // cog.outl("\tret.param_value = static_cast<%s &>(param_value);" % c)
237  // cog.outl("\tret.param_type = enum_value(MT::%s);" % b.upper())
238  // cog.outl("\tbreak;")
239  // ]]]
241  ret.param_value = static_cast<bool &>(param_value);
242  ret.param_type = enum_value(MT::UINT8);
243  break;
245  ret.param_value = static_cast<int32_t &>(param_value);
246  ret.param_type = enum_value(MT::INT32);
247  break;
249  ret.param_value = static_cast<double &>(param_value);
250  ret.param_type = enum_value(MT::REAL32);
251  break;
252  // [[[end]]] (checksum: 5b10c0e1f2e916f1c31313eaa5cc83e0)
253 
254  default:
255  ROS_WARN_NAMED("param", "PR: Unsupported XmlRpcValue type: %u", param_value.getType());
256  }
257 
258  return ret;
259  }
260 
264  int64_t to_integer()
265  {
266  switch (param_value.getType()) {
267  // [[[cog:
268  // for a, b, c in xmlrpc_types:
269  // if 'int' not in b:
270  // continue
271  // cog.outl("case XmlRpcValue::%s:\treturn static_cast<%s>(param_value);" % (a, c))
272  // ]]]
273  case XmlRpcValue::TypeBoolean: return static_cast<bool>(param_value);
274  case XmlRpcValue::TypeInt: return static_cast<int32_t>(param_value);
275  // [[[end]]] (checksum: ce23a3bc04354d8cfcb82341beb83709)
276 
277  default:
278  return 0;
279  }
280  }
281 
282  double to_real()
283  {
284  if (param_value.getType() == XmlRpcValue::TypeDouble)
285  return static_cast<double>(param_value);
286  else
287  return 0.0;
288  }
289 
290  // for debugging
291  std::string to_string() const
292  {
293  return utils::format("%s (%u/%u): %s", param_id.c_str(), param_index, param_count, param_value.toXml().c_str());
294  }
295 
296  mavros_msgs::Param to_msg()
297  {
298  mavros_msgs::Param msg;
299 
300  // XXX(vooon): find better solution
301  msg.header.stamp = ros::Time::now();
302 
303  msg.param_id = param_id;
304  msg.value.integer = to_integer();
305  msg.value.real = to_real();
306  msg.param_index = param_index;
307  msg.param_count = param_count;
308 
309  return msg;
310  }
311 
315  static bool check_exclude_param_id(std::string param_id)
316  {
317  return param_id == "SYSID_SW_MREV" ||
318  param_id == "SYS_NUM_RESETS" ||
319  param_id == "ARSPD_OFFSET" ||
320  param_id == "GND_ABS_PRESS" ||
321  param_id == "GND_ABS_PRESS2" ||
322  param_id == "GND_ABS_PRESS3" ||
323  param_id == "STAT_BOOTCNT" ||
324  param_id == "STAT_FLTTIME" ||
325  param_id == "STAT_RESET" ||
326  param_id == "STAT_RUNTIME" ||
327  param_id == "GND_TEMP" ||
328  param_id == "CMD_TOTAL" ||
329  param_id == "CMD_INDEX" ||
330  param_id == "LOG_LASTFILE" ||
331  param_id == "FENCE_TOTAL" ||
332  param_id == "FORMAT_VERSION";
333  }
334 };
335 
336 
340 class ParamSetOpt {
341 public:
342  ParamSetOpt(Parameter &_p, size_t _rem) :
343  param(_p),
344  retries_remaining(_rem),
345  is_timedout(false)
346  { }
347 
352  std::condition_variable ack;
353 };
354 
355 
360 public:
361  ParamPlugin() : PluginBase(),
362  param_nh("~param"),
363  param_count(-1),
364  param_state(PR::IDLE),
365  is_timedout(false),
366  RETRIES_COUNT(_RETRIES_COUNT),
367  param_rx_retries(RETRIES_COUNT),
368  BOOTUP_TIME_DT(BOOTUP_TIME_MS / 1000.0),
369  LIST_TIMEOUT_DT(LIST_TIMEOUT_MS / 1000.0),
370  PARAM_TIMEOUT_DT(PARAM_TIMEOUT_MS / 1000.0)
371  { }
372 
373  void initialize(UAS &uas_) override
374  {
375  PluginBase::initialize(uas_);
376 
377  pull_srv = param_nh.advertiseService("pull", &ParamPlugin::pull_cb, this);
378  push_srv = param_nh.advertiseService("push", &ParamPlugin::push_cb, this);
379  set_srv = param_nh.advertiseService("set", &ParamPlugin::set_cb, this);
380  get_srv = param_nh.advertiseService("get", &ParamPlugin::get_cb, this);
381 
382  param_value_pub = param_nh.advertise<mavros_msgs::Param>("param_value", 100);
383 
384  shedule_timer = param_nh.createTimer(BOOTUP_TIME_DT, &ParamPlugin::shedule_cb, this, true);
385  shedule_timer.stop();
386  timeout_timer = param_nh.createTimer(PARAM_TIMEOUT_DT, &ParamPlugin::timeout_cb, this, true);
387  timeout_timer.stop();
388  enable_connection_cb();
389  }
390 
392  {
393  return {
394  make_handler(&ParamPlugin::handle_param_value),
395  };
396  }
397 
398 private:
399  using lock_guard = std::lock_guard<std::recursive_mutex>;
400  using unique_lock = std::unique_lock<std::recursive_mutex>;
401 
402  std::recursive_mutex mutex;
404 
409 
411 
414 
415  static constexpr int BOOTUP_TIME_MS = 10000;
416  static constexpr int PARAM_TIMEOUT_MS = 1000;
417  static constexpr int LIST_TIMEOUT_MS = 30000;
418  static constexpr int _RETRIES_COUNT = 3;
419 
423  const int RETRIES_COUNT;
424 
425  std::unordered_map<std::string, Parameter> parameters;
426  std::list<uint16_t> parameters_missing_idx;
427  std::unordered_map<std::string, std::shared_ptr<ParamSetOpt>> set_parameters;
428  ssize_t param_count;
429  enum class PR {
430  IDLE,
431  RXLIST,
432  RXPARAM,
433  RXPARAM_TIMEDOUT,
434  TXPARAM
435  };
437 
441  std::condition_variable list_receiving;
442 
443  /* -*- message handlers -*- */
444 
445  void handle_param_value(const mavlink::mavlink_message_t *msg, mavlink::common::msg::PARAM_VALUE &pmsg)
446  {
447  lock_guard lock(mutex);
448 
449  auto param_id = mavlink::to_string(pmsg.param_id);
450 
451  // search
452  auto param_it = parameters.find(param_id);
453  if (param_it != parameters.end()) {
454  // parameter exists
455  auto &p = param_it->second;
456 
457  if (m_uas->is_ardupilotmega())
458  p.set_value_apm_quirk(pmsg);
459  else
460  p.set_value(pmsg);
461 
462  // check that ack required
463  auto set_it = set_parameters.find(param_id);
464  if (set_it != set_parameters.end()) {
465  set_it->second->ack.notify_all();
466  }
467 
468  param_value_pub.publish(p.to_msg());
469 
471  ((p.param_index != pmsg.param_index &&
472  pmsg.param_index != UINT16_MAX) ||
473  p.param_count != pmsg.param_count),
474  "param",
475  "PR: Param " << p.to_string() << " different index: " << pmsg.param_index << "/" << pmsg.param_count);
476  ROS_DEBUG_STREAM_NAMED("param", "PR: Update param " << p.to_string());
477  }
478  else {
479  // insert new element
480  Parameter p{};
481  p.param_id = param_id;
482  p.param_index = pmsg.param_index;
483  p.param_count = pmsg.param_count;
484 
485  if (m_uas->is_ardupilotmega())
486  p.set_value_apm_quirk(pmsg);
487  else
488  p.set_value(pmsg);
489 
490  parameters[param_id] = p;
491 
492  param_value_pub.publish(p.to_msg());
493 
494  ROS_DEBUG_STREAM_NAMED("param", "PR: New param " << p.to_string());
495  }
496 
497  if (param_state == PR::RXLIST || param_state == PR::RXPARAM || param_state == PR::RXPARAM_TIMEDOUT) {
498 
499  // we received first param. setup list timeout
500  if (param_state == PR::RXLIST) {
501  param_count = pmsg.param_count;
502  param_state = PR::RXPARAM;
503 
504  parameters_missing_idx.clear();
505  if (param_count != UINT16_MAX) {
506  ROS_DEBUG_NAMED("param", "PR: waiting %zu parameters", param_count);
507  // declare that all parameters are missing
508  for (uint16_t idx = 0; idx < param_count; idx++)
509  parameters_missing_idx.push_back(idx);
510  }
511  else
512  ROS_WARN_NAMED("param", "PR: FCU does not know index for first element! "
513  "Param list may be truncated.");
514  }
515 
516  // trying to avoid endless rerequest loop
517  // Issue #276
518  bool it_is_first_requested = parameters_missing_idx.front() == pmsg.param_index;
519 
520  // remove idx for that message
521  parameters_missing_idx.remove(pmsg.param_index);
522 
523  // in receiving mode we use param_rx_retries for LIST and PARAM
524  if (it_is_first_requested) {
525  ROS_DEBUG_NAMED("param", "PR: got a value of a requested param idx=%u, "
526  "resetting retries count", pmsg.param_index);
527  param_rx_retries = RETRIES_COUNT;
528  } else if (param_state == PR::RXPARAM_TIMEDOUT) {
529  ROS_INFO_NAMED("param", "PR: got an unsolicited param value idx=%u, "
530  "not resetting retries count %zu", pmsg.param_index, param_rx_retries);
531  }
532 
533  restart_timeout_timer();
534 
535  /* index starting from 0, receivig done */
536  if (parameters_missing_idx.empty()) {
537  ssize_t missed = param_count - parameters.size();
538  ROS_INFO_COND_NAMED(missed == 0, "param", "PR: parameters list received");
539  ROS_WARN_COND_NAMED(missed > 0, "param",
540  "PR: parameters list received, but %zd parametars are missed",
541  missed);
542  go_idle();
543  list_receiving.notify_all();
544  } else if (param_state == PR::RXPARAM_TIMEDOUT) {
545  uint16_t first_miss_idx = parameters_missing_idx.front();
546  ROS_DEBUG_NAMED("param", "PR: requesting next timed out parameter idx=%u", first_miss_idx);
547  param_request_read("", first_miss_idx);
548  }
549  }
550  }
551 
552  /* -*- low-level send function -*- */
553 
555  {
556  ROS_DEBUG_NAMED("param", "PR:m: request list");
557 
558  mavlink::common::msg::PARAM_REQUEST_LIST rql{};
559  m_uas->msg_set_target(rql);
560 
561  UAS_FCU(m_uas)->send_message_ignore_drop(rql);
562  }
563 
564  void param_request_read(std::string id, int16_t index = -1)
565  {
566  ROS_ASSERT(index >= -1);
567 
568  ROS_DEBUG_NAMED("param", "PR:m: request '%s', idx %d", id.c_str(), index);
569 
570  mavlink::common::msg::PARAM_REQUEST_READ rqr{};
571  m_uas->msg_set_target(rqr);
572  rqr.param_index = index;
573 
574  if (index != -1) {
575  mavlink::set_string(rqr.param_id, id);
576  }
577 
578  UAS_FCU(m_uas)->send_message_ignore_drop(rqr);
579  }
580 
581  void param_set(Parameter &param)
582  {
583  ROS_DEBUG_STREAM_NAMED("param", "PR:m: set param " << param.to_string());
584 
585  // GCC 4.8 can't type out lambda return
586  auto ps = ([this, &param]() -> mavlink::common::msg::PARAM_SET {
587  if (m_uas->is_ardupilotmega())
588  return param.to_param_set_apm_qurk();
589  else
590  return param.to_param_set();
591  })();
592 
593  m_uas->msg_set_target(ps);
594 
595  UAS_FCU(m_uas)->send_message_ignore_drop(ps);
596  }
597 
598  /* -*- mid-level functions -*- */
599 
600  void connection_cb(bool connected) override
601  {
602  lock_guard lock(mutex);
603  if (connected) {
604  shedule_pull(BOOTUP_TIME_DT);
605  }
606  else {
607  shedule_timer.stop();
608  }
609  }
610 
611  void shedule_pull(const ros::Duration &dt)
612  {
613  shedule_timer.stop();
614  shedule_timer.setPeriod(dt);
615  shedule_timer.start();
616  }
617 
618  void shedule_cb(const ros::TimerEvent &event)
619  {
620  lock_guard lock(mutex);
621  if (param_state != PR::IDLE) {
622  // try later
623  ROS_DEBUG_NAMED("param", "PR: busy, reshedule pull");
624  shedule_pull(BOOTUP_TIME_DT);
625  }
626 
627  ROS_DEBUG_NAMED("param", "PR: start sheduled pull");
628  param_state = PR::RXLIST;
629  param_rx_retries = RETRIES_COUNT;
630  parameters.clear();
631 
632  restart_timeout_timer();
633  param_request_list();
634  }
635 
636  void timeout_cb(const ros::TimerEvent &event)
637  {
638  lock_guard lock(mutex);
639  if (param_state == PR::RXLIST && param_rx_retries > 0) {
640  param_rx_retries--;
641  ROS_WARN_NAMED("param", "PR: request list timeout, retries left %zu", param_rx_retries);
642 
643  restart_timeout_timer();
644  param_request_list();
645  }
646  else if (param_state == PR::RXPARAM || param_state == PR::RXPARAM_TIMEDOUT) {
647  if (parameters_missing_idx.empty()) {
648  ROS_WARN_NAMED("param", "PR: missing list is clear, but we in RXPARAM state, "
649  "maybe last rerequest fails. Params missed: %zd",
650  param_count - parameters.size());
651  go_idle();
652  list_receiving.notify_all();
653  return;
654  }
655 
656  param_state = PR::RXPARAM_TIMEDOUT;
657  uint16_t first_miss_idx = parameters_missing_idx.front();
658  if (param_rx_retries > 0) {
659  param_rx_retries--;
660  ROS_WARN_NAMED("param", "PR: request param #%u timeout, retries left %zu, and %zu params still missing",
661  first_miss_idx, param_rx_retries, parameters_missing_idx.size());
662  restart_timeout_timer();
663  param_request_read("", first_miss_idx);
664  }
665  else {
666  ROS_ERROR_NAMED("param", "PR: request param #%u completely missing.", first_miss_idx);
667  parameters_missing_idx.pop_front();
668  restart_timeout_timer();
669  if (!parameters_missing_idx.empty()) {
670  param_rx_retries = RETRIES_COUNT;
671  first_miss_idx = parameters_missing_idx.front();
672 
673  ROS_WARN_NAMED("param", "PR: %zu params still missing, trying to request next: #%u",
674  parameters_missing_idx.size(), first_miss_idx);
675  param_request_read("", first_miss_idx);
676  }
677  }
678  }
679  else if (param_state == PR::TXPARAM) {
680  auto it = set_parameters.begin();
681  if (it == set_parameters.end()) {
682  ROS_DEBUG_NAMED("param", "PR: send list empty, but state TXPARAM");
683  go_idle();
684  return;
685  }
686 
687  if (it->second->retries_remaining > 0) {
688  it->second->retries_remaining--;
689  ROS_WARN_NAMED("param", "PR: Resend param set for %s, retries left %zu",
690  it->second->param.param_id.c_str(),
691  it->second->retries_remaining);
692  restart_timeout_timer();
693  param_set(it->second->param);
694  }
695  else {
696  ROS_ERROR_NAMED("param", "PR: Param set for %s timed out.",
697  it->second->param.param_id.c_str());
698  it->second->is_timedout = true;
699  it->second->ack.notify_all();
700  }
701  }
702  else {
703  ROS_DEBUG_NAMED("param", "PR: timeout in IDLE!");
704  }
705  }
706 
708  {
709  is_timedout = false;
710  timeout_timer.stop();
711  timeout_timer.start();
712  }
713 
714  void go_idle()
715  {
716  param_state = PR::IDLE;
717  timeout_timer.stop();
718  }
719 
721  {
722  std::unique_lock<std::mutex> lock(list_cond_mutex);
723 
724  return list_receiving.wait_for(lock, std::chrono::nanoseconds(LIST_TIMEOUT_DT.toNSec()))
725  == std::cv_status::no_timeout
726  && !is_timedout;
727  }
728 
729  bool wait_param_set_ack_for(std::shared_ptr<ParamSetOpt> opt)
730  {
731  std::unique_lock<std::mutex> lock(opt->cond_mutex);
732 
733  return opt->ack.wait_for(lock, std::chrono::nanoseconds(PARAM_TIMEOUT_DT.toNSec()) * (RETRIES_COUNT + 2))
734  == std::cv_status::no_timeout
735  && !opt->is_timedout;
736  }
737 
739  {
740  unique_lock lock(mutex);
741 
742  // add to waiting list
743  auto opt = std::make_shared<ParamSetOpt>(param, RETRIES_COUNT);
744  set_parameters[param.param_id] = opt;
745 
746  param_state = PR::TXPARAM;
747  restart_timeout_timer();
748  param_set(param);
749 
750  lock.unlock();
751  bool is_not_timeout = wait_param_set_ack_for(opt);
752  lock.lock();
753 
754  // free opt data
755  set_parameters.erase(param.param_id);
756 
757  go_idle();
758  return is_not_timeout;
759  }
760 
763  {
764  if (m_uas->is_px4() && p.param_id == "_HASH_CHECK") {
765  auto v = p.param_value; // const XmlRpcValue can't cast
766  ROS_INFO_NAMED("param", "PR: PX4 parameter _HASH_CHECK ignored: 0x%8x", static_cast<int32_t>(v));
767  return false;
768  }
769 
770  param_nh.setParam(p.param_id, p.param_value);
771  return true;
772  }
773 
774  /* -*- ROS callbacks -*- */
775 
780  bool pull_cb(mavros_msgs::ParamPull::Request &req,
781  mavros_msgs::ParamPull::Response &res)
782  {
783  unique_lock lock(mutex);
784 
785  if ((param_state == PR::IDLE && parameters.empty())
786  || req.force_pull) {
787  if (!req.force_pull)
788  ROS_DEBUG_NAMED("param", "PR: start pull");
789  else
790  ROS_INFO_NAMED("param", "PR: start force pull");
791 
792  param_state = PR::RXLIST;
793  param_rx_retries = RETRIES_COUNT;
794  parameters.clear();
795 
796  shedule_timer.stop();
797  restart_timeout_timer();
798  param_request_list();
799 
800  lock.unlock();
801  res.success = wait_fetch_all();
802  }
803  else if (param_state == PR::RXLIST || param_state == PR::RXPARAM || param_state == PR::RXPARAM_TIMEDOUT) {
804  lock.unlock();
805  res.success = wait_fetch_all();
806  }
807  else {
808  lock.unlock();
809  res.success = true;
810  }
811 
812  lock.lock();
813  res.param_received = parameters.size();
814 
815  for (auto &p : parameters) {
816  lock.unlock();
817  rosparam_set_allowed(p.second);
818  lock.lock();
819  }
820 
821  return true;
822  }
823 
828  bool push_cb(mavros_msgs::ParamPush::Request &req,
829  mavros_msgs::ParamPush::Response &res)
830  {
831  XmlRpc::XmlRpcValue param_dict;
832  if (!param_nh.getParam("", param_dict))
833  return true;
834 
836 
837  int tx_count = 0;
838  for (auto &param : param_dict) {
840  ROS_DEBUG_STREAM_NAMED("param", "PR: Exclude param: " << param.first);
841  continue;
842  }
843 
844  unique_lock lock(mutex);
845  auto param_it = parameters.find(param.first);
846  if (param_it != parameters.end()) {
847  // copy current state of Parameter
848  auto to_send = param_it->second;
849 
850  // Update XmlRpcValue
851  to_send.param_value = param.second;
852 
853  lock.unlock();
854  bool set_res = send_param_set_and_wait(to_send);
855  lock.lock();
856 
857  if (set_res)
858  tx_count++;
859  }
860  else {
861  ROS_WARN_STREAM_NAMED("param", "PR: Unknown rosparam: " << param.first);
862  }
863  }
864 
865  res.success = true;
866  res.param_transfered = tx_count;
867 
868  return true;
869  }
870 
875  bool set_cb(mavros_msgs::ParamSet::Request &req,
876  mavros_msgs::ParamSet::Response &res)
877  {
878  unique_lock lock(mutex);
879 
880  if (param_state == PR::RXLIST || param_state == PR::RXPARAM || param_state == PR::RXPARAM_TIMEDOUT) {
881  ROS_ERROR_NAMED("param", "PR: receiving not complete");
882  return false;
883  }
884 
885  auto param_it = parameters.find(req.param_id);
886  if (param_it != parameters.end()) {
887  auto to_send = param_it->second;
888 
889  // according to ParamValue description
890  if (req.value.integer != 0)
891  to_send.param_value = static_cast<int>(req.value.integer);
892  else if (req.value.real != 0.0)
893  to_send.param_value = req.value.real;
894  else if (param_it->second.param_value.getType() == XmlRpc::XmlRpcValue::Type::TypeDouble)
895  to_send.param_value = req.value.real;
896  else
897  to_send.param_value = 0;
898 
899  lock.unlock();
900  res.success = send_param_set_and_wait(to_send);
901  lock.lock();
902 
903  res.value.integer = param_it->second.to_integer();
904  res.value.real = param_it->second.to_real();
905 
906  lock.unlock();
907  rosparam_set_allowed(param_it->second);
908  }
909  else {
910  ROS_ERROR_STREAM_NAMED("param", "PR: Unknown parameter to set: " << req.param_id);
911  res.success = false;
912  }
913 
914  return true;
915  }
916 
921  bool get_cb(mavros_msgs::ParamGet::Request &req,
922  mavros_msgs::ParamGet::Response &res)
923  {
924  lock_guard lock(mutex);
925 
926  auto param_it = parameters.find(req.param_id);
927  if (param_it != parameters.end()) {
928  res.success = true;
929 
930  res.value.integer = param_it->second.to_integer();
931  res.value.real = param_it->second.to_real();
932  }
933  else {
934  ROS_ERROR_STREAM_NAMED("param", "PR: Unknown parameter to get: " << req.param_id);
935  res.success = false;
936  }
937 
938  return true;
939  }
940 };
941 } // namespace std_plugins
942 } // namespace mavros
943 
msg
MAVROS Plugin base class.
Definition: mavros_plugin.h:36
#define ROS_WARN_COND_NAMED(cond, name,...)
std::unique_lock< std::recursive_mutex > unique_lock
Definition: param.cpp:400
bool param(const std::string &param_name, T &param_val, const T &default_val)
#define ROS_WARN_STREAM_COND_NAMED(cond, name, args)
#define ROS_INFO_NAMED(name,...)
#define ROS_INFO_COND_NAMED(cond, name,...)
#define ROS_DEBUG_STREAM_NAMED(name, args)
std::string format(const std::string &fmt, Args...args)
void publish(const boost::shared_ptr< M > &message) const
#define ROS_ERROR_STREAM_NAMED(name, args)
void timeout_cb(const ros::TimerEvent &event)
Definition: param.cpp:636
#define ROS_WARN_NAMED(name,...)
std::condition_variable ack
Definition: param.cpp:352
def param_set(param_id, value)
Definition: param.py:165
MAVROS Plugin base.
std::string to_string() const
Definition: param.cpp:291
std::condition_variable list_receiving
Definition: param.cpp:441
void set_value_apm_quirk(mavlink::common::msg::PARAM_VALUE &pmsg)
Definition: param.cpp:126
void stop()
ros::Publisher param_value_pub
Definition: param.cpp:410
void setPeriod(const Duration &period, bool reset=true)
void start()
const ros::Duration PARAM_TIMEOUT_DT
Definition: param.cpp:422
mavlink::common::msg::PARAM_SET PARAM_SET
Definition: param.cpp:52
bool wait_param_set_ack_for(std::shared_ptr< ParamSetOpt > opt)
Definition: param.cpp:729
PARAM_SET to_param_set()
Make PARAM_SET message. Set target ids manually!
Definition: param.cpp:178
void shedule_cb(const ros::TimerEvent &event)
Definition: param.cpp:618
static bool check_exclude_param_id(std::string param_id)
Definition: param.cpp:315
void set_value(mavlink::common::msg::PARAM_VALUE &pmsg)
Definition: param.cpp:60
mavros_msgs::Param to_msg()
Definition: param.cpp:296
Type const & getType() const
bool set_cb(mavros_msgs::ParamSet::Request &req, mavros_msgs::ParamSet::Response &res)
sets parameter value ~param/set
Definition: param.cpp:875
#define ROS_DEBUG_NAMED(name,...)
PARAM_SET to_param_set_apm_qurk()
Make PARAM_SET message. Set target ids manually!
Definition: param.cpp:226
Parameter manipulation plugin.
Definition: param.cpp:359
std::string toXml() const
void param_request_read(std::string id, int16_t index=-1)
Definition: param.cpp:564
UAS for plugins.
Definition: mavros_uas.h:67
Subscriptions get_subscriptions() override
Return vector of MAVLink message subscriptions (handlers)
Definition: param.cpp:391
mavlink::common::MAV_PARAM_TYPE MT
Definition: param.cpp:51
ros::ServiceServer push_srv
Definition: param.cpp:406
int64_t toNSec() const
bool send_param_set_and_wait(Parameter &param)
Definition: param.cpp:738
#define UAS_FCU(uasobjptr)
helper accessor to FCU link interface
Definition: mavros_uas.h:42
ros::Timer shedule_timer
for startup shedule fetch
Definition: param.cpp:412
ParamSetOpt(Parameter &_p, size_t _rem)
Definition: param.cpp:342
void param_set(Parameter &param)
Definition: param.cpp:581
const ros::Duration BOOTUP_TIME_DT
Definition: param.cpp:420
bool get_cb(mavros_msgs::ParamGet::Request &req, mavros_msgs::ParamGet::Response &res)
get parameter ~param/get
Definition: param.cpp:921
bool pull_cb(mavros_msgs::ParamPull::Request &req, mavros_msgs::ParamPull::Response &res)
fetches all parameters from device ~param/pull
Definition: param.cpp:780
void shedule_pull(const ros::Duration &dt)
Definition: param.cpp:611
ros::Timer timeout_timer
for timeout resend
Definition: param.cpp:413
const ros::Duration LIST_TIMEOUT_DT
Definition: param.cpp:421
ros::ServiceServer pull_srv
Definition: param.cpp:405
std::vector< HandlerInfo > Subscriptions
Subscriptions vector.
Definition: mavros_plugin.h:47
Parameter set transaction data.
Definition: param.cpp:340
bool getParam(const std::string &key, std::string &s) const
std::unordered_map< std::string, std::shared_ptr< ParamSetOpt > > set_parameters
Definition: param.cpp:427
Parameter storage.
Definition: param.cpp:49
bool push_cb(mavros_msgs::ParamPush::Request &req, mavros_msgs::ParamPush::Response &res)
push all parameter value to device ~param/push
Definition: param.cpp:828
std::unordered_map< std::string, Parameter > parameters
Definition: param.cpp:425
#define ROS_ERROR_NAMED(name,...)
static Time now()
void handle_param_value(const mavlink::mavlink_message_t *msg, mavlink::common::msg::PARAM_VALUE &pmsg)
Definition: param.cpp:445
ros::ServiceServer set_srv
Definition: param.cpp:407
#define ROS_ASSERT(cond)
std::lock_guard< std::recursive_mutex > lock_guard
Definition: param.cpp:399
ros::ServiceServer get_srv
Definition: param.cpp:408
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void connection_cb(bool connected) override
Definition: param.cpp:600
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
void initialize(UAS &uas_) override
Plugin initializer.
Definition: param.cpp:373
std::list< uint16_t > parameters_missing_idx
Definition: param.cpp:426
std::recursive_mutex mutex
Definition: param.cpp:402
constexpr std::underlying_type< _T >::type enum_value(_T e)
Definition: utils.h:55
std::recursive_mutex mutex
#define ROS_WARN_STREAM_NAMED(name, args)
bool rosparam_set_allowed(const Parameter &p)
Set ROS param only if name is good.
Definition: param.cpp:762


mavros
Author(s): Vladimir Ermakov
autogenerated on Tue Jun 1 2021 02:36:26