uuv_simulator是一个基于ROS1的属下机器人仿真包，具体详见其github主页，它可以安装在kinetic/lunar /melodic/noetic版本的ROS中。其中noetic版为其他开发者移植。作者使用的WSL2运行下的noetic版本，文中所列问题可能不具备普遍性。官方Wiki是最好的学习资料，在搭建仿真平台的过程中，也可参考Dave。

一、运行key_board_velocity.launch时报错

报错内容如下：

[rexrov/keyboard_uuv_velocity_teleop-4] process has died [pid 61583, exit code 255, cmd /home/lak/catkin_ws/src/uuv_simulator/uuv_teleop/scripts/vehicle_keyboard_teleop.py output:=/rexrov/cmd_vel __name:=keyboard_uuv_velocity_teleop __log:=/home/lak/.ros/log/3087991e-2dcb-11eb-82db-000c29a26e1a/rexrov-keyboard_uuv_velocity_teleop-4.log].
log file: /home/lak/.ros/log/3087991e-2dcb-11eb-82db-000c29a26e1a/rexrov-keyboard_uuv_velocity_teleop-4*.log

该问题在issue中有记录，简单来说就是vehicle_keyboard_teleop.py文件没有找到.
解决办法：在/uuv_teleop/CMakeList.txt中的catkin_install_python一栏中，添加一行代码：scripts/vehicle_keyboard_teleop.py，然后再进行编译即可。

二、PositionControl.py解析

  1. 没有节点发布cmd_pose话题，所以cmd_pose_callback从未触发过（原因未知）。

  2. 当里程计信息更新时，触发odometry_callback回调函数，该函数的结构为：(ChatGPT所得)

    这段代码是一个回调函数，用于处理来自底盘系统的测量速度更新。首先，从消息中提取出位置和姿态信息，存储在变量 p 和 q 中。然后，将位置和姿态信息转换为 numpy 数组形式。如果系统尚未初始化（initialized = False），将当前的位置和姿态作为目标位置和目标姿态，并将 initialized 设置为 True，以便下次回调时不再执行此部分代码。
    接下来，计算时间戳 t。然后，通过将目标位置与当前位置之差变换到机体坐标系下，得到在机体坐标系下的位置误差 e_pos_body。利用四元数进行姿态差计算，得到在机体坐标系下的姿态误差 e_rot_quat。如果位置误差的欧氏范数（仅考虑前两个维度）大于5.0，表示当前距离目标较远，此时将目标航向修正为朝向目标位置，并计算修正后的姿态误差。
    接下来，利用位置误差和时间戳调用 pid_pos 的 regulate 方法计算线速度控制输出v_linear，再利用姿态误差和时间戳调用 pid_rot 的 regulate 方法计算角速度控制输出 v_angular。
    最后，将计算得到的线速度和角速度转换为 Twist 消息格式，发布给底盘系统。

  3. 得到e_pos_body的过程：

  • trans.quaternion_matrix(q)方法可以将四元数转换为变换矩阵T，再取T矩阵三阶子式即为旋转矩阵R。
  • e_pos_world为世界坐标系下机器人的位置误差，使它左乘旋转矩阵R，即R.dot(e_pos_world)，即可得到机体坐标系下的位置误差。
  • 整理上述步骤代码，即可得到：
     e_pos_body = trans.quaternion_matrix(q).transpose()[0:3,0:3].dot(e_pos_world)
  1. 得到e_rot_quat的过程：
  • 使用trans.quaternion_conjugate(q)方法得到四元数q的共轭q'。
  • 将q'与目标姿态quat_des相乘得到机体姿态四元数的误差。（不是很懂四元数）
  • 整理上述步骤代码，即可得到：
    e_rot_quat = trans.quaternion_multiply(trans.quaternion_conjugate(q), quat_des)
    1. 若在世界坐标系下的xy平面中，机体的实际位置与目标位置之间距离（范数）大于5，即norm(e_pos_world[0:2])>5.0时：
    • 通过arctan(Y之差/X之差)计算实际机体坐标目标位置坐标之间的角度heading，即heading = math.atan2(e_pos_world[1],e_pos_world[0])
    • 按照计算所得的角度来更新目标四元数姿态,作为新的目标姿态四元数quat_des，使得机器人沿着[0,0,1]即Z轴旋转heading角度，朝着目标位置坐标方向前进。代码实现为：
     quat_des = numpy.array([0, 0, math.sin(0.5*heading), math.cos(0.5*heading)])
    • 最后计算新的机体姿态四元数误差。
    • 整理上述步骤代码，即可得到：
      if numpy.linalg.norm(e_pos_world[0:2]) > 5.0:
      heading = math.atan2(e_pos_world[1],e_pos_world[0])
      quat_des = numpy.array([0, 0, math.sin(0.5*heading), math.cos(0.5*heading)])
      e_rot_quat = trans.quaternion_multiply(trans.quaternion_conjugate(q), quat_des)