KiSpace

【Gazebo 学习】260516

260516

嗯...还是有问题，我们启动后还是会发生崩溃等一系列的问题。

所以我想了想，还是得祭出我们新版的工程式学习的 Skill：

交给 Codex 去完成我们 02 的新版教程吧。

在一顿操作后，我们算是弄好了 02 的 roadmap 文档以及其他供我们进行阅读的文档。

我们新开一个 02 的分支来放这部分代码好了，实际主分支的代码由我们自行实现。

按照 roadmap，我们先检查一下自己的依赖是否完备：

#!/usr/bin/env bash
set -euo pipefail

missing=0

echo "[check] ROS_DISTRO=${ROS_DISTRO:-unset}"
if [[ "${ROS_DISTRO:-}" != "jazzy" ]]; then
  echo "[error] 请先 source ROS2 Jazzy 环境，例如：source .vscode/project-terminal-init.sh"
  missing=1
fi

if ! command -v ros2 >/dev/null 2>&1; then
  echo "[error] ros2 命令不可用。"
  missing=1
fi

required_packages=(
  nav2_bringup
  nav2_controller
  nav2_planner
  nav2_bt_navigator
  nav2_msgs
  slam_toolbox
  ros_gz_bridge
)

for package_name in "${required_packages[@]}"; do
  if ros2 pkg prefix "${package_name}" >/dev/null 2>&1; then
    echo "[ok] ros2 package ${package_name}"
  else
    echo "[error] 缺少 ROS2 package: ${package_name}"
    missing=1
  fi
done

fastcdr_lib="/opt/ros/jazzy/lib/libfastcdr.so.2"
if [[ -f "${fastcdr_lib}" ]]; then
  fastcdr_symbols="$(nm -D "${fastcdr_lib}" 2>/dev/null | c++filt)"
  if [[ "${fastcdr_symbols}" == *"eprosima::fastcdr::Cdr::serialize(unsigned int)"* ]]; then
    echo "[ok] ${fastcdr_lib} exports Cdr::serialize(unsigned int)"
  else
    echo "[error] ${fastcdr_lib} 缺少 Cdr::serialize(unsigned int)，Nav2 controller_server 会在运行时崩溃。"
    missing=1
  fi
else
  echo "[error] 未找到 ${fastcdr_lib}"
  missing=1
fi

if [[ "${missing}" -ne 0 ]]; then
  cat <<'EOF'

请更新 Nav2/Jazzy 运行依赖：

sudo apt-get update
sudo apt-get install -y \
  ros-jazzy-fastcdr \
  ros-jazzy-fastrtps \
  ros-jazzy-rmw-fastrtps-cpp \
  ros-jazzy-rmw-fastrtps-shared-cpp \
  ros-jazzy-rosidl-typesupport-fastrtps-c \
  ros-jazzy-rosidl-typesupport-fastrtps-cpp \
  ros-jazzy-rmw-cyclonedds-cpp

然后重新 source 环境并再次运行本检查。
EOF
  exit 1
fi

echo "[ok] Nav2 runtime dependencies look usable."


(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotTwo$ bash src/kibot_one_sim/scripts/check_nav2_runtime_deps.sh
[check] ROS_DISTRO=jazzy
[ok] ros2 package nav2_bringup
[ok] ros2 package nav2_controller
[ok] ros2 package nav2_planner
[ok] ros2 package nav2_bt_navigator
[ok] ros2 package nav2_msgs
[ok] ros2 package slam_toolbox
[ok] ros2 package ros_gz_bridge
[ok] /opt/ros/jazzy/lib/libfastcdr.so.2 exports Cdr::serialize(unsigned int)
[ok] Nav2 runtime dependencies look usable.

运行环境正常。

然后我们验收一下结果，我们启动一下仿真、SLAM 和 Nav2：

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

检查状态是否符合预期：

(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotTwo$ ros2 topic info /clock # 预期结果：Publisher count: 1
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]
ros2 lifecycle get /planner_server
ros2 lifecycle get /bt_navigator
ros2 lifecycle get /waypoint_follower
active [3]
active [3]
active [3]
active [3]
(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotTwo$ ros2 action list | grep navigate_to_pose # 预期结果：/navigate_to_pose
/navigate_to_pose
(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotTwo$ ros2 topic info /cmd_vel_smoothed -v | grep -A2 'Node name: kibot_one_bridge' # 预期结果：能看到 `kibot_one_bridge`
Node name: kibot_one_bridge
Node namespace: /
Topic type: geometry_msgs/msg/Twist

可以看到全部符合预期。

简单看一下我们的 map -> base_link 的 TF 坐标：

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

可以看到几乎为 0，也是符合我们预期的。

接下来我们发一个短距离的目标看看：

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

Result:
    error_code: 0
error_msg: ''

Goal finished with status: SUCCEEDED

我们要求小车的目标位置为 x: 0.5，方向为正向，该 nav2 的 /navigate_to_pose 这个 action 成功执行了，我们看看现在的 map -> base_link：

(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotTwo$ ros2 run tf2_ros tf2_echo map base_link
[INFO] [1778928985.674640708] [tf2_echo]: Waiting for transform map ->  base_link: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist
[INFO] [1778928987.655299722] [tf2_echo]: Waiting for transform map ->  base_link: Could not find a connection between 'map' and 'base_link' because they are not part of the same tree.Tf has two or more unconnected trees.
At time 113.80000000
- Translation: [0.411, 0.009, 0.000]
- Rotation: in Quaternion (xyzw) [0.000, 0.000, -0.002, 1.000]
- Rotation: in RPY (radian) [0.000, 0.000, -0.004]
- Rotation: in RPY (degree) [0.000, 0.000, -0.216]
- Matrix:
  1.000  0.004  0.000  0.411
 -0.004  1.000 -0.000  0.009
 -0.000  0.000  1.000  0.000
  0.000  0.000  0.000  1.000

可以看到 x 接近 0.5 了，虽然有点误差，但是在可接受的范围内。

我们发个 x: -5 的看看：

(.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: -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: -5.0
      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: 7e4c1f86ce874d2b8140db3f7ba55590

Result:
    error_code: 0
error_msg: ''

Goal finished with status: SUCCEEDED

然后再看看我们的 map -> base_link 的 TF：

At time 262.420000000
- Translation: [-5.071, 0.012, 0.000]
- Rotation: in Quaternion (xyzw) [0.000, 0.000, 0.138, 0.990]
- Rotation: in RPY (radian) [0.000, -0.000, 0.276]
- Rotation: in RPY (degree) [0.000, -0.000, 15.830]
- Matrix:
  0.962 -0.273  0.000 -5.071
  0.273  0.962  0.000  0.012
  0.000  0.000  1.000  0.000
  0.000  0.000  0.000  1.000

时间比较长，但是依旧是达到了我们的目标。

总的来说，我们目前接入的 nav2 是符合我们的需求的。

接下来就可以切换到 main 分支并合并文档来进行实际的实现了。

site logo

Here is KISPACE

© 2026 KiSpace