From 832ad006508bcd55e95d057133de549181997aaf Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 28 May 2024 12:46:37 +0200 Subject: [PATCH 1/6] readd get_param --- crazyflie_py/crazyflie_py/crazyflie.py | 43 ++++++++++++++++---------- 1 file changed, 27 insertions(+), 16 deletions(-) diff --git a/crazyflie_py/crazyflie_py/crazyflie.py b/crazyflie_py/crazyflie_py/crazyflie.py index 006dcb1bb..aff4379a5 100644 --- a/crazyflie_py/crazyflie_py/crazyflie.py +++ b/crazyflie_py/crazyflie_py/crazyflie.py @@ -484,26 +484,37 @@ def notifySetpointsStop(self, remainValidMillisecs=100, groupMask=0): # '/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] + 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 + return param_value def setParam(self, name, value): """ From 92906489b2312b5aad0440e4910b161d4fe4aabf Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 28 May 2024 12:59:11 +0200 Subject: [PATCH 2/6] add blank line --- crazyflie_py/crazyflie_py/crazyflie.py | 1 + 1 file changed, 1 insertion(+) diff --git a/crazyflie_py/crazyflie_py/crazyflie.py b/crazyflie_py/crazyflie_py/crazyflie.py index aff4379a5..d0e733bd0 100644 --- a/crazyflie_py/crazyflie_py/crazyflie.py +++ b/crazyflie_py/crazyflie_py/crazyflie.py @@ -503,6 +503,7 @@ def getParam(self, name): Returns: value (Any): The parameter's value. + """ param_name = self.prefix[1:] + '.params.' + name req = GetParameters.Request() From dddf703ca798a3e09c832dd5d47d327df51f3d7e Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 28 May 2024 13:11:10 +0200 Subject: [PATCH 3/6] check param in setparam script --- crazyflie_examples/crazyflie_examples/set_param.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/crazyflie_examples/crazyflie_examples/set_param.py b/crazyflie_examples/crazyflie_examples/set_param.py index 69c49daf0..db5ca347b 100644 --- a/crazyflie_examples/crazyflie_examples/set_param.py +++ b/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) From 7720abb183f89b3a854be645a8853d965663bb3f Mon Sep 17 00:00:00 2001 From: knmcguire Date: Tue, 28 May 2024 16:10:50 +0200 Subject: [PATCH 4/6] fix issue with not finding parameter service --- crazyflie_py/crazyflie_py/crazyflie.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/crazyflie_py/crazyflie_py/crazyflie.py b/crazyflie_py/crazyflie_py/crazyflie.py index d0e733bd0..5402bd21b 100644 --- a/crazyflie_py/crazyflie_py/crazyflie.py +++ b/crazyflie_py/crazyflie_py/crazyflie.py @@ -141,11 +141,11 @@ 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(): From c235e25f13f473ed247c3ebef1d4f0d5f63ab87d Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 29 May 2024 09:45:10 +0200 Subject: [PATCH 5/6] add try except for getparam --- crazyflie_py/crazyflie_py/crazyflie.py | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/crazyflie_py/crazyflie_py/crazyflie.py b/crazyflie_py/crazyflie_py/crazyflie.py index 5402bd21b..7471028d2 100644 --- a/crazyflie_py/crazyflie_py/crazyflie.py +++ b/crazyflie_py/crazyflie_py/crazyflie.py @@ -511,10 +511,16 @@ def getParam(self, name): future = self.getParamsService.call_async(req) rclpy.spin_until_future_complete(self.node, future) param_type = self.paramTypeDict[name] - 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 + 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): From 1118919d03b7fd5c302e440665cbabe86e41b203 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 29 May 2024 09:46:35 +0200 Subject: [PATCH 6/6] fix too long line --- crazyflie_py/crazyflie_py/crazyflie.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/crazyflie_py/crazyflie_py/crazyflie.py b/crazyflie_py/crazyflie_py/crazyflie.py index 7471028d2..2e653dee3 100644 --- a/crazyflie_py/crazyflie_py/crazyflie.py +++ b/crazyflie_py/crazyflie_py/crazyflie.py @@ -141,7 +141,8 @@ def __init__(self, node, cfname, paramTypeDict): self.status = {} # Query some settings - self.getParamsService = node.create_client(GetParameters, '/crazyflie_server/get_parameters') + 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)]