发布器和订阅器

cpp
展开代码
 ros::Publisher so3_command_pub_;    // 发布SO3控制命令的发布器
ros::Subscriber odom_sub_;          // 订阅里程计数据的订阅器
ros::Subscriber position_cmd_sub_;  // 订阅位置命令的订阅器
ros::Subscriber enable_motors_sub_; // 订阅电机使能信号的订阅器
ros::Subscriber corrections_sub_;   // 订阅校正参数的订阅器
ros::Subscriber imu_sub_;          // 订阅IMU数据的订阅器

回调函数声明

这些是消息处理函数，当ROS话题收到消息时会自动调用对应的回调函数。

cpp
展开代码
void publishSO3Command(void);                                    // 发布SO3控制命令
void position_cmd_callback(const quadrotor_msgs::PositionCommand::ConstPtr& cmd);  // 位置命令回调
void odom_callback(const nav_msgs::Odometry::ConstPtr& odom);    // 里程计数据回调
void enable_motors_callback(const std_msgs::Bool::ConstPtr& msg); // 电机使能回调
void corrections_callback(const quadrotor_msgs::Corrections::ConstPtr& msg); // 校正参数回调
void imu_callback(const sensor_msgs::Imu& imu);                 // IMU数据回调

回调函数参数详解

position_cmd_callback 参数

cpp
展开代码
void
SO3ControlNodelet::position_cmd_callback( const quadrotor_msgs::PositionCommand::ConstPtr& cmd)

• 参数类型：

quadrotor_msgs::PositionCommand::ConstPtr&

• 消息类型：

quadrotor_msgs::PositionCommand - 四旋翼位置控制命令

• ConstPtr：指向常量对象的智能指针
• const：只能读取，不能修改消息内容

odom_callback 参数

cpp
展开代码
void
SO3ControlNodelet::odom_callback(const nav_msgs::Odometry::ConstPtr& odom)

• 参数类型：nav_msgs::Odometry::ConstPtr&

消息类型：nav_msgs::Odometry - 导航里程计消息

• 消息内容

cpp
展开代码
// nav_msgs::Odometry 包含：
odom->pose.pose.position.x/y/z     // 当前位置
odom->pose.pose.orientation        // 当前姿态（四元数）
odom->twist.twist.linear.x/y/z     // 当前线速度
odom->twist.twist.angular.x/y/z    // 当前角速度
odom->header.frame_id              // 坐标系ID
odom->header.stamp                 // 时间戳

enable_motors_callback 参数

cpp
展开代码
void
SO3ControlNodelet::enable_motors_callback(const std_msgs::Bool::ConstPtr& msg)

• 参数类型：std_msgs::Bool::ConstPtr&
• 消息类型：std_msgs::Bool - 标准布尔消息
• 消息内容

cpp
展开代码
msg->data  // 布尔值：true=启用电机，false=禁用电机

作用：控制电机开关状态

corrections_callback 参数

cpp
展开代码
void
SO3ControlNodelet::corrections_callback(const quadrotor_msgs::Corrections::ConstPtr& msg)

imu_callback 参数

cpp
展开代码
void
SO3ControlNodelet::imu_callback(const sensor_msgs::Imu& imu)

• 参数类型：sensor_msgs::Imu&
• 消息类型：sensor_msgs::Imu - 惯性测量单元消息

作用：提供加速度和角速度传感器数据

• 消息内容

cpp
展开代码
// sensor_msgs::Imu 包含：
imu.linear_acceleration.x/y/z  // 线性加速度 (m/s²)
imu.angular_velocity.x/y/z     // 角速度 (rad/s)
imu.orientation               // 姿态（四元数）
imu.header.frame_id           // 坐标系ID
imu.header.stamp              // 时间戳

小结

text
展开代码
ROS标准消息类型：
├── std_msgs::Bool          (基础类型)
├── sensor_msgs::Imu        (传感器数据)
├── nav_msgs::Odometry      (导航数据)
└── quadrotor_msgs::*       (四旋翼专用消息)
    ├── PositionCommand     (位置控制命令)
    └── Corrections         (校正参数)

智能指针 vs 直接引用

cpp
展开代码
// 错误方式 - 会拷贝整个消息
void callback(quadrotor_msgs::PositionCommand msg) {
    // 这里会拷贝整个消息对象，性能差
}

// 正确方式 - 只传递指针
void callback(const quadrotor_msgs::PositionCommand::ConstPtr& msg) {
    // 只传递指针，不拷贝数据
}

如果使用值传递，可能导致：

1. 控制延迟增加
2. 系统不稳定
3. 响应速度下降
4. 电池消耗增加

代码解析：

cpp
展开代码
// 创建消息对象
  quadrotor_msgs::SO3Command::Ptr so3_command(
    new quadrotor_msgs::SO3Command);

  // 设置消息头信息
  so3_command->header.stamp    = ros::Time::now();
  so3_command->header.frame_id = frame_id_;

  // 设置力向量
  so3_command->force.x = force(0);
  so3_command->force.y = force(1);
  so3_command->force.z = force(2);

  // 设置姿态四元数
  so3_command->orientation.x = orientation.x();
  so3_command->orientation.y = orientation.y();
  so3_command->orientation.z = orientation.z();
  so3_command->orientation.w = orientation.w();

  // 设置控制增益
  for (int i = 0; i < 3; i++) {
    so3_command->kR[i]  = kR_[i];
    so3_command->kOm[i] = kOm_[i];
  }

  // 设置辅助信息
  so3_command->aux.current_yaw = current_yaw_;
  so3_command->aux.enable_motors = enable_motors_;
  so3_command->aux.kf_correction = corrections_[0];
  so3_command->aux.angle_corrections[0] = corrections_[1];
  so3_command->aux.angle_corrections[1] = corrections_[2];

  // 发布消息
  so3_command_pub_.publish(so3_command);

1.创建消息对象语句的参数含义

cpp
展开代码
quadrotor_msgs::SO3Command::Ptr so3_command(new quadrotor_msgs::SO3Command);

动态创建一个名为 so3_command的智能指针，该指针指向一个类型为 quadrotor_msgs::SO3Command的全新的、空的消息对象。用于后续填充数据

• quadrotor_msgs::SO3Command::Ptr：这是 SO3Command消息类型的智能指针

quadrotor_msgs 是消息包名

SO3Command 是具体的消息类型

::Ptr：这是 SO3Command消息类型的智能指针

• new quadrotor_msgs::SO3Command：在堆内存中分配空间，创建一个全新的、未初始化的 SO3Command消息对象

2.-> 操作符的含义和用法

-> 是C++中用于访问指针指向的对象的成员的操作符。

cpp
展开代码
// 访问消息对象的成员
so3_command->header.stamp = ros::Time::now();
so3_command->force.x = force(0);
so3_command->orientation.x = orientation.x();

// 访问消息的辅助信息结构
so3_command->aux.current_yaw = current_yaw_;
so3_command->aux.enable_motors = enable_motors_;
so3_command->aux.kf_correction = corrections_[0];

• so3_command 是一个智能指针，指向 SO3Command 消息对象
• -> 用来访问这个指针指向的对象内部的成员
• aux 是消息内部的一个结构体成员
• current_yaw_、enable_motors_ 等是 aux 结构体内部的成员

cpp
展开代码
// 使用 -> 操作符
so3_command->aux.current_yaw = current_yaw_;

// 等价于使用 * 操作符
(*so3_command).aux.current_yaw = current_yaw_;

n.subscribe() 创建订阅器，参数包括话题名、队列大小、回调函数、对象指针

cpp
展开代码
odom_sub_ = n.subscribe("odom", 10, &SO3ControlNodelet::odom_callback, this,ros::TransportHints().tcpNoDelay());

• this 参数：

this 是C++中的当前对象指针

指向当前 SO3ControlNodelet 类的实例

当ROS调用回调函数时，需要知道调用哪个对象的成员函数

this 告诉ROS："当收到消息时，调用这个对象的odom_callback 函数"

• ros::TransportHints().tcpNoDelay() 参数：

ros::TransportHints() - 创建传输提示对象

.tcpNoDelay() - 设置TCP传输选项，禁用Nagle算法

作用：减少网络延迟，提高实时性

在 RViz 中手动绘制一个自定义的三维点云地图（.pcd 文件），用于为 ego-planner的仿真提供环境模型。

具体步骤及其作用如下：

1.安装绘图工具：

• 从 HKUST-Aerial-Robotics/FUEL的 GitHub 仓库中获取 map_generator功能包。

• 将该功能包放入你的 ROS 工作空间。

• 使用 catkin_make编译整个工作空间。这会将 map_generator包中的节点（可执行程序）编译出来，使你能够使用其中的地图生成和记录工具

bash
展开代码
docker run \
-e QT_X11_NO_MITSHM=1 \
-e DISPLAY \
--net=host \
--gpus all \
-v ~/.Xauthority:/root/.Xauthority:rw \
-v ~/tmp/.X11-unix:/tmp/.X11-unix:ro \
-v ~/out_home:/out_home \
-v /data/xiedong/cc_ws:/cc_ws \
-it kevinchina/deeplearning:ros-noetic-cuda11.4.2-v5 bash

bash
展开代码
# 创建ROS工作空间   终端1
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src   

# 从GitHub克隆FUEL仓库（如果尚未安装git：sudo apt-get install git）
git clone https://github.com/HKUST-Aerial-Robotics/FUEL.git

# 或者如果只需要map_generator，可以只克隆该包
# git clone https://github.com/HKUST-Aerial-Robotics/FUEL/tree/master/fuel_planner/map_generator

# 返回工作空间根目录
cd ~/catkin_ws

# 编译工作空间
catkin_make

完成了：

1. 将 map_generator文件夹放在 ROS 工作空间的 src目录下 (e.g., ~/catkin_ws/src/)
2. catkin_make根据 CMakeLists.txt文件中的指令，编译 map_generator包中的 C++ 源代码：

将 click_map_node.cpp(假设名) 编译成名为 click_map的可执行文件。

编译生成的可执行文件 (click_map, map_recorder, map_pub) 最终会出现在工作空间的 devel/lib/map_generator/目录下。catkin_make还会生成环境设置脚本 (devel/setup.bash)。

2.启动绘图环境：

• 激活工作空间环境：

bash
展开代码
source devel/setup.bash

实现： 该脚本设置当前终端会话的环境变量。最关键的是修改 ROS_PACKAGE_PATH，将当前工作空间的路径（包含 map_generator）添加进去。这样，当运行 roslaunch、rosrun或 rviz时，ROS 就能找到工作空间内的功能包（如 map_generator, exploration_manager）和可执行文件（如 click_map）。

• 启动一个预设好的 RViz 配置：

bash
展开代码
roslaunch exploration_manager rviz.launch

作用： 启动一个或多个 ROS 节点，并加载预设的 RViz 配置

这个终端窗口会被 roslaunch进程占用，显示 RViz 的启动日志和可能的运行时信息。无法在这个终端里再输入其他命令，因为它在等待 roslaunch进程结束

新开终端：

• 同样导航到您的工作空间：

bash
展开代码
cd ~/catkin_ws    #终端2

(如果您的工作空间路径不同，请修改)。

• 同样激活工作空间环境：

bash
展开代码
source devel/setup.bash

(这必须在每个新终端里运行一次，以设置环境)。

• 启动地图点击生成节点：

bash
展开代码
rosrun map_generator click_map

map_generator功能包中click_map节点的实现。这个节点负责监听RViz中的点击事件，并根据两个点击点生成一堵三维的墙

关键点：执行此命令后，终端看起来会卡住（“不会返回任何指令”），但这是正常现象，意味着节点已启动并等待接收 RViz 中的点击指令。

3.在 RViz 中绘制地图：

• 在 RViz 的工具栏中，找到并选择 2D Nav Goal工具。

• 在 RViz 的 3D 视图（通常是 TopDownOrtho或其他平面视图）中点击一次，设置一个起始点。

• 再次在视图中的另一个位置点击一次，设置一个终止点。

• 效果：点击完成后，程序会自动在起始点和终止点之间生成一个竖直的、长方体的“墙壁”障碍物。这条“墙”就是构成你自定义仿真环境的一个基本元素。

• 重复： 你可以重复上述点击操作（选点 -> 选点），在场景的不同位置添加多条这样的墙壁，逐步构建出你想要的迷宫、房间或其他包含障碍物的三维环境结构。

4.保存绘制的地图：

• 当你完成所有墙壁的绘制后，新开终端：

bash
展开代码
cd ~/catkin_ws      # 终端3——进入您的工作空间目录
source devel/setup.bash  # 激活ROS环境

• 执行保存命令（运行地图保存命令，将地图保存到指定路径。注意，为了避免覆盖，建议指定一个完整的文件名（而不仅仅是目录）。例如，保存到主目录下并命名为mymap.pcd：）

bash
展开代码
rosrun map_generator map_recorder ~/mymap.pcd

#或者，如果你想保存在当前目录（比如工作空间的某个文件夹中），可以这样：
rosrun map_generator map_recorder $(pwd)/src/map_generator/resource/mymap.pcd

map_recorder节点会订阅当前已经发布在ROS系统中的点云地图（由click_map节点发布），并将该点云保存为PCD文件。因此，在运行这个命令时，确保click_map节点仍在运行（即终端2还在运行），并且你绘制的墙壁点云已经发布。

保存完成后，你可以在指定的路径（如主目录）找到这个.pcd文件。

• 路径： ~/表示文件保存在当前用户的主目录 (/home/your_username/)下。

• 文件名： 默认生成的文件名是 tmp.pcd。

• 重要提示： 如果你要绘制多个不同的地图，必须在 ~/后面添加不同的文件名或子目录。例如：

bash
展开代码
rosrun map_generator map_recorder ~/map1.pcd

(保存为 map1.pcd)

bash
展开代码
rosrun map_generator map_recorder ~/mymaps/scenarioA.pcd

(保存到 mymaps子目录下)

如果每次都只用 ~/，新的 tmp.pcd文件会覆盖之前保存的同名文件。

• 确认保存结果

bash
展开代码
ls -lh ~/your_map_name.pcd

命令执行后会显示保存信息（如点数、文件大小）

5.在仿真中使用自定义地图(悬而未决)

• 将保存好的 .pcd文件（如 2tmp.pcd）复制或移动到你的仿真项目目录中（例如 map_generator功能包的 resource文件夹下，如示例所示）。

• 修改启动仿真环境的 .launch文件。

• 在 launch 文件中，添加一个 node声明来启动 map_generator包中的 map_pub节点：

<node pkg="map_generator" name="map_pub" type="map_pub" output="screen" args="$(find map_generator)/resource/2tmp.pcd"/>

• pkg="map_generator": 指定功能包名。
• name="map_pub": 给这个节点实例起个名字。
• type="map_pub": 指定要运行的节点类型（对应编译出的可执行文件）。
• output="screen": 将节点输出打印到终端屏幕（方便调试）。
• args="$(find map_generator)/resource/2tmp.pcd": 最关键参数！ 它告诉 map_pub节点去加载哪个 .pcd文件作为地图。$(find map_generator)会自动找到 map_generator包的安装路径，然后拼接上 /resource/2tmp.pcd得到地图文件的完整路径。你需要将 2tmp.pcd替换成你实际保存的地图文件名和相对路径。

当运行这个 .launch文件启动仿真时，map_pub节点就会读取指定的 .pcd文件，并将里面保存的三维墙壁信息发布为点云话题。路径规划算法（如 ego-planner）订阅这个话题，就能感知到这个自定义的环境地图，并在这个环境中进行路径规划和避障仿真。

bash
展开代码
docker run \
-e QT_X11_NO_MITSHM=1 \
-e 

准备工作

创建和构建一个名为xi的catkin工作空间：

bash
展开代码
mkdir -p /

已知线性系统的状态空间方程为：