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

@@ -141,11 +141,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():
@@ -484,26 +485,44 @@ 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]
+        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):
         """