260525
上面我们引入了 Nav2 的具体配置项和入口启动文件。
接下来我们就要验证是否可用了。
先启动一下完整环境：
ros2 launch kibot_one_sim nav2.launch.py use_rviz:=false
我们这里先不启用 RViz。
启动后，我们看看我们的时间发布者：
(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotTwo$ ros2 topic info /clock
Type: rosgraph_msgs/msg/Clock
Publisher count: 1
Subscription count: 18
可以看到只有一个发布者，这说明我们的时间是可信的。
然后检查一下我们的 Nav2 的 lifecycle。
先检查一下 controller 的：
(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotTwo$ ros2 lifecycle get /controller_server
inactive [2]
没有按照预期的工作，我们继续看看后续的这些 lifecycle：
(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotTwo$ ros2 lifecycle get /planner_server
inactive [2]
(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotTwo$ ros2 lifecycle get /bt_navigator
inactive [2]
会发现都是没有激活的。
我们看看是怎么回事。
嗯...看了一下日志：
Configuring controller_server
Configuring smoother_server
Configuring planner_server
Configuring route_server
Configuring behavior_server
Configuring velocity_smoother
Configuring collision_monitor
Configuring bt_navigator
Configuring waypoint_follower
Configuring docking_server
Failed to change state for node: docking_server
Failed to bring up all requested nodes. Aborting bringup.
是 docking_server 的问题，这个东西主要是让机器人导航到充电桩里面进行充电这样的，在我们这里用不上。
而生命周期循环的逻辑是，将所有管理的节点都配置好后，才统一进行激活。
我们的 docking_server 由于没有对应的配置内容，所以就连着其他节点也没有激活。
我们最小的改动就是，在我们的 src/kibot_one_sim/config/nav2_params.yaml 中增加这部分配置，保证我们的系统可以正常跑起来就是了，不需要真的有一个充电桩之类的东西。
docking_server:
ros__parameters:
controller_frequency: 50.0
initial_perception_timeout: 5.0
wait_charge_timeout: 5.0
dock_approach_timeout: 30.0
undock_linear_tolerance: 0.05
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_charging_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
实际上我们的实现分支就是这么干的，但是却没有写在教学文档里...
确实是有必要按照这个文档中的内容来一步一步实现的时候进行检查，这个应当作为第二步。
重新构建后运行，现在这些节点就都正常激活了：
(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotTwo$ ros2 lifecycle get /controller_server
active [3]
(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotTwo$ ros2 lifecycle get /planner_server
active [3]
(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotTwo$ ros2 lifecycle get /bt_navigator
active [3]
然后看看有没有我们所预期的 action：
(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotTwo$ ros2 action list | grep navigate_to_pose
/navigate_to_pose
在看看这个动作的信息：
(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotTwo$ ros2 action info /navigate_to_pose
Action: /navigate_to_pose
Action clients: 3
/bt_navigator
/waypoint_follower
/docking_server
Action servers: 1
/bt_navigator
可以看到都已经有了。
接下来看看我们的速度链路，Gazebo 到底有没有订阅我们预期的速度：
(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotTwo$ ros2 topic info /cmd_vel_smoothed -v
Type: geometry_msgs/msg/Twist
Publisher count: 1
Node name: velocity_smoother
Node namespace: /
Topic type: geometry_msgs/msg/Twist
Topic type hash: RIHS01_9c45bf16fe0983d80e3cfe750d6835843d265a9a6c46bd2e609fcddde6fb8d2a
Endpoint type: PUBLISHER
GID: 01.0f.ed.30.99.1e.7e.60.00.00.00.00.00.00.20.03
QoS profile:
Reliability: RELIABLE
History (Depth): UNKNOWN
Durability: VOLATILE
Lifespan: Infinite
Deadline: Infinite
Liveliness: AUTOMATIC
Liveliness lease duration: Infinite
Subscription count: 2
Node name: kibot_one_bridge
Node namespace: /
Topic type: geometry_msgs/msg/Twist
Topic type hash: RIHS01_9c45bf16fe0983d80e3cfe750d6835843d265a9a6c46bd2e609fcddde6fb8d2a
Endpoint type: SUBSCRIPTION
GID: 01.0f.ed.30.8f.1e.54.a8.00.00.00.00.00.00.14.04
QoS profile:
Reliability: RELIABLE
History (Depth): UNKNOWN
Durability: VOLATILE
Lifespan: Infinite
Deadline: Infinite
Liveliness: AUTOMATIC
Liveliness lease duration: Infinite
Node name: collision_monitor
Node namespace: /
Topic type: geometry_msgs/msg/Twist
Topic type hash: RIHS01_9c45bf16fe0983d80e3cfe750d6835843d265a9a6c46bd2e609fcddde6fb8d2a
Endpoint type: SUBSCRIPTION
GID: 01.0f.ed.30.9a.1e.ec.89.00.00.00.00.00.00.24.04
QoS profile:
Reliability: RELIABLE
History (Depth): UNKNOWN
Durability: VOLATILE
Lifespan: Infinite
Deadline: Infinite
Liveliness: AUTOMATIC
Liveliness lease duration: Infinite
可以看到 bridge 是订阅了我们的平滑后速度的这个话题的，因此也是符合预期的。
最后我们发送一个最小目标，先发一个近距离目标看看：
ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose \
"{pose: {header: {frame_id: map}, pose: {position: {x: 0.5, y: 0.0, z: 0.0}, orientation: {w: 1.0}}}}"
这里表示移动到 (0.5, 0) 这一 map 坐标。
可以看到正常执行：
(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotTwo$ ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose \
"{pose: {header: {frame_id: map}, pose: {position: {x: 0.5, y: 0.0, z: 0.0}, orientation: {w: 1.0}}}}"
Waiting for an action server to become available...
Sending goal:
pose:
header:
stamp:
sec: 0
nanosec: 0
frame_id: map
pose:
position:
x: 0.5
y: 0.0
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
behavior_tree: ''
Goal accepted with ID: 7fefe3bb7e754a02bb7cfd0978e28eac
Result:
error_code: 0
error_msg: ''
Goal finished with status: SUCCEEDED
看看我们的 odom：
(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotTwo$ ros2 topic echo /odom --once
header:
stamp:
sec: 78
nanosec: 920000000
frame_id: odom
child_frame_id: base_link
pose:
pose:
position:
x: 0.26082301608214026
y: -0.027240628486577406
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.10541815193390462
w: 0.9944279829343301
covariance:
可以看到确实动了。
然后是我们的 TF：
(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotTwo$ ros2 run tf2_ros tf2_echo map base_link
[INFO] [1779690245.758324491] [tf2_echo]: Waiting for transform map -> base_link: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist
At time 39.720000000
- Translation: [0.261, 0.022, 0.000]
- Rotation: in Quaternion (xyzw) [0.000, 0.000, -0.100, 0.995]
- Rotation: in RPY (radian) [0.000, 0.000, -0.201]
- Rotation: in RPY (degree) [0.000, 0.000, -11.503]
- Matrix:
0.980 0.199 0.000 0.261
-0.199 0.980 -0.000 0.022
-0.000 0.000 1.000 0.000
0.000 0.000 0.000 1.000
嗯，可以看到不符合我们的预期。
通过检查，我们发现是我们的 goal_checker 的插件键写错了：
goal_checker_plugin: ["general_goal_checker"]
这里应该是 goal_checker_plugins，少了个 s。
现在我们再测试一下看看，似乎还是有问题，会一直转圈，不通向目标地点，很奇怪。
嗯，经过我们的检查，似乎是我们的MPPI controller 配置太空了。
[bt_navigator]: Begin navigating from current location (-0.00, 0.00) to (5.00, 0.00)
这说明我们的 5m 的目标确实是发进去了，但是 controller 现在运行的参数是：
(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotTwo$ ros2 param get /controller_server FollowPath.critics
String values are: []
因为MPPI 是采样很多候选轨迹，然后用 critics 给这些轨迹打分，我们当前的配置里是这样的：
FollowPath:
plugin: "nav2_mppi_controller::MPPIController"
motion_model: "DiffDrive"
vx_max: 0.45
vx_min: -0.20
wz_max: 1.2
ax_max: 0.8
ax_min: -0.8
az_max: 1.5
只有速度约束，而没有告诉 MPPI 要贴着 path 走，要朝着 goal 靠近，要对齐 goal 的朝向。
所以就它能启动，也能收到路径，但是局部的控制行为会很奇怪。
因此需要将我们的 FollowPath 替换为完整版本：
FollowPath:
plugin: "nav2_mppi_controller::MPPIController"
time_steps: 56
model_dt: 0.05
batch_size: 2000
ax_max: 0.8
ax_min: -0.8
ay_max: 3.0
ay_min: -3.0
az_max: 1.5
vx_std: 0.2
vy_std: 0.2
wz_std: 0.4
vx_max: 0.45
vx_min: -0.20
vy_max: 0.5
wz_max: 1.2
iteration_count: 1
prune_distance: 1.7
transform_tolerance: 0.1
temperature: 0.3
gamma: 0.015
motion_model: "DiffDrive"
visualize: true
regenerate_noises: true
TrajectoryVisualizer:
trajectory_step: 5
time_step: 3
AckermannConstraints:
min_turning_r: 0.2
critics: [
"ConstraintCritic", "CostCritic", "GoalCritic",
"GoalAngleCritic", "PathAlignCritic", "PathFollowCritic",
"PathAngleCritic", "PreferForwardCritic"
]
ConstraintCritic:
enabled: true
cost_power: 1
cost_weight: 4.0
GoalCritic:
enabled: true
cost_power: 1
cost_weight: 5.0
threshold_to_consider: 1.4
GoalAngleCritic:
enabled: true
cost_power: 1
cost_weight: 3.0
threshold_to_consider: 0.5
PreferForwardCritic:
enabled: true
cost_power: 1
cost_weight: 5.0
threshold_to_consider: 0.5
CostCritic:
enabled: true
cost_power: 1
cost_weight: 3.81
near_collision_cost: 253
critical_cost: 300.0
consider_footprint: false
collision_cost: 1000000.0
near_goal_distance: 1.0
trajectory_point_step: 2
PathAlignCritic:
enabled: true
cost_power: 1
cost_weight: 14.0
max_path_occupancy_ratio: 0.05
trajectory_point_step: 4
threshold_to_consider: 0.5
offset_from_furthest: 20
use_path_orientations: false
PathFollowCritic:
enabled: true
cost_power: 1
cost_weight: 5.0
offset_from_furthest: 5
threshold_to_consider: 1.4
PathAngleCritic:
enabled: true
cost_power: 1
cost_weight: 2.0
offset_from_furthest: 4
threshold_to_consider: 0.5
max_angle_to_furthest: 1.0
mode: 0
现在启动后，就可以看到我们的参数符合预期了：
(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotTwo$ ros2 param get /controller_server FollowPath.critics
String values are: ['ConstraintCritic', 'CostCritic', 'GoalCritic', 'GoalAngleCritic', 'PathAlignCritic', 'PathFollowCritic', 'PathAngleCritic', 'PreferForwardCritic']
不过似乎在实际运行的时候，哪怕是冒烟运行都会有问题。
嗯， 我觉得我们当前的教程有问题，需要进行重写。
让 AI 重写一份好了，不过我们需要让 AI 先进行检查一份。
