2.2算法控制模板

算法节点

tip

你是否需要去了解一个算法？

从功利角度来说：

• 当你需要改进这个算法时，需要了解它的工作原理和实现细节。
• 当你需要调参时，需要了解参数背后的意义。

如果某个算法默认参数表现足够，其实可以先不用知道它是怎么实现的。

下面代码实现了一个视觉运动控制节点，从/camera话题订阅图像数据，并处理后发布到/cmd_vel话题。

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import numpy as np
from geometry_msgs.msg import Twist

class VisualMotionController(Node):
    def __init__(self):
        super().__init__('visual_motion_controller')
        
        # 订阅摄像头话题 (simple.sdf 中是 /camera)
        self.image_subscription = self.create_subscription(
            Image,
            '/camera',
            self.process_and_control,
            10
        )
        
        # 发布控制话题 (simple.sdf 中是 /cmd_vel)
        self.velocity_publisher = self.create_publisher(
            Twist,
            '/cmd_vel',
            10
        )

        self.bridge = CvBridge()
        self.move_speed = 0.5
        self.turn_speed = 0.5
        self.step = 0  # 计步器
        self.get_logger().info('视觉运动控制节点已启动')
        
        
    def process_and_control(self, msg):
        """处理图像并生成控制命令"""
        try:
            # 将ROS图像消息转换为OpenCV格式
            cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
            cv2.imshow('car',cv_image)
            cv2.waitKey(1)
            self.step += 1
            
            # 这里可以添加你的视觉处理算法
            # 例如：检测障碍物、识别路径等
            # gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
            # edges = cv2.Canny(gray, 50, 150)
            # 在这里编写你的运动控制算法
            
            # 简单的控制逻辑示例
            cmd = Twist()
            if self.step < 100:
                # 前进100帧
                cmd.linear.x = self.move_speed
                cmd.angular.z = 0.0
                self.get_logger().info(f'前进中... 步数: {self.step}')
            elif self.step < 160:
                # 左转60帧
                cmd.linear.x = 0.0
                cmd.angular.z = self.turn_speed
                self.get_logger().info(f'左转中... 步数: {self.step}')
            else:
                # 重置计步器
                self.step = 0

            self.velocity_publisher.publish(cmd)
            
        except Exception as e:
            self.get_logger().error(f'处理图像时出错: {str(e)}')
        

def main(args=None):
    """主函数"""
    rclpy.init(args=args)
    
    # 创建视觉运动控制节点
    controller = VisualMotionController()
    
    try:
        # 启动节点
        rclpy.spin(controller)
    except KeyboardInterrupt:
        controller.get_logger().info('节点被用户中断')
    finally:
        # 发送停止指令
        stop_cmd = Twist()
        controller.velocity_publisher.publish(stop_cmd)
        # 清理资源
        controller.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

启动命令：

# 编译工作空间（以learning_node为例）
colcon build --packages-select learning_node

# 启动节点（以learning_node为例）
ros2 run learning_node node_helloworld

项目目标是在一个凹字形的地图上完成循迹运动，同时注意避开斑马线上的行人。

不是一个很难的项目，但是一个和实际项目流程一致要独立完成思路设计、算法选型、参数可视化的项目。

任务分析与思路：