ROS2工程实践之参数处理技巧

上周末在忙着给FishBot的配套摄像头写ROS2驱动，外加上周加班导致身体不舒服就没发文，今天分享下小鱼在写配套驱动中，关于参数处理的一些心得和技巧。

首先介绍下这个无线摄像头。它基于ESP32系列的无线模块，小鱼重新设计了一块底板，并编写了新的固件。这样就可以利用配置助手来轻松完成固件的烧录和配置。当然，仅仅有硬件是不够的，为了能够在ROS2中获取图像，并结合小鱼之前开发的yolov5_ros2开源库进行目标检测等任务，小鱼还为这个摄像头编写了一个配套的ROS2驱动。

这个摄像头拥有众多可调整的参数，如画面大小、前照灯亮度、上下反转和左右镜像等。在ROS2中，我选择了通过节点参数来进行这些参数的封装。但这里存在一个问题：每个参数都有其特定的取值范围。例如，亮度的范围是0-255，图像质量的范围是4-63，而上下反转这个参数则只能是0或1。因此，在代码中，我不能仅仅声明这些参数，还需要对它们的取值范围进行限制。

为了解决这个问题，我编写了一个函数declare_param_with_range，用于声明带有取值范围的参数。下面是这个函数的完整代码：

from rcl_interfaces.msg import ParameterDescriptor, IntegerRange

class CameraDriverNode:
    # ... 其他类成员和方法 ...

    def declare_param_with_range(self, name, value, start, end):
        pd = ParameterDescriptor()
        pd_range = IntegerRange()
        pd_range.from_value = start
        pd_range.to_value = end
        pd_range.step = 1
        pd.integer_range = [pd_range]  # 注意这里需要使用列表
        self.declare_parameter(name, value, pd)

    # ... 其他类成员和方法 ...

使用这个函数，我可以为每个参数指定一个合适的范围，并确保用户在设置参数时不会超出这个范围。

然而，仅仅声明参数并限制其取值范围还不够。当参数的值发生改变时，我们需要能够动态地感知到这个变化，并将这个变化同步到摄像头设备上。为了实现这一功能，我利用了ROS2的参数更新回调机制。

在ROS2中，我们可以通过add_on_set_parameters_callback方法为节点添加一个参数更新回调函数。当参数的值发生改变时，ROS2会自动调用这个回调函数，并将改变后的参数作为参数传入。下面是参数更新回调的完整代码实现：

from rcl_interfaces.msg import SetParametersResult, SetParameterResult

class CameraDriverNode:
    # ... 其他类成员和方法 ...

    def __init__(self):
        # ... 初始化其他部分 ...
        self.add_on_set_parameters_callback(self.parameter_callback)

    def parameter_callback(self, parameters):
        results = []
        for parameter in parameters:
            try:
                # 获取参数的新值，并进行相应的处理
                new_value = parameter.value
                self.get_logger().info(f'参数 {parameter.name} 设置为：{new_value}')
                self.sync_param(parameter.name, new_value)
                # 构造参数设置结果
                result = SetParameterResult(successful=True)
            except Exception as e:
                self.get_logger().error(f'设置参数 {parameter.name} 失败：{e}')
                result = SetParameterResult(successful=False, reason=str(e))
            results.append(result)
        # 返回所有参数的设置结果
        return SetParametersResult(successful=True, results=results)

    def sync_param(self, param_name, param_value):
        # 这里应该实现将参数值同步到摄像头设备的逻辑
        # 例如，发送命令到摄像头以设置相应的参数值
        pass  # 这里留空，需要根据具体硬件接口实现

    # ... 其他类成员和方法 ...

在这个回调函数中，首先遍历了所有改变的参数，获取了它们的新值，并执行了相应的处理逻辑（例如，调用sync_param函数将新值同步到摄像头设备上）。然后为每个参数构造了一个SetParameterResult消息，表示该参数的设置是否成功，并将这些结果添加到results列表中。最后返回了一个SetParametersResult消息，其中包含了所有参数的设置结果。