发布器和订阅器

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) {
    // 只传递指针，不拷贝数据
}

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

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

代码解析：

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_;

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算法

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

目录