Merge pull request #509 from IMRCLab/get_param_try2

Re-enable getparam for a single crazyflie try2

crazyflie_examples/crazyflie_examples/set_param.py

@@ -12,6 +12,8 @@ def main():
  for cf in allcfs.crazyflies:
  cf.setParam('led.bitmask', 128)
  timeHelper.sleep(1.0)
+ if cf.getParam('led.bitmask') != 128:
+ print('LED of cf', cf.id, 'is not disabled!')
 
  timeHelper.sleep(2.0)
 

crazyflie_py/crazyflie_py/crazyflie.py

@@ -144,11 +144,12 @@ def __init__(self, node, cfname, paramTypeDict):
  self.status = {}
 
  # Query some settings
- getParamsService = node.create_client(GetParameters, '/crazyflie_server/get_parameters')
- getParamsService.wait_for_service()
+ self.getParamsService = node.create_client(
+ GetParameters, '/crazyflie_server/get_parameters')
+ self.getParamsService.wait_for_service()
  req = GetParameters.Request()
  req.names = ['robots.{}.initial_position'.format(cfname), 'robots.{}.uri'.format(cfname)]
- future = getParamsService.call_async(req)
+ future = self.getParamsService.call_async(req)
  while rclpy.ok():
  rclpy.spin_once(node)
  if future.done():
@@ -502,26 +503,44 @@ def arm(self, arm=True):
  # '/world', '/cf' + str(self.id), rospy.Time(0))
  # return np.array(position)
 
- # def getParam(self, name):
- # """Returns the current value of the onboard named parameter.
+ def getParam(self, name):
+ """
+ Get the current value of the onboard named parameter.
 
- #  Parameters are named values of various primitive C types that control
- #  the firmware's behavior. For more information, see
- #  https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/userguides/logparam/.
+ Parameters are named values of various primitive C types that control
+ the firmware's behavior. For more information, see
+ https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/userguides/logparam/.
 
- #  Parameters are read at system startup over the radio and cached.
- #  The ROS launch file can also be used to set parameter values at startup.
- #  Subsequent calls to :meth:`setParam()` will update the cached value.
- #  However, if the parameter changes for any other reason, the cached value
- #  might become stale. This situation is not common.
+ Parameters are read at system startup over the radio and cached.
+ The ROS launch file can also be used to set parameter values at startup.
+ Subsequent calls to :meth:`setParam()` will update the cached value.
+ However, if the parameter changes for any other reason, the cached value
+ might become stale. This situation is not common.
 
- #  Args:
- #  name (str): The parameter's name.
+ Args:
+ name (str): The parameter's name.
 
- # Returns:
- # value (Any): The parameter's value.
- # """
- # return rospy.get_param(self.prefix + '/' + name)
+ Returns:
+ value (Any): The parameter's value.
+
+ """
+ param_name = self.prefix[1:] + '.params.' + name
+ req = GetParameters.Request()
+ req.names = [param_name]
+ future = self.getParamsService.call_async(req)
+ rclpy.spin_until_future_complete(self.node, future)
+ param_type = self.paramTypeDict[name]
+ try:
+ if param_type == ParameterType.PARAMETER_INTEGER:
+ param_value = future.result().values[0].integer_value
+ elif param_type == ParameterType.PARAMETER_DOUBLE:
+ param_value = future.result().values[0].double_value
+ except KeyError as e:
+ self.get_logger().warn(f'(crazyflie.py)getParam : keyError raised {e}')
+ except Exception as e:
+ self.get_logger().warn(f'(crazyflie.py)getParam : exception raised {e}')
+
+ return param_value
 
  def setParam(self, name, value):
  """