param.py
Go to the documentation of this file.
1 # -*- coding: utf-8 -*-
2 # vim:set ts=4 sw=4 et:
3 #
4 # Copyright 2014 Vladimir Ermakov.
5 #
6 # This file is part of the mavros package and subject to the license terms
7 # in the top-level LICENSE file of the mavros repository.
8 # https://github.com/mavlink/mavros/tree/master/LICENSE.md
9 
10 import csv
11 import time
12 import rospy
13 import mavros
14 
15 from mavros_msgs.msg import ParamValue
16 from mavros_msgs.srv import ParamPull, ParamPush, ParamGet, ParamSet
17 
18 
19 class Parameter(object):
20  """Class representing one parameter"""
21  def __init__(self, param_id, param_value=0):
22  self.param_id = param_id
23  self.param_value = param_value
24 
25  def __repr__(self):
26  return "<Parameter '{}': {}>".format(self.param_id, self.param_value)
27 
28 
29 class ParamFile(object):
30  """Base class for param file parsers"""
31  def __init__(self, args):
32  pass
33 
34  def read(self, file_):
35  """Returns a iterable of Parameters"""
36  raise NotImplementedError
37 
38  def write(self, file_, parametes):
39  """Writes Parameters to file"""
40  raise NotImplementedError
41 
42 
44  """Parse MavProxy param files"""
45 
46  class CSVDialect(csv.Dialect):
47  delimiter = ' '
48  doublequote = False
49  skipinitialspace = True
50  lineterminator = '\r\n'
51  quoting = csv.QUOTE_NONE
52 
53  def read(self, file_):
54  to_numeric = lambda x: float(x) if '.' in x else int(x)
55 
56  for data in csv.reader(file_, self.CSVDialect):
57  if data[0].startswith('#'):
58  continue # skip comments
59 
60  if len(data) != 2:
61  raise ValueError("wrong field count")
62 
63  yield Parameter(data[0].strip(), to_numeric(data[1]));
64 
65  def write(self, file_, parameters):
66  writer = csv.writer(file_, self.CSVDialect)
67  file_.write("#NOTE: " + time.strftime("%d.%m.%Y %T") + self.CSVDialect.lineterminator)
68  for p in parameters:
69  writer.writerow((p.param_id, p.param_value))
70 
71 
73  """Parse MissionPlanner param files"""
74 
75  class CSVDialect(csv.Dialect):
76  delimiter = ','
77  doublequote = False
78  skipinitialspace = True
79  lineterminator = '\r\n'
80  quoting = csv.QUOTE_NONE
81 
82  def read(self, file_):
83  to_numeric = lambda x: float(x) if '.' in x else int(x)
84 
85  for data in csv.reader(file_, self.CSVDialect):
86  if data[0].startswith('#'):
87  continue # skip comments
88 
89  if len(data) != 2:
90  raise ValueError("wrong field count")
91 
92  yield Parameter(data[0].strip(), to_numeric(data[1]));
93 
94  def write(self, file_, parameters):
95  writer = csv.writer(file_, self.CSVDialect)
96  writer.writerow(("#NOTE: " + time.strftime("%d.%m.%Y %T") ,))
97  for p in parameters:
98  writer.writerow((p.param_id, p.param_value))
99 
100 
102  """Parse QGC param files"""
103 
104  class CSVDialect(csv.Dialect):
105  delimiter = '\t'
106  doublequote = False
107  skipinitialspace = True
108  lineterminator = '\n'
109  quoting = csv.QUOTE_NONE
110 
111  def read(self, file_):
112  to_numeric = lambda x: float(x) if '.' in x else int(x)
113 
114  for data in csv.reader(file_, self.CSVDialect):
115  if data[0].startswith('#'):
116  continue # skip comments
117 
118  if len(data) != 5:
119  raise ValueError("wrong field count")
120 
121  yield Parameter(data[2].strip(), to_numeric(data[3]));
122 
123  def write(self, file_, parameters):
124  def to_type(x):
125  if isinstance(x, float):
126  return 9 # REAL32
127  elif isinstance(x, int):
128  return 6 # INT32
129  else:
130  raise ValueError("unknown type: " + repr(type(x)))
131 
132  sysid = rospy.get_param(mavros.get_topic('target_system_id'), 1)
133  compid = rospy.get_param(mavros.get_topic('target_component_id'), 1)
134 
135  writer = csv.writer(file_, self.CSVDialect)
136  writer.writerow(("# NOTE: " + time.strftime("%d.%m.%Y %T"), ))
137  writer.writerow(("# Onboard parameters saved by mavparam for ({}, {})".format(sysid, compid), ))
138  writer.writerow(("# MAV ID" , "COMPONENT ID", "PARAM NAME", "VALUE", "(TYPE)"))
139  for p in parameters:
140  writer.writerow((sysid, compid, p.param_id, p.param_value, to_type(p.param_value), )) # XXX
141 
142 
144  if ret.value.integer != 0:
145  return ret.value.integer
146  elif ret.value.real != 0.0:
147  return ret.value.real
148  else:
149  return 0
150 
151 
152 def param_get(param_id):
153  try:
154  get = rospy.ServiceProxy(mavros.get_topic('param', 'get'), ParamGet)
155  ret = get(param_id=param_id)
156  except rospy.ServiceException as ex:
157  raise IOError(str(ex))
158 
159  if not ret.success:
160  raise IOError("Request failed.")
161 
162  return param_ret_value(ret)
163 
164 
165 def param_set(param_id, value):
166  if isinstance(value, float):
167  val = ParamValue(integer=0, real=value)
168  else:
169  val = ParamValue(integer=value, real=0.0)
170 
171  try:
172  set = rospy.ServiceProxy(mavros.get_topic('param', 'set'), ParamSet)
173  ret = set(param_id=param_id, value=val)
174  except rospy.ServiceException as ex:
175  raise IOError(str(ex))
176 
177  if not ret.success:
178  raise IOError("Request failed.")
179 
180  return param_ret_value(ret)
181 
182 
183 def param_get_all(force_pull=False):
184  try:
185  pull = rospy.ServiceProxy(mavros.get_topic('param', 'pull'), ParamPull)
186  ret = pull(force_pull=force_pull)
187  except rospy.ServiceException as ex:
188  raise IOError(str(ex))
189 
190  if not ret.success:
191  raise IOError("Request failed.")
192 
193  params = rospy.get_param(mavros.get_topic('param'))
194 
195  return (ret.param_received,
196  sorted((Parameter(k, v) for k, v in params.items()),
197  key=lambda p: p.param_id)
198  )
199 
200 
201 def param_set_list(param_list):
202  # 1. load parameters to parameter server
203  for p in param_list:
204  rospy.set_param(mavros.get_topic('param', p.param_id), p.param_value)
205 
206  # 2. request push all
207  try:
208  push = rospy.ServiceProxy(mavros.get_topic('param', 'push'), ParamPush)
209  ret = push()
210  except rospy.ServiceException as ex:
211  raise IOError(str(ex))
212 
213  if not ret.success:
214  raise IOError("Request failed.")
215 
216  return ret.param_transfered
def read(self, file_)
Definition: param.py:82
def __init__(self, args)
Definition: param.py:31
def write(self, file_, parameters)
Definition: param.py:65
def read(self, file_)
Definition: param.py:111
std::string format(const std::string &fmt, Args...args)
def param_set(param_id, value)
Definition: param.py:165
def get_topic(args)
Definition: __init__.py:49
def write(self, file_, parametes)
Definition: param.py:38
def __repr__(self)
Definition: param.py:25
def write(self, file_, parameters)
Definition: param.py:123
def read(self, file_)
Definition: param.py:34
ssize_t len
def param_get_all(force_pull=False)
Definition: param.py:183
def write(self, file_, parameters)
Definition: param.py:94
def param_ret_value(ret)
Definition: param.py:143
def param_get(param_id)
Definition: param.py:152
def read(self, file_)
Definition: param.py:53
def __init__(self, param_id, param_value=0)
Definition: param.py:21
def param_set_list(param_list)
Definition: param.py:201


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