from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
node = rclpy.create_node('simple_follower')
pub = node.create_publisher(Twist, '/cmd_vel', 10)
total_points = len(msg.ranges)
front_sector = msg.ranges[0:100]
left_sector_start = total_points // 4
left_sector = msg.ranges[left_sector_start-70:left_sector_start+70]
front_distances = [r for r in front_sector if 0.01 < r < 10.0]
left_distances = [r for r in left_sector if 0.01 < r < 10.0]
# Выводим расстояние до передней стены
front_distance = min(front_distances)
front_clear = front_distance > 0.24
# Выводим расстояние в консоль
node.get_logger().info(f'Distance to front wall: {front_distance:.2f}m')
node.get_logger().info('No front sensor data')
wall_on_left = min(left_distances) <= 9
if front_clear and wall_on_left:
node.get_logger().info('Moving: wall on left and path clear ahead')
node.get_logger().warn('Stopping: obstacle ahead!')
node.get_logger().info('Waiting: no wall on left side')
node.create_subscription(LaserScan, '/scan', scan_callback, 10)
Подробное объяснение кода:
1. Импорт библиотек и инициализация ROS 2
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
· rclpy — основной клиент ROS 2 на Python
· Twist — сообщение для управления скоростью (линейной и угловой)
· LaserScan — сообщение с данными лазерного сканера
node = rclpy.create_node('simple_follower')
· rclpy.init() — инициализация ROS 2
· Создается узел с именем 'simple_follower'
2. Создание публикатора для управления роботом
pub = node.create_publisher(Twist, '/cmd_vel', 10)
· Создается публикатор, который будет отправлять команды скорости на топик /cmd_vel
· 10 — размер очереди сообщений (QoS)
3. Основная функция-обработчик данных лидара
Эта функция вызывается каждый раз, когда приходят новые данные с лидара.
3.1. Инициализация и подготовка данных
total_points = len(msg.ranges)
· Создается объект twist для управления скоростью
· total_points — общее количество измерений лидара (обычно 360°)
3.2. Выделение секторов сканирования
front_sector = msg.ranges[0:100]
left_sector_start = total_points // 4
left_sector = msg.ranges[left_sector_start-70:left_sector_start+70]
· Схема работы лидара в ROS:
· msg.ranges[0] — измерение справа от робота (0°)
· msg.ranges[90] — сзади робота (90°)
· msg.ranges[180] — слева от робота (180°)
· msg.ranges[270] — спереди робота (270°)
· Полный круг = 360 точек (зависит от настройки лидара)
· front_sector = индексы [0:100] — это сектор справа-спереди (примерно 100° справа)
Внимание! Здесь возможна ошибка в логике — скорее всего, должно быть не [0:100], а сектор спереди
· left_sector = центр на total_points // 4 (90°) — это сектор слева от робота
Диапазон ±70 точек от центра (широкая зона слева)
3.3. Фильтрация данных лидара
front_distances = [r for r in front_sector if 0.01 < r < 10.0]
left_distances = [r for r in left_sector if 0.01 < r < 10.0]
· Отфильтровываются некорректные значения:
· r < 0.01 — слишком близко или ошибка датчика
· r > 10.0 — слишком далеко (лидар не видит)
3.4. Логика принятия решений
# Анализ препятствий спереди
front_distance = min(front_distances) # Ближайшее препятствие
front_clear = front_distance > 0.24 # Если больше 24 см — путь свободен
node.get_logger().info(f'Distance to front wall: {front_distance:.2f}m')
node.get_logger().info('No front sensor data')
wall_on_left = min(left_distances) <= 9 # Если что-то ближе 9 метров слева
3.5. Логика управления роботом
if front_clear and wall_on_left:
twist.linear.x = 0.2 # Едем вперед со скоростью 0.2 м/с
node.get_logger().info('Moving: wall on left and path clear ahead')
twist.linear.x = 0.0 # Стоим
node.get_logger().warn('Stopping: obstacle ahead!')
node.get_logger().info('Waiting: no wall on left side')
1. Двигаться вперед, только если:
· Спереди нет препятствий ближе 24 см
· Слева есть стена (что-то ближе 9 метров)
2. Останавливаться, если:
3.6. Отправка команды движения
pub.publish(twist) # Публикуем команду скорости
4. Подписка на данные лидара и запуск
node.create_subscription(LaserScan, '/scan', scan_callback, 10)
· Создается подписка на топик /scan (данные лидара)
· При получении сообщения вызывается scan_callback
· Запускает бесконечный цикл обработки сообщений ROS
· Узел работает, пока его не завершат
