lx/go2_navigation_provincial
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: 初始化 Go2 导航相关代码与配置

Browse Source
This commit is contained in:
龙正扬应用学生后端
2026-10-15 09:40:29 +08:00
commit 07426fcc09
1282 changed files with 331220 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
go2_autonomous_navigation/pending_patches/uncommitted_backup/dashboard.env Normal file
View File

@@ -0,0 +1,11 @@
# Allow AMCL map->odom post-dating tolerance; controller_server uses TF across map/odom/base.
GO2_NAV_HEALTH_TF_STAMP_FRESHNESS_S=2.0
# HTTP video stream from robot (bypasses DDS VideoClient)
GO2_DASHBOARD_VIDEO_URL=http://192.168.123.18:8080/video
GO2_REALTIME_SEGMENT_URL=http://192.168.3.11:5000/realtime-segment
GO2_INSPECTION_ARRIVAL_STABLE_HOLD_S=0.5
GO2_INSPECTION_MAX_YAW_RATE_DEG_S=8.0
GO2_INSPECTION_MAX_LINEAR_SPEED_MPS=0.15

22
go2_autonomous_navigation/pending_patches/uncommitted_backup/default.yaml Normal file
View File

@@ -0,0 +1,22 @@
route_name: default
frame_id: map
created_by: dashboard
waypoints:
- name: waypoint_1
frame_id: map
x: 0.235
y: -0.214
yaw: 1.562
enabled: true
- name: waypoint_2
frame_id: map
x: 1.963
y: 0.303
yaw: -1.489
enabled: true
- name: waypoint_3
frame_id: map
x: 0.688
y: -0.123
yaw: 3.075
enabled: true

661
go2_autonomous_navigation/pending_patches/uncommitted_backup/nav2_params.yaml Normal file
View File

@@ -0,0 +1,661 @@
amcl:
ros__parameters:
use_sim_time: false
alpha1: 0.1 # 旋转噪声（小角度转向误差）
alpha2: 0.1 # 旋转噪声（大角度转向误差）
alpha3: 0.1 # 平移噪声（直线运动误差）
alpha4: 0.1 # 平移噪声（侧向运动误差，全向模型关键）
alpha5: 0.3 # 平移时的旋转耦合噪声（保持不变）
base_frame_id: "base_link"
beam_skip_distance: 0.5
beam_skip_error_threshold: 0.9
beam_skip_threshold: 0.3
do_beamskip: true #false
global_frame_id: "map"
lambda_short: 0.1
laser_likelihood_max_dist: 1.0
laser_max_range: 10.0
laser_min_range: 0.0
laser_model_type: "likelihood_field"
max_beams: 120
max_particles: 8000 #2000
min_particles: 3000 #500
odom_frame_id: "odom"
pf_err: 0.08
pf_z: 0.99
recovery_alpha_fast: 0.0
recovery_alpha_slow: 0.0
resample_interval: 1
robot_model_type: "nav2_amcl::OmniMotionModel" # nav2_amcl::DifferentialMotionModel差分 nav2_amcl::OmniMotionModel全向
save_pose_rate: 0.5
always_reset_initial_pose: false
sigma_hit: 0.2
tf_broadcast: true
transform_tolerance: 1.0 # AMCL publishes map->odom post-dated; keep >= Nav2 default to avoid stale TF aborts
update_min_a: 0.05 #0.2
update_min_d: 0.05 #0.25
z_hit: 0.5 # 降低有效命中权重（激光噪声多）
z_max: 0.05
z_rand: 0.5 # 提高随机噪声权重（容忍更多异常值）
z_short: 0.05
scan_topic: /scan
bt_navigator:
ros__parameters:
use_sim_time: false
global_frame: map
robot_base_frame: base_link
odom_topic: /odom
bt_loop_duration: 20
default_server_timeout: 20
wait_for_service_timeout: 1000
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
# They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_compute_path_through_poses_action_bt_node
- nav2_smooth_path_action_bt_node
- nav2_follow_path_action_bt_node
- nav2_spin_action_bt_node
- nav2_wait_action_bt_node
- nav2_assisted_teleop_action_bt_node
- nav2_back_up_action_bt_node
- nav2_drive_on_heading_bt_node
- nav2_clear_costmap_service_bt_node
- nav2_is_stuck_condition_bt_node
- nav2_goal_reached_condition_bt_node
- nav2_goal_updated_condition_bt_node
- nav2_globally_updated_goal_condition_bt_node
- nav2_is_path_valid_condition_bt_node
- nav2_initial_pose_received_condition_bt_node
- nav2_reinitialize_global_localization_service_bt_node
- nav2_rate_controller_bt_node
- nav2_distance_controller_bt_node
- nav2_speed_controller_bt_node
- nav2_truncate_path_action_bt_node
- nav2_truncate_path_local_action_bt_node
- nav2_goal_updater_node_bt_node
- nav2_recovery_node_bt_node
- nav2_pipeline_sequence_bt_node
- nav2_round_robin_node_bt_node
- nav2_transform_available_condition_bt_node
- nav2_time_expired_condition_bt_node
- nav2_path_expiring_timer_condition
- nav2_distance_traveled_condition_bt_node
- nav2_single_trigger_bt_node
- nav2_goal_updated_controller_bt_node
- nav2_is_battery_low_condition_bt_node
- nav2_navigate_through_poses_action_bt_node
- nav2_navigate_to_pose_action_bt_node
- nav2_remove_passed_goals_action_bt_node
- nav2_planner_selector_bt_node
- nav2_controller_selector_bt_node
- nav2_goal_checker_selector_bt_node
- nav2_controller_cancel_bt_node
- nav2_path_longer_on_approach_bt_node
- nav2_wait_cancel_bt_node
- nav2_spin_cancel_bt_node
- nav2_back_up_cancel_bt_node
- nav2_assisted_teleop_cancel_bt_node
- nav2_drive_on_heading_cancel_bt_node
- nav2_is_battery_charging_condition_bt_node
bt_navigator_navigate_through_poses_rclcpp_node:
ros__parameters:
use_sim_time: false
bt_navigator_navigate_to_pose_rclcpp_node:
ros__parameters:
use_sim_time: false
controller_server:
ros__parameters:
use_sim_time: false
# 控制器频率略提高，轨迹跟踪更跟手（仍受 max_speed_xy 限制）
controller_frequency: 15.0
min_x_velocity_threshold: 0.01
min_y_velocity_threshold: 0.05
min_theta_velocity_threshold: 0.001
failure_tolerance: 0.5
progress_checker_plugin: "progress_checker"
goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
controller_plugins: ["FollowPath"]
# Progress checker parameters
progress_checker:
plugin: "nav2_controller::PoseProgressChecker"
required_movement_radius: 0.10
required_movement_angle: 0.04
movement_time_allowance: 20.0
# Goal checker parameters
#precise_goal_checker:
# plugin: "nav2_controller::SimpleGoalChecker"
# xy_goal_tolerance: 0.25
# yaw_goal_tolerance: 3.14
# stateful: True
general_goal_checker:
stateful: False
plugin: "nav2_controller::SimpleGoalChecker"
# Nav2 must reach the waypoint position tightly; dashboard handles only
# the final yaw alignment and dwell confirmation.
xy_goal_tolerance: 0.25
yaw_goal_tolerance: 3.14
# DWB parameters
FollowPath:
plugin: "dwb_core::DWBLocalPlanner"
debug_trajectory_details: False
min_vel_x: 0.0
min_vel_y: 0.0
max_vel_x: 0.4
max_vel_y: 0.0
max_vel_theta: 0.45
min_vel_theta: -0.45
min_speed_xy: 0.0
# 合成线速度上限 sqrt(vx^2+vy^2)，原先 0.26 过慢；可按场地逐步加到 0.7~1.0
max_speed_xy: 0.4
min_speed_theta: 0.0
# Add high threshold velocity for turtlebot 3 issue.
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
acc_lim_x: 1.2
acc_lim_y: 0.0
acc_lim_theta: 1.0
decel_lim_x: -1.2
decel_lim_y: 0.0
decel_lim_theta: -1.0
vx_samples: 12
vy_samples: 1
vtheta_samples: 40
sim_time: 0.8
linear_granularity: 0.05
angular_granularity: 0.025
transform_tolerance: 0.2
xy_goal_tolerance: 0.25
trans_stopped_velocity: 0.08
short_circuit_trajectory_evaluation: False
stateful: True
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
BaseObstacle.scale: 0.10
PathAlign.scale: 32.0
PathAlign.forward_point_distance: 0.1
GoalAlign.scale: 8.0
GoalAlign.forward_point_distance: 0.1
PathDist.scale: 32.0
GoalDist.scale: 24.0
RotateToGoal.scale: 32.0
RotateToGoal.slowing_factor: 2.0
RotateToGoal.lookahead_time: 1.0
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 2.0
global_frame: odom
robot_base_frame: base_link
use_sim_time: false
rolling_window: true
transform_tolerance: 0.5
width: 4
height: 4
resolution: 0.05
robot_radius: 0.22
plugins: ["voxel_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.50
voxel_layer:
plugin: "nav2_costmap_2d::VoxelLayer"
enabled: True
publish_voxel_map: false
origin_z: -0.2
z_resolution: 0.05
z_voxels: 16
max_obstacle_height: 1.0
mark_threshold: 1
observation_sources: scan
scan:
topic: /go2/scan
max_obstacle_height: 1.0
clearing: True
marking: True
data_type: "LaserScan"
raytrace_max_range: 3.0
raytrace_min_range: 0.2
obstacle_max_range: 2.5
obstacle_min_range: 0.2
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
always_send_full_costmap: True
global_costmap:
global_costmap:
ros__parameters:
update_frequency: 1.0
publish_frequency: 1.0
global_frame: map
robot_base_frame: base_link
use_sim_time: false
robot_radius: 0.22
resolution: 0.05
track_unknown_space: true
transform_tolerance: 0.5
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
observation_sources: scan
scan:
topic: /go2/scan
max_obstacle_height: 1.0
clearing: True
marking: True
data_type: "LaserScan"
raytrace_max_range: 3.0
raytrace_min_range: 0.2
obstacle_max_range: 2.5
obstacle_min_range: 0.2
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.50
always_send_full_costmap: True
map_server:
ros__parameters:
use_sim_time: false
# Overridden in launch by the "map" launch configuration or provided default value.
# To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below.
yaml_filename: ""
map_saver:
ros__parameters:
use_sim_time: false
save_map_timeout: 5.0
free_thresh_default: 0.25
occupied_thresh_default: 0.70 #0.65
map_subscribe_transient_local: True
planner_server:
ros__parameters:
expected_planner_frequency: 15.0
use_sim_time: false
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_navfn_planner/NavfnPlanner"
tolerance: 0.7
use_astar: false
allow_unknown: true
smoother_server:
ros__parameters:
use_sim_time: false
smoother_plugins: ["simple_smoother"]
simple_smoother:
plugin: "nav2_smoother::SimpleSmoother"
tolerance: 1.0e-10
max_its: 1000
do_refinement: True
behavior_server:
ros__parameters:
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 8.0
behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
spin:
plugin: "nav2_behaviors/Spin"
backup:
plugin: "nav2_behaviors/BackUp"
drive_on_heading:
plugin: "nav2_behaviors/DriveOnHeading"
wait:
plugin: "nav2_behaviors/Wait"
assisted_teleop:
plugin: "nav2_behaviors/AssistedTeleop"
global_frame: odom
robot_base_frame: base_link
transform_tolerance: 0.5
use_sim_time: false
simulate_ahead_time: 1.0 #1.0
max_rotational_vel: 0.80
min_rotational_vel: 0.25
rotational_acc_lim: 1.6
robot_state_publisher:
ros__parameters:
use_sim_time: false
waypoint_follower:
ros__parameters:
use_sim_time: false
loop_rate: 20
stop_on_failure: false
waypoint_task_executor_plugin: "wait_at_waypoint"
wait_at_waypoint:
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
enabled: True
waypoint_pause_duration: 2
velocity_smoother:
ros__parameters:
use_sim_time: false
smoothing_frequency: 20.0
scale_velocities: False
# go2_driver 发布的 /odom 未填充 twist，CLOSED_LOOP 会把速度误判为 0 导致走半步就停
feedback: "OPEN_LOOP"
# 需 >= DWB 期望速度；略放宽加减速，体感更利落
max_velocity: [0.55, 0.0, 0.80]
min_velocity: [0.0, 0.0, -0.80]
max_accel: [1.2, 0.0, 1.6]
max_decel: [-1.2, 0.0, -1.6]
odom_topic: "odom"
odom_duration: 0.1
deadband_velocity: [0.0, 0.0, 0.0]
velocity_timeout: 1.0
# amcl:
# ros__parameters:
# use_sim_time: False
# alpha1: 0.1
# alpha2: 0.1
# alpha3: 0.1
# alpha4: 0.1
# alpha5: 0.2
# base_frame_id: "base_link"
# beam_skip_distance: 0.5
# beam_skip_error_threshold: 0.9
# beam_skip_threshold: 0.3
# do_beamskip: false
# global_frame_id: "map"
# lambda_short: 0.1
# laser_likelihood_max_dist: 1.0
# laser_max_range: 100.0
# laser_min_range: -1.0
# laser_model_type: "likelihood_field"
# max_beams: 120
# max_particles: 2000
# min_particles: 500
# odom_frame_id: "odom"
# pf_err: 0.05
# pf_z: 0.99
# recovery_alpha_fast: 0.0
# recovery_alpha_slow: 0.0
# resample_interval: 1
# robot_model_type: "differential"
# save_pose_rate: 0.5
# sigma_hit: 0.2
# tf_broadcast: true
# transform_tolerance: 1.0
# update_min_a: 0.2
# update_min_d: 0.25
# z_hit: 0.5
# z_max: 0.05
# z_rand: 0.5
# z_short: 0.05
# scan_topic: /scan
# amcl_map_client:
# ros__parameters:
# use_sim_time: False
# amcl_rclcpp_node:
# ros__parameters:
# use_sim_time: False
# bt_navigator:
# ros__parameters:
# use_sim_time: False
# global_frame: map
# robot_base_frame: base_link
# odom_topic: /odom
# enable_groot_monitoring: True
# groot_zmq_publisher_port: 1666
# groot_zmq_server_port: 1667
# default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
# plugin_lib_names:
# - nav2_compute_path_to_pose_action_bt_node
# - nav2_follow_path_action_bt_node
# - nav2_back_up_action_bt_node
# - nav2_spin_action_bt_node
# - nav2_wait_action_bt_node
# - nav2_clear_costmap_service_bt_node
# - nav2_is_stuck_condition_bt_node
# - nav2_goal_reached_condition_bt_node
# - nav2_goal_updated_condition_bt_node
# - nav2_initial_pose_received_condition_bt_node
# - nav2_reinitialize_global_localization_service_bt_node
# - nav2_rate_controller_bt_node
# - nav2_distance_controller_bt_node
# - nav2_speed_controller_bt_node
# - nav2_truncate_path_action_bt_node
# - nav2_goal_updater_node_bt_node
# - nav2_recovery_node_bt_node
# - nav2_pipeline_sequence_bt_node
# - nav2_round_robin_node_bt_node
# - nav2_transform_available_condition_bt_node
# - nav2_time_expired_condition_bt_node
# - nav2_distance_traveled_condition_bt_node
# bt_navigator_rclcpp_node:
# ros__parameters:
# use_sim_time: False
# controller_server:
# ros__parameters:
# use_sim_time: False
# controller_frequency: 20.0
# min_x_velocity_threshold: 0.001
# min_y_velocity_threshold: 0.5
# min_theta_velocity_threshold: 0.001
# progress_checker_plugin: "progress_checker"
# goal_checker_plugin: "goal_checker"
# controller_plugins: ["FollowPath"]
# # Progress checker parameters
# progress_checker:
# plugin: "nav2_controller::SimpleProgressChecker"
# required_movement_radius: 0.5
# movement_time_allowance: 10.0
# # Goal checker parameters
# goal_checker:
# plugin: "nav2_controller::SimpleGoalChecker"
# xy_goal_tolerance: 0.25
# yaw_goal_tolerance: 3.14
# stateful: True
# # DWB parameters
# FollowPath:
# plugin: "dwb_core::DWBLocalPlanner"
# debug_trajectory_details: True
# min_vel_x: 0.0
# min_vel_y: 0.0
# max_vel_x: 3.0 #changed
# max_vel_y: 0.0
# max_vel_theta: 1.5 #changed
# min_speed_xy: 0.0
# max_speed_xy: 3.0 #changed
# min_speed_theta: 0.0
# # Add high threshold velocity for turtlebot 3 issue.
# # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
# acc_lim_x: 10.0
# acc_lim_y: 0.0
# acc_lim_theta: 5.0
# decel_lim_x: -2.5
# decel_lim_y: 0.0
# decel_lim_theta: -3.2
# vx_samples: 20
# vy_samples: 5
# vtheta_samples: 40
# sim_time: 0.8
# linear_granularity: 0.05
# angular_granularity: 0.025
# transform_tolerance: 0.2
# xy_goal_tolerance: 0.25
# trans_stopped_velocity: 0.25
# short_circuit_trajectory_evaluation: False
# stateful: True
# critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
# BaseObstacle.scale: 0.10
# PathAlign.scale: 32.0
# PathAlign.forward_point_distance: 0.1
# GoalAlign.scale: 24.0
# GoalAlign.forward_point_distance: 0.1
# PathDist.scale: 32.0
# GoalDist.scale: 24.0
# RotateToGoal.scale: 32.0
# RotateToGoal.slowing_factor: 2.0
# RotateToGoal.lookahead_time: 1.0
# controller_server_rclcpp_node:
# ros__parameters:
# use_sim_time: False
# local_costmap:
# local_costmap:
# ros__parameters:
# update_frequency: 10.0
# publish_frequency: 10.0
# global_frame: odom
# robot_base_frame: base_link
# use_sim_time: False
# rolling_window: true
# width: 3
# height: 3
# resolution: 0.05
# robot_radius: 0.22
# plugins: ["voxel_layer", "inflation_layer"]
# inflation_layer:
# plugin: "nav2_costmap_2d::InflationLayer"
# cost_scaling_factor: 3.0
# inflation_radius: 0.55
# voxel_layer:
# plugin: "nav2_costmap_2d::VoxelLayer"
# enabled: True
# publish_voxel_map: True
# origin_z: 0.0
# z_resolution: 0.05
# z_voxels: 16
# max_obstacle_height: 2.0
# mark_threshold: 0
# observation_sources: scan
# scan:
# topic: /go2/scan
# max_obstacle_height: 2.0
# clearing: True
# marking: True
# data_type: "LaserScan"
# static_layer:
# map_subscribe_transient_local: True
# always_send_full_costmap: True
# local_costmap_client:
# ros__parameters:
# use_sim_time: False
# local_costmap_rclcpp_node:
# ros__parameters:
# use_sim_time: False
# global_costmap:
# global_costmap:
# ros__parameters:
# update_frequency: 1.0
# publish_frequency: 1.0
# global_frame: map
# robot_base_frame: base_link
# use_sim_time: False
# robot_radius: 0.22
# resolution: 0.05
# track_unknown_space: true
# plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
# obstacle_layer:
# plugin: "nav2_costmap_2d::ObstacleLayer"
# enabled: True
# observation_sources: scan
# scan:
# topic: /go2/scan
# max_obstacle_height: 2.0
# clearing: True
# marking: True
# data_type: "LaserScan"
# static_layer:
# plugin: "nav2_costmap_2d::StaticLayer"
# map_subscribe_transient_local: True
# inflation_layer:
# plugin: "nav2_costmap_2d::InflationLayer"
# cost_scaling_factor: 3.0
# inflation_radius: 0.55
# always_send_full_costmap: True
# global_costmap_client:
# ros__parameters:
# use_sim_time: False
# global_costmap_rclcpp_node:
# ros__parameters:
# use_sim_time: False
# map_server:
# ros__parameters:
# use_sim_time: False
# yaml_filename: "turtlebot3_world.yaml"
# map_saver:
# ros__parameters:
# use_sim_time: False
# save_map_timeout: 5000
# free_thresh_default: 0.25
# occupied_thresh_default: 0.65
# map_subscribe_transient_local: False
# planner_server:
# ros__parameters:
# expected_planner_frequency: 20.0
# use_sim_time: False
# planner_plugins: ["GridBased"]
# GridBased:
# plugin: "nav2_navfn_planner/NavfnPlanner"
# tolerance: 0.5
# use_astar: false
# allow_unknown: true
# planner_server_rclcpp_node:
# ros__parameters:
# use_sim_time: False
# recoveries_server:
# ros__parameters:
# costmap_topic: local_costmap/costmap_raw
# footprint_topic: local_costmap/published_footprint
# cycle_frequency: 10.0
# recovery_plugins: ["spin", "back_up", "wait"]
# spin:
# plugin: "nav2_recoveries/Spin"
# back_up:
# plugin: "nav2_recoveries/BackUp"
# wait:
# plugin: "nav2_recoveries/Wait"
# global_frame: odom
# robot_base_frame: base_link
# transform_timeout: 0.1
# use_sim_time: False
# simulate_ahead_time: 2.0
# max_rotational_vel: 1.0
# min_rotational_vel: 0.4
# rotational_acc_lim: 3.2
# robot_state_publisher:
# ros__parameters:
# use_sim_time: False

422
go2_autonomous_navigation/pending_patches/uncommitted_backup/patrol_executor_node.py Normal file
View File

@@ -0,0 +1,422 @@
from __future__ import annotations
import json
import math
import os
import time
import threading
from pathlib import Path
from typing import Optional
import rclpy
from action_msgs.msg import GoalStatus
from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped, Twist
from std_srvs.srv import Empty
from nav2_msgs.action import NavigateToPose
from rclpy.action import ActionClient
from rclpy.duration import Duration
from rclpy.executors import ExternalShutdownException, MultiThreadedExecutor
from rclpy.node import Node
from rclpy.task import Future
from rclpy.time import Time
from std_msgs.msg import String
from std_srvs.srv import Trigger
from tf2_ros import Buffer, TransformException, TransformListener
from visualization_msgs.msg import Marker, MarkerArray
from .route_store import enabled_waypoints, load_route, normalize_angle, quaternion_from_yaw
DEFAULT_ROUTE_FILE = "/home/limerly/go2_robot_system/config/ros2_patrol_routes/default.yaml"
class PatrolExecutorNode(Node):
def __init__(self) -> None:
super().__init__("go2_patrol_executor")
self.declare_parameter("route_file", os.getenv("GO2_PATROL_ROUTE_FILE", DEFAULT_ROUTE_FILE))
self.declare_parameter("action_name", "/navigate_to_pose")
self.declare_parameter("cmd_vel_topic", "/cmd_vel")
self.declare_parameter("status_topic", "/go2_patrol/status")
self.declare_parameter("current_goal_marker_topic", "/go2_patrol/current_goal_marker")
self.declare_parameter("base_frame", "base_link")
self.declare_parameter("xy_goal_tolerance_m", 0.20)
self.declare_parameter("yaw_goal_tolerance_deg", 10.0)
self.declare_parameter("yaw_align_timeout_s", 20.0)
self.declare_parameter("yaw_align_speed_rad_s", 0.5)
self.declare_parameter("dwell_s", 3.0)
self.declare_parameter("nav_goal_timeout_s", 180.0)
self.declare_parameter("autostart", False)
self.route_file = Path(str(self.get_parameter("route_file").value)).expanduser()
self.action_name = str(self.get_parameter("action_name").value)
self.base_frame = str(self.get_parameter("base_frame").value).strip("/")
self.xy_goal_tolerance_m = float(self.get_parameter("xy_goal_tolerance_m").value)
self.yaw_goal_tolerance = math.radians(float(self.get_parameter("yaw_goal_tolerance_deg").value))
self.yaw_align_timeout_s = float(self.get_parameter("yaw_align_timeout_s").value)
self.yaw_align_speed_rad_s = abs(float(self.get_parameter("yaw_align_speed_rad_s").value))
self.dwell_s = float(self.get_parameter("dwell_s").value)
self.nav_goal_timeout_s = float(self.get_parameter("nav_goal_timeout_s").value)
self.status_pub = self.create_publisher(String, str(self.get_parameter("status_topic").value), 10)
self.cmd_vel_pub = self.create_publisher(Twist, str(self.get_parameter("cmd_vel_topic").value), 10)
self.initialpose_pub = self.create_publisher(PoseWithCovarianceStamped, "/initialpose", 10)
self.reinit_global_loc_client = self.create_client(
Empty, "/reinitialize_global_localization")
self.current_marker_pub = self.create_publisher(
MarkerArray, str(self.get_parameter("current_goal_marker_topic").value), 10
)
self.action_client = ActionClient(self, NavigateToPose, self.action_name)
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
self.create_service(Trigger, "/go2_patrol/start", self._start_service)
self.create_service(Trigger, "/go2_patrol/cancel", self._cancel_service)
self.create_service(Trigger, "/go2_patrol/reload_route", self._reload_service)
self.create_timer(0.5, self._on_timer)
self._running = False
self._cancel_requested = False
self._current_goal_handle = None
self._worker_future: Optional[Future] = None
self._state = "READY"
self._current_index = 0
self._total = 0
self._current_name = ""
self._nav2_status = ""
self._position_error_m: Optional[float] = None
self._yaw_error_deg: Optional[float] = None
self._dwell_remaining_s = 0.0
self._error = ""
self.get_logger().info(
f"patrol executor ready; route_file={self.route_file}, action={self.action_name}, autostart={self.get_parameter('autostart').value}"
)
if bool(self.get_parameter("autostart").value):
self._start_patrol()
def _start_service(self, request: Trigger.Request, response: Trigger.Response) -> Trigger.Response:
del request
ok, message = self._start_patrol()
response.success = ok
response.message = message
return response
def _cancel_service(self, request: Trigger.Request, response: Trigger.Response) -> Trigger.Response:
del request
self._cancel_requested = True
self._cancel_current_goal()
self._publish_zero_for(1.0)
self._state = "CANCELED"
self._publish_status()
response.success = True
response.message = "cancel requested"
return response
def _reload_service(self, request: Trigger.Request, response: Trigger.Response) -> Trigger.Response:
del request
try:
route = load_route(self.route_file)
count = len(enabled_waypoints(route))
except Exception as exc:
response.success = False
response.message = f"failed to reload route: {exc}"
return response
response.success = True
response.message = f"route ok: {count} enabled waypoints"
return response
def _start_patrol(self) -> tuple[bool, str]:
if self._running:
return False, "patrol is already running"
try:
route = load_route(self.route_file)
waypoints = enabled_waypoints(route)
except Exception as exc:
self._state = "FAILED"
self._error = f"failed to load route: {exc}"
self._publish_status()
return False, self._error
if not waypoints:
self._state = "FAILED"
self._error = f"route has no enabled waypoints: {self.route_file}"
self._publish_status()
return False, self._error
if not self.action_client.wait_for_server(timeout_sec=10.0):
self._state = "FAILED"
self._error = f"Nav2 action server not available: {self.action_name}"
self._publish_status()
return False, self._error
self._running = True
self._cancel_requested = False
self._error = ""
self._total = len(waypoints)
thread = threading.Thread(target=self._run_patrol, args=(waypoints,), daemon=True)
thread.start()
return True, f"started patrol: {len(waypoints)} waypoints"
def _run_patrol(self, waypoints) -> None:
try:
for index, waypoint in enumerate(waypoints, start=1):
if self._cancel_requested:
self._state = "CANCELED"
return
self._current_index = index
self._current_name = waypoint.name
self._state = "NAVIGATING"
self._nav2_status = "SENDING_GOAL"
self._position_error_m = None
self._yaw_error_deg = None
self._dwell_remaining_s = 0.0
self._error = ""
self._publish_current_goal_marker(waypoint)
self._publish_status()
ok, nav_status = self._send_nav_goal_and_wait(waypoint)
self._nav2_status = nav_status
self._publish_zero_for(0.5)
if not ok:
self._state = "FAILED"
self._error = f"Nav2 failed at {waypoint.name}: {nav_status}"
self._publish_status()
return
self._state = "ALIGNING_YAW"
self._publish_status()
if not self._align_yaw(waypoint):
self._state = "YAW_TIMEOUT"
self._error = f"yaw alignment timed out at {waypoint.name}"
self._publish_zero_for(1.0)
self._publish_status()
return
self._state = "DWELLING"
dwell_end = time.monotonic() + self.dwell_s
while time.monotonic() < dwell_end:
if self._cancel_requested:
self._state = "CANCELED"
self._publish_zero_for(1.0)
self._publish_status()
return
self._dwell_remaining_s = max(0.0, dwell_end - time.monotonic())
self._publish_zero()
self._publish_status()
time.sleep(0.2)
self._dwell_remaining_s = 0.0
self._state = "COMPLETED"
self._publish_zero_for(1.0)
self._publish_status()
except Exception as exc:
self._state = "FAILED"
self._error = str(exc)
self._publish_zero_for(1.0)
self._publish_status()
finally:
self._running = False
self._current_goal_handle = None
def _send_nav_goal_and_wait(self, waypoint) -> tuple[bool, str]:
goal = NavigateToPose.Goal()
goal.pose = PoseStamped()
goal.pose.header.frame_id = waypoint.frame_id
goal.pose.header.stamp = self.get_clock().now().to_msg()
goal.pose.pose.position.x = waypoint.x
goal.pose.pose.position.y = waypoint.y
qx, qy, qz, qw = quaternion_from_yaw(waypoint.yaw)
goal.pose.pose.orientation.x = qx
goal.pose.pose.orientation.y = qy
goal.pose.pose.orientation.z = qz
goal.pose.pose.orientation.w = qw
send_future = self.action_client.send_goal_async(goal)
if not self._wait_future(send_future, 15.0):
return False, "SEND_GOAL_TIMEOUT"
goal_handle = send_future.result()
if not goal_handle or not goal_handle.accepted:
return False, "GOAL_REJECTED"
self._current_goal_handle = goal_handle
self._nav2_status = "GOAL_ACCEPTED"
self._publish_status()
result_future = goal_handle.get_result_async()
if not self._wait_future(result_future, self.nav_goal_timeout_s):
self._cancel_current_goal()
return False, "GOAL_TIMEOUT"
result = result_future.result()
if result.status == GoalStatus.STATUS_SUCCEEDED:
return True, "SUCCEEDED"
return False, f"STATUS_{result.status}"
def _wait_future(self, future, timeout_s: float) -> bool:
deadline = time.monotonic() + timeout_s
while rclpy.ok() and time.monotonic() < deadline:
if self._cancel_requested:
return False
if future.done():
return True
time.sleep(0.05)
return bool(future.done())
def _reinitialize_amcl(self, waypoint) -> None:
"""Force AMCL scan update using current robot pose while stationary."""
# Wait for quadruped gait to fully settle (stationary laser = clean data)
time.sleep(2.0)
pose = self._lookup_robot_pose(waypoint.frame_id)
if pose is None:
self.get_logger().warn("Cannot lookup robot pose for AMCL refresh, skipping")
return
x, y, yaw = pose
msg = PoseWithCovarianceStamped()
msg.header.frame_id = waypoint.frame_id
msg.header.stamp = self.get_clock().now().to_msg()
msg.pose.pose.position.x = x
msg.pose.pose.position.y = y
msg.pose.pose.position.z = 0.0
qx, qy, qz, qw = quaternion_from_yaw(yaw)
msg.pose.pose.orientation.x = qx
msg.pose.pose.orientation.y = qy
msg.pose.pose.orientation.z = qz
msg.pose.pose.orientation.w = qw
msg.pose.covariance[0] = 0.05
msg.pose.covariance[7] = 0.05
msg.pose.covariance[35] = 0.02
self.initialpose_pub.publish(msg)
# Wait for AMCL to process next laser scan with stationary data
time.sleep(1.5)
# Check new pose after AMCL update
new_pose = self._lookup_robot_pose(waypoint.frame_id)
if new_pose:
nx, ny, nyaw = new_pose
err = math.hypot(waypoint.x - nx, waypoint.y - ny)
self.get_logger().info(
f"AMCL refresh at {waypoint.name}: "
f"pos=({nx:.2f},{ny:.2f}) err={err:.3f}m")
else:
self.get_logger().info(
f"AMCL refresh published at {waypoint.name}")
def _align_yaw(self, waypoint) -> bool:
deadline = time.monotonic() + self.yaw_align_timeout_s
while rclpy.ok() and time.monotonic() < deadline:
if self._cancel_requested:
return False
pose = self._lookup_robot_pose(waypoint.frame_id)
if pose is None:
self._publish_zero()
time.sleep(0.1)
continue
x, y, yaw = pose
self._position_error_m = math.hypot(waypoint.x - x, waypoint.y - y)
yaw_error = normalize_angle(waypoint.yaw - yaw)
self._yaw_error_deg = math.degrees(abs(yaw_error))
self._publish_status()
if abs(yaw_error) <= self.yaw_goal_tolerance:
self._publish_zero_for(0.5)
# Wait for AMCL to update pose estimate after rotation
self.get_logger().info(f"Yaw aligned at {waypoint.name}, waiting for AMCL to stabilize...")
time.sleep(1.0) # Give AMCL time to update with new scan data
return True
twist = Twist()
twist.angular.z = self.yaw_align_speed_rad_s if yaw_error > 0.0 else -self.yaw_align_speed_rad_s
self.cmd_vel_pub.publish(twist)
self.get_logger().info(f'[align_yaw] err={math.degrees(yaw_error):.1f}deg cmd_z={twist.angular.z:.2f}')
time.sleep(0.1)
return False
def _lookup_robot_pose(self, frame_id: str) -> Optional[tuple[float, float, float]]:
try:
transform = self.tf_buffer.lookup_transform(
frame_id.strip("/"), self.base_frame, Time(), timeout=Duration(seconds=0.2)
)
except TransformException:
return None
t = transform.transform.translation
q = transform.transform.rotation
siny_cosp = 2.0 * (q.w * q.z + q.x * q.y)
cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
yaw = math.atan2(siny_cosp, cosy_cosp)
return (float(t.x), float(t.y), yaw)
def _cancel_current_goal(self) -> None:
if self._current_goal_handle is None:
return
try:
self._current_goal_handle.cancel_goal_async()
except Exception as exc:
self.get_logger().warn(f"failed to cancel current Nav2 goal: {exc}")
def _publish_zero(self) -> None:
self.cmd_vel_pub.publish(Twist())
def _publish_zero_for(self, seconds: float) -> None:
end = time.monotonic() + seconds
while time.monotonic() < end:
self._publish_zero()
time.sleep(0.1)
def _publish_status(self) -> None:
payload = {
"source": "patrol_executor",
"state": self._state,
"route_file": str(self.route_file),
"current_index": self._current_index,
"total": self._total,
"current_name": self._current_name,
"nav2_status": self._nav2_status,
"position_error_m": self._position_error_m,
"yaw_error_deg": self._yaw_error_deg,
"dwell_remaining_s": round(self._dwell_remaining_s, 2),
"error": self._error,
}
self.status_pub.publish(String(data=json.dumps(payload, ensure_ascii=False)))
def _publish_current_goal_marker(self, waypoint) -> None:
markers = MarkerArray()
marker = Marker()
marker.header.frame_id = waypoint.frame_id
marker.header.stamp = self.get_clock().now().to_msg()
marker.ns = "go2_patrol_current_goal"
marker.id = 1
marker.type = Marker.SPHERE
marker.action = Marker.ADD
marker.pose.position.x = waypoint.x
marker.pose.position.y = waypoint.y
marker.pose.position.z = 0.18
marker.scale.x = 0.35
marker.scale.y = 0.35
marker.scale.z = 0.35
marker.color.r = 0.2
marker.color.g = 1.0
marker.color.b = 0.3
marker.color.a = 0.9
markers.markers.append(marker)
self.current_marker_pub.publish(markers)
def _on_timer(self) -> None:
self._publish_status()
def main(args: list[str] | None = None) -> None:
rclpy.init(args=args)
node = PatrolExecutorNode()
executor = MultiThreadedExecutor(num_threads=4)
executor.add_node(node)
try:
executor.spin()
except (KeyboardInterrupt, ExternalShutdownException):
pass
finally:
executor.shutdown()
node.destroy_node()
if rclpy.ok():
rclpy.shutdown()
if __name__ == "__main__":
main()