Viacheslav/webots-module-b
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update nav_test_lidar_dynamic_v2.1.py

Browse Source
This commit is contained in:
Viacheslav
2026-04-02 08:40:31 +03:00
parent 545951da03
commit 44731cb3b3
1 changed files with 32 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files

148
nav_test_lidar_dynamic_v2.1.py
View File

@@ -16,28 +16,19 @@ from sensor_msgs.msg import Image, LaserScan
from std_msgs.msg import String from std_msgs.msg import String
# ── Настраиваемые коэффициенты ────────────────────────────────────────────── # ── Настраиваемые коэффициенты ──────────────────────────────────────────────
K_VISUAL = 0.55 # Мягче визуальная коррекция, чтобы снизить рыскание K_VISUAL = 0.8 # П-коэффициент визуального cross-track (агрессивная коррекция)
K_VISUAL_DECAY = 0.85 # Быстрее забываем старую visual error, если маркеры пропали K_VISUAL_DECAY = 0.92 # Коэффициент затухания визуальной ошибки (память)
VISUAL_FILTER_ALPHA = 0.65 # Сглаживание визуальной ошибки K_HEADING = 0.6 # П-регулятор удержания курса
VISUAL_ENABLE_THRESH = 0.015 K_CROSS_P = 2.0 # Пропорциональный коэффициент odom cross-track
K_HEADING = 0.95 # Сильнее удерживаем целевой yaw на прямой MAX_ANGULAR = 0.35 # Максимальная скорость поворота (рад/с)
K_CROSS_P = 1.35 # Коррекция бокового ухода по одометрии
CROSS_TRACK_FILTER_ALPHA = 0.75
MAX_ANGULAR = 0.28 # Чуть уменьшаем макс. угловую скорость на прямой
ANGULAR_RATE_LIMIT = 0.03 # Ограничение резкой смены angular.z за цикл
K_TURN = 1.5 # П-регулятор поворота на месте K_TURN = 1.5 # П-регулятор поворота на месте
DRIVE_SPEED = 0.24 # Базовая скорость движения вперёд (м/с) DRIVE_SPEED = 0.3 # Скорость движения вперёд (м/с)
MIN_DRIVE_SPEED = 0.11 # Минимальная скорость на прямой при коррекции
RECOVERY_SPEED = 0.08 # Осторожная скорость при большом отклонении
CELL_SIZE_M = 0.65 # Предполагаемая длина одной клетки поля (м) CELL_SIZE_M = 0.65 # Предполагаемая длина одной клетки поля (м)
HEADING_SLOWDOWN_RAD = math.radians(10.0)
CROSS_TRACK_SLOWDOWN_M = 0.05
VISUAL_SLOWDOWN_ERR = 0.18
# ── Параметры проверки препятствия лидаром ────────────────────────────────── # ── Параметры проверки препятствия лидаром ──────────────────────────────────
LIDAR_FORWARD_HALF_ANGLE_DEG = 15.0 # Проверяем узкий сектор строго перед роботом LIDAR_FORWARD_HALF_ANGLE_DEG = 15.0 # Проверяем узкий сектор строго перед роботом
LIDAR_MIN_VALID_RANGE_M = 0.12 # Игнорируем слишком близкие шумные значения LIDAR_MIN_VALID_RANGE_M = 0.05 # Игнорируем слишком близкие шумные значения
LIDAR_BLOCK_DISTANCE_M = 0.5 # Если впереди объект ближе этого порога — следующая клетка занята LIDAR_BLOCK_DISTANCE_M = 0.85 # Если впереди объект ближе этого порога — следующая клетка занята
LIDAR_CONFIRM_CYCLES = 1 # Сколько циклов подряд нужно подтвердить препятствие LIDAR_CONFIRM_CYCLES = 1 # Сколько циклов подряд нужно подтвердить препятствие
# ──────────────────────────────────────────────────────────────────────────── # ────────────────────────────────────────────────────────────────────────────
@@ -102,11 +93,6 @@ class RMC2Navigator(Node):
self._last_front_range = None self._last_front_range = None
self._occupancy_check_after_turn_pending = False self._occupancy_check_after_turn_pending = False
# Стабилизация прямолинейного хода.
self._filtered_cross_track = 0.0
self._last_angular_cmd = 0.0
self._last_visual_source = 'none'
# Таймер главного цикла (20 Гц) # Таймер главного цикла (20 Гц)
self.timer = self.create_timer(0.05, self.control_loop) self.timer = self.create_timer(0.05, self.control_loop)
@@ -395,7 +381,7 @@ class RMC2Navigator(Node):
self._line_start_x, self._line_start_y = self.get_marker_coords(self.current_marker_id) self._line_start_x, self._line_start_y = self.get_marker_coords(self.current_marker_id)
self._line_perp_yaw = self.target_yaw + math.pi / 2.0 self._line_perp_yaw = self.target_yaw + math.pi / 2.0
self._cross_track_ref_marker = self.current_marker_id self._cross_track_ref_marker = self.current_marker_id
self._reset_drive_stabilization() self._visual_err_x_memory = 0.0
self._reset_lidar_block_detector() self._reset_lidar_block_detector()
self._occupancy_check_after_turn_pending = True self._occupancy_check_after_turn_pending = True
@@ -408,62 +394,15 @@ class RMC2Navigator(Node):
def _cross_track_error(self, x, y): def _cross_track_error(self, x, y):
""" """
Рассчитывает signed cross-track error — расстояние от текущей позиции Рассчитывает signed cross-track error — расстояние от текущей позиции
до идеальной линии движения. до идеальной линии движения (с указанием знака: + справа, − слева).
Линия: проходит через (_line_start_x, _line_start_y) под углом target_yaw.
""" """
dx = x - self._line_start_x dx = x - self._line_start_x
dy = y - self._line_start_y dy = y - self._line_start_y
cross_track = -dx * math.sin(self.target_yaw) + dy * math.cos(self.target_yaw) cross_track = -dx * math.sin(self.target_yaw) + dy * math.cos(self.target_yaw)
return cross_track return cross_track
def _reset_drive_stabilization(self):
self._visual_err_x_memory = 0.0
self._filtered_cross_track = 0.0
self._last_angular_cmd = 0.0
self._last_visual_source = 'none'
def _limit_step(self, target, current, max_step):
if target > current + max_step:
return current + max_step
if target < current - max_step:
return current - max_step
return target
def _select_visual_guidance_error(self, ids_list, corners, img_w, img_h):
if not ids_list:
return None, 'none'
preferred_markers = []
if self.waypoint_idx < len(self.waypoints):
preferred_markers.append((self.waypoints[self.waypoint_idx], 'next_wp'))
if self.current_marker_id is not None:
preferred_markers.append((self.current_marker_id, 'current_wp'))
if self.target_marker_id is not None:
preferred_markers.append((self.target_marker_id, 'target'))
seen = set()
for marker_id, label in preferred_markers:
if marker_id in seen:
continue
seen.add(marker_id)
ex, ey = self._marker_offset(marker_id, ids_list, corners, img_w, img_h)
if ex is not None:
return ex, label
best_err_x = None
best_score = float('inf')
for mid in ids_list:
ex, ey = self._marker_offset(mid, ids_list, corners, img_w, img_h)
if ex is None:
continue
score = abs(ex) + 0.7 * abs(ey)
if score < best_score:
best_score = score
best_err_x = ex
if best_err_x is None:
return None, 'none'
return best_err_x, 'nearest_visible'
# ── ArUco-детектор (общий, переиспользуемый) ──────────────────────────── # ── ArUco-детектор (общий, переиспользуемый) ────────────────────────────
def _detect_markers(self): def _detect_markers(self):
@@ -614,54 +553,32 @@ class RMC2Navigator(Node):
self.cmd_pub.publish(twist) self.cmd_pub.publish(twist)
return return
visual_err_x, visual_source = self._select_visual_guidance_error( visual_err_x = None
ids_list, corners, img_w, img_h if ids_list:
) best_dist = float('inf')
for mid in ids_list:
ex, ey = self._marker_offset(mid, ids_list, corners, img_w, img_h)
if ex is None:
continue
dist = abs(ex) + abs(ey)
if dist < best_dist:
best_dist = dist
visual_err_x = ex
if visual_err_x is not None: if visual_err_x is not None:
self._visual_err_x_memory = ( self._visual_err_x_memory = visual_err_x
VISUAL_FILTER_ALPHA * self._visual_err_x_memory
+ (1.0 - VISUAL_FILTER_ALPHA) * visual_err_x
)
self._last_visual_source = visual_source
else: else:
self._visual_err_x_memory *= K_VISUAL_DECAY self._visual_err_x_memory *= K_VISUAL_DECAY
self._last_visual_source = 'none'
base_yaw_err = self.normalize_angle(self.target_yaw - self.current_yaw) base_yaw_err = self.normalize_angle(self.target_yaw - self.current_yaw)
raw_cross_track = self._cross_track_error(self.current_x, self.current_y)
self._filtered_cross_track = (
CROSS_TRACK_FILTER_ALPHA * self._filtered_cross_track
+ (1.0 - CROSS_TRACK_FILTER_ALPHA) * raw_cross_track
)
raw_angular = ( use_visual = abs(self._visual_err_x_memory) > 0.01
K_HEADING * base_yaw_err twist.linear.x = DRIVE_SPEED
- K_CROSS_P * self._filtered_cross_track
)
use_visual = abs(self._visual_err_x_memory) > VISUAL_ENABLE_THRESH
if use_visual: if use_visual:
raw_angular -= K_VISUAL * self._visual_err_x_memory twist.angular.z = K_HEADING * base_yaw_err - K_VISUAL * self._visual_err_x_memory
else:
raw_angular = max(-MAX_ANGULAR, min(MAX_ANGULAR, raw_angular)) twist.angular.z = K_HEADING * base_yaw_err
twist.angular.z = self._limit_step(raw_angular, self._last_angular_cmd, ANGULAR_RATE_LIMIT) twist.angular.z = max(-MAX_ANGULAR, min(MAX_ANGULAR, twist.angular.z))
self._last_angular_cmd = twist.angular.z
heading_ratio = min(1.0, abs(base_yaw_err) / HEADING_SLOWDOWN_RAD)
cross_ratio = min(1.0, abs(self._filtered_cross_track) / CROSS_TRACK_SLOWDOWN_M)
visual_ratio = min(1.0, abs(self._visual_err_x_memory) / VISUAL_SLOWDOWN_ERR)
slowdown = max(heading_ratio, cross_ratio, visual_ratio)
twist.linear.x = DRIVE_SPEED - (DRIVE_SPEED - MIN_DRIVE_SPEED) * slowdown
if (
abs(base_yaw_err) > math.radians(18.0)
or abs(self._filtered_cross_track) > 0.09
or abs(self._visual_err_x_memory) > 0.24
):
twist.linear.x = min(twist.linear.x, RECOVERY_SPEED)
twist.linear.x = max(RECOVERY_SPEED, min(DRIVE_SPEED, twist.linear.x))
if not hasattr(self, '_cross_log_counter'): if not hasattr(self, '_cross_log_counter'):
self._cross_log_counter = 0 self._cross_log_counter = 0
@@ -669,10 +586,9 @@ class RMC2Navigator(Node):
if self._cross_log_counter % 10 == 0: if self._cross_log_counter % 10 == 0:
lidar_msg = 'n/a' if self._last_front_range is None else f'{self._last_front_range:.2f} m' lidar_msg = 'n/a' if self._last_front_range is None else f'{self._last_front_range:.2f} m'
self.get_logger().info( self.get_logger().info(
f'[STAB] vis={self._visual_err_x_memory:+.3f} ({self._last_visual_source}), ' f'[VIS-MEM] err_x={self._visual_err_x_memory:+.3f}, '
f'cross={self._filtered_cross_track:+.3f} m, '
f'yaw={math.degrees(base_yaw_err):+.1f}°, ' f'yaw={math.degrees(base_yaw_err):+.1f}°, '
f'v={twist.linear.x:.2f}, w={twist.angular.z:+.3f}, lidar={lidar_msg}' f'tw.z={twist.angular.z:+.3f}, lidar={lidar_msg}'
) )
# ── Визуальное центрирование над финальным маркером ───────────────── # ── Визуальное центрирование над финальным маркером ─────────────────