260528

上次我们完成了 Nav2 的参数文件，我们接下来完成 Nav2 的启动入口文件，基本上内容和之前的没有什么区别，这里就不放出来了。

我们构建后，启动看看：

ros2 launch kibot_one_sim nav2.launch.py use_rviz:=false

嗯...似乎出现了报错，我这边看了一下，是因为我之前在写 nav2_behaviors::Backup 的时候，写成了 nav2_behaviors::BackUp，以及 FootprintApproach 下的 type 应当是 polygon 而非 pylygons 以及多处 src/kibot_one_sim/launch/nav2.launch.py 中的变量名填写错误。

重新构建并成功运行后，我们测试一下看看：

(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotTwo$ ros2 topic info /clock
Type: rosgraph_msgs/msg/Clock
Publisher count: 1
Subscription count: 18

时间的发布者符合预期。

然后是控制服务器、规划服务器和导航器的状态：

(.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]

都是激活的，符合我们预期。

(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotTwo$ ros2 action list | grep navigate_to_pose
/navigate_to_pose

导航到指定 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

整个信息也是没问题的。

然后我们发布一个近距离目标看看：

(.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: ff333a1e4356472199583a80ca9e8296

Result:
    error_code: 0
error_msg: ''

Goal finished with status: SUCCEEDED

(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotTwo$ ros2 run tf2_ros tf2_echo map base_link
[INFO] [1779966016.597133456] [tf2_echo]: Waiting for transform map ->  base_link: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist
At time 172.160000000
- Translation: [0.410, -0.019, 0.000]
- Rotation: in Quaternion (xyzw) [0.000, 0.000, -0.002, 1.000]
- Rotation: in RPY (radian) [0.000, 0.000, -0.005]
- Rotation: in RPY (degree) [0.000, 0.000, -0.279]
- Matrix:
  1.000  0.005  0.000  0.410
 -0.005  1.000 -0.000 -0.019
 -0.000  0.000  1.000  0.000
  0.000  0.000  0.000  1.000
At time 173.140000000
- Translation: [0.410, -0.019, 0.000]
- Rotation: in Quaternion (xyzw) [0.000, 0.000, -0.002, 1.000]
- Rotation: in RPY (radian) [0.000, 0.000, -0.005]
- Rotation: in RPY (degree) [0.000, 0.000, -0.279]
- Matrix:
  1.000  0.005  0.000  0.410
 -0.005  1.000 -0.000 -0.019
 -0.000  0.000  1.000  0.000
  0.000  0.000  0.000  1.000

根据 TF，发现我们的小车成功移动到了目标点附近，误差也在可接受范围内。

然后我们发一个稍微大一点的目标：

(.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: -3, y: 
1.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: -3.0
      y: 1.0
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
behavior_tree: ''

Goal accepted with ID: 848957169fb944b6a2a3fc9aec3dff58

Result:
    error_code: 0
error_msg: ''

Goal finished with status: SUCCEEDED
(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotTwo$ ros2 run tf2_ros tf2_echo map base_link
[INFO] [1779966128.153129614] [tf2_echo]: Waiting for transform map ->  base_link: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist
At time 283.380000000
- Translation: [-3.091, 0.837, 0.000]
- Rotation: in Quaternion (xyzw) [0.000, 0.000, 0.242, 0.970]
- Rotation: in RPY (radian) [0.000, -0.000, 0.488]
- Rotation: in RPY (degree) [0.000, -0.000, 27.960]
- Matrix:
  0.883 -0.469  0.000 -3.091
  0.469  0.883  0.000  0.837
  0.000  0.000  1.000  0.000
  0.000  0.000  0.000  1.000

可以看到我们的小车也依旧到达了我们预期的位置。

综上，我们目前的小车成功接入了 Nav2。