Added scale factor to parameters

rosflight_rqt_plugins/resources/param_tuning_example_config.yaml

@@ -4,46 +4,62 @@
 # {Name of parameter group}:
 #   node: '/{ROS_node_name}'
 #   params:
-#     {param_name}: '{Parameter Description}'
+#     {param_name}:
+#       description: '{Parameter Description}'
+#       scale: {Scale Factor to use in GUI} (optional, default is 1.0)
 #     ...
 
 Roll Angle:
   node: '/autopilot'
   params:
-    r_kp: 'Roll Angle P Gain'
-    r_kd: 'Roll Angle D Gain'
+    r_kp:
+      description: 'Roll Angle P Gain'
+    r_kd:
+      description: 'Roll Angle D Gain'
 
 Pitch Angle:
   node: '/autopilot'
   params:
-    p_kp: 'Pitch Angle P Gain'
-    p_kd: 'Pitch Angle D Gain'
+    p_kp:
+      description: 'Pitch Angle P Gain'
+    p_kd:
+      description:  'Pitch Angle D Gain'
 
 Airspeed:
   node: '/autopilot'
   params:
-    a_t_kp: 'Airspeed P Gain'
-    a_t_ki: 'Airspeed I Gain'
+    a_t_kp:
+      description:  'Airspeed P Gain'
+    a_t_ki:
+      description:  'Airspeed I Gain'
 
 Course:
   node: '/autopilot'
   params:
-    c_kp: 'Course P Gain'
-    c_ki: 'Course I Gain'
+    c_kp:
+      description:  'Course P Gain'
+    c_ki:
+      description: 'Course I Gain'
 
 Altitude:
   node: '/autopilot'
   params:
-    a_kp: 'Altitude P Gain'
-    a_ki: 'Altitude I Gain'
+    a_kp:
+      description:  'Altitude P Gain'
+    a_ki:
+      description: 'Altitude I Gain'
 
 Line Following:
   node: '/path_follower'
   params:
-    k_path: 'Line Following P Gain'
-    chi_infty: 'Line Following I Gain'
+    k_path:
+      description:  'Line Following K Gain'
+    chi_infty:
+      description:  'Max Approach Angle (degrees)'
+      scale: 57.2957795131
 
 Orbit Following:
   node: '/path_follower'
   params:
-    k_orbit: 'Orbit Following P Gain'
+    k_orbit:
+      description:  'Orbit Following K Gain'

rosflight_rqt_plugins/src/param_tuning/param_tuning_client.py

@@ -7,6 +7,7 @@
 class ParameterClient(Node):
     def __init__(self, config: dict):
         super().__init__('param_tuning_client')
+        self.config = config
 
         self.set_clients = {}
         self.get_clients = {}
@@ -21,33 +22,44 @@ def __init__(self, config: dict):
                 while not self.get_clients[node_name].wait_for_service(timeout_sec=1.0):
                     self.get_logger().info(f'{node_name}/get_parameters service not available, waiting...')
 
-    def get_param(self, node_name: str, param_name: str) -> float:
+    def get_param(self, group: str, param: str, scaled: bool = True) -> float:
+        if not scaled or 'scale' not in self.config[group]['params'][param]:
+            scale = 1.0
+        else:
+            scale = self.config[group]['params'][param]['scale']
+        node_name = self.config[group]['node']
+
         request = GetParameters.Request()
-        request.names = [param_name]
+        request.names = [param]
+
         future = self.get_clients[node_name].call_async(request)
         rclpy.spin_until_future_complete(self, future)
         if future.result() is not None:
             if len(future.result().values) == 0:
-                self.get_logger().error(f'Parameter {param_name} not found')
+                self.get_logger().error(f'Parameter {param} not found')
                 return 0.0
             if future.result().values[0].type == ParameterType.PARAMETER_DOUBLE:
-                return future.result().values[0].double_value
+                return future.result().values[0].double_value * scale
             else:
-                self.get_logger().error(f'Unsupported parameter type for {param_name}, only double is supported')
+                self.get_logger().error(f'Unsupported parameter type for {param}, only double is supported')
         else:
             self.get_logger().error('Service call failed %r' % (future.exception(),))
 
-    def set_param(self, node_name: str, param_name: str, value: float) -> None:
+    def set_param(self, group: str, param: str, value: float, scaled: bool = True) -> None:
+        if scaled and 'scale' in self.config[group]['params'][param]:
+            value /= self.config[group]['params'][param]['scale']
+
+        node_name = self.config[group]['node']
         request = SetParameters.Request()
-        parameter = Parameter(param_name, Parameter.Type.DOUBLE, value)
+        parameter = Parameter(param, Parameter.Type.DOUBLE, value)
         request.parameters.append(parameter.to_parameter_msg())
         future = self.set_clients[node_name].call_async(request)
         rclpy.spin_until_future_complete(self, future)
         if future.result() is not None:
             if future.result().results[0].successful:
-                self.get_logger().info(f'Set {node_name}/{param_name} to {value}')
+                self.get_logger().info(f'Set {node_name}/{param} to {value}')
             else:
-                self.get_logger().error(f'Failed to set {param_name} to {value}: {future.result().results[0].reason}')
+                self.get_logger().error(f'Failed to set {param} to {value}: {future.result().results[0].reason}')
         else:
             self.get_logger().error('Service call failed %r' % (future.exception(),))
 

rosflight_rqt_plugins/src/param_tuning/param_tuning_widget.py

@@ -31,9 +31,8 @@ def __init__(self, config: dict, paramClient, paramFilepath):
         self.paramClient = paramClient
         self.valueStack = {}
         for group in config:
-            node_name = config[group]['node']
             for param in config[group]['params']:
-                value = self.paramClient.get_param(node_name, param)
+                value = self.paramClient.get_param(group, param)
                 self.valueStack[(group, param)] = [value]
 
         # Set up the widget
@@ -57,7 +56,7 @@ def setupTableModels(self):
             model = QStandardItemModel()
             model.setHorizontalHeaderLabels(self.tableHeaders)
             for param in self.config[group]['params']:
-                desc = self.config[group]['params'][param]
+                desc = self.config[group]['params'][param]['description']
                 param_item = QStandardItem(param)
                 param_item.setEditable(False)
                 value_item = QStandardItem('0.0')
@@ -126,10 +125,9 @@ def refreshTableValues(self):
         self.paramTableView.model().dataChanged.disconnect(self.onModelChange)
 
         # Get current values of the parameters
-        node_name = self.config[self.currentGroupKey]['node']
         for i in range(self.models[self.currentGroupKey].rowCount()):
             param = self.models[self.currentGroupKey].item(i, 0).text()
-            value = self.paramClient.get_param(node_name, param)
+            value = self.paramClient.get_param(self.currentGroupKey, param)
 
             # If the value is different from the previous value, add it to the stack and update the buttons
             if value != self.valueStack[(self.currentGroupKey, param)][-1]:
@@ -179,13 +177,12 @@ def saveButtonCallback(self):
         # Create a dictionary formatted for ROS parameters, based on the current ROS parameters
         for group in self.config:
             param_dict = {}
-            node_name = self.config[group]['node']
             for i in range(self.models[group].rowCount()):
                 param_name = self.models[group].item(i, 0).text()
-                param_dict[param_name] = self.paramClient.get_param(node_name, param_name)
+                param_dict[param_name] = self.paramClient.get_param(group, param_name, False)
 
             # Add new items to dictionary, appending it if already exists
-            stripped_node_name = node_name.lstrip('/')
+            stripped_node_name = self.config[group]['node'].lstrip('/')
             if stripped_node_name in params:
                 params[stripped_node_name]['ros__parameters'].update(param_dict)
             else:
@@ -207,8 +204,7 @@ def onModelChange(self, topLeft, bottomRight):
 
         # Set the new value
         param = self.models[self.currentGroupKey].item(topLeft.row(), 0).text()
-        node_name = self.config[self.currentGroupKey]['node']
-        self.paramClient.set_param(node_name, param, value)
+        self.paramClient.set_param(self.currentGroupKey, param, value)
 
         # Add the new value to the previous values list
         if self.addChangedValuesToHist: