260527

继我们上次让 AI 重写了一下教程，补全了我们之前很多没有的地方。

就在新版本的教程上重新编写一下代码吧。

首先还是 01，我们将我们 Gazebo 的速度入口改为 Nav2 的平滑速度。

- ros_topic_name: "/cmd_vel_smoothed"
  gz_topic_name: "/model/kibot_one_base/cmd_vel"
  ros_type_name: "geometry_msgs/msg/Twist"
  gz_type_name: "gz.msgs.Twist"
  direction: ROS_TO_GZ

然后新建 src/kibot_one_sim/launch/nav2.launch.py：

from launch import LaunchDescription

def generate_launch_description() -> LaunchDescription:
    return LaunchDescription([])

目前就先这么写就可以了，作为占位符。

我们和之前不同的重头戏在于 src/kibot_one_sim/config/nav2_params.yaml。

我们的bt_navigator、controller_server、local_costmap、global_costmap、planner_server、velocity_smoother 这几个和之前都是一样的。

不同的点在下面。

首先是，我们新增一个 behavior_server：

behavior_server:
  ros__parameters:
    local_costmap_topic: local_costmap/costmap_raw
    global_costmap_topic: global_costmap/costmap_raw
    local_footprint_topic: local_costmap/published_footprint
    global_footprint_topic: global_costmap/published_footprint
    cycle_frequency: 10.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"
    assisted_teleop:
      plugin: "nav2_behaviors::AssistedTeleop"
    wait:
      plugin: "nav2_behaviors::Wait"
    local_frame: odom
    global_frame: map
    robot_base_frame: base_link
    transform_tolerance: 0.1
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

然后增加一个 waypoint_follower：

waypoint_follower:
  ros__parameters:
    loop_rate: 20
    stop_on_failure: false
    action_server_result_timeout: 900.0
    waypoint_task_executor_plugin: "wait_at_waypoint"
    wait_at_waypoint:
      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
      enabled: True
      waypoint_pause_duration: 200

router_server:

route_server:
  ros__parameters:
    boundary_radius_to_achieve_node: 1.0
    radius_to_achieve_node: 2.0
    smooth_corner: true
    operations: ["AdjustSpeedLimit", "ReroutingService", "CollisionMonitor"]
    AdjustSpeedLimit:
      plugin: "nav2_route::AdjustSpeedLimit"
    ReroutingService:
      plugin: "nav2_route::ReroutingService"
    CollisionMonitor:
      plugin: "nav2_route::CollisionMonitor"
      max_collision_dist: 3.0
    edge_cost_functions: ["DistanceScorer", "CostmapScorer"]
    DistanceScorer:
      plugin: "nav2_route::DistanceScorer"
    CostmapScorer:
      plugin: "nav2_route::CostmapScorer"

再补一个 docking_server：

docking_server:
  ros__parameters:
    controller_frequency: 50.0
    initial_perception_timeout: 5.0
    wait_charge_timeout: 5.0
    dock_approach_timeout: 30.0
    undock_angular_tolerance: 0.1
    max_retries: 3
    base_frame: base_link
    fixed_frame: "odom"
    dock_backwards: false
    dock_prestaging_tolerance: 0.5
    dock_plugins: ["simple_charge_dock"]
    simple_charging_dock:
      plugin: 'opennav_docking::SimpleChargingDock'
      docking_threshold: 0.05
      staging_x_offset: -0.7
      use_external_detection_pose: true
      use_battery_status: false
      use_stall_detection: false
      external_detection_timeout: 1.0
      external_detection_translation_x: -0.18
      external_detection_translation_y: 0.0
      external_detection_rotation_roll: -1.57
      external_detection_rotation_pitch: -1.57
      external_detection_rotation_yaw: 0.0
      filter_coef: 0.1
    controller:
      k_phi: 3.0
      k_delta: 2.0
      v_linear_min: 0.15
      v_linear_max: 0.15
      use_collision_detection: true
      costmap_topic: "local_costmap/costmap_raw"
      footprint_topic: "local_costmap/published_footprint"
      transform_tolerance: 0.1
      projection_time: 5.0
      simulation_step: 0.1
      dock_collision_threshold: 0.3

这里的 docking_server 主要是让我们的 Nav2 可以正常运行，不至于因漏掉配置导致无法正常启动。

最后补一个collision_monitor来进行碰撞检测：

collision_monitor:
  ros__parameters:
    base_frame_id: "base_link"
    odom_frame_id: "odom"
    cmd_vel_in_topic: "cmd_vel_smoothed"
    cmd_vel_out_topic: "cmd_vel"
    polygons: ["FootprintApproach"]
    FootprintApproach:
      type: "polygons"
      action_type: "approach"
      footprint_topic: "/local_costmap/published_footprint"
      time_before_collision: 1.2
    observation_sources: ["scan"]
    scan:
      type: "scan"
      topic: "scan"
      min_height: 0.15
      max_height: 2.0
      enabled: True