引言

在 ROS（Robot Operating System）生态中，无人机控制是一个常见且重要的应用场景。demo3.cpp 是一个典型的 ROS 节点，用于控制无人机进入 OFFBOARD 模式，并使其按照预设路径飞行。本文将详细解析该节点的主要功能、关键函数以及所涉及的算法。

1. ROS 节点概述

1.1 节点初始化

在 main 函数中，首先调用 ros::init 初始化 ROS 节点，节点名称为 offboard_node。接着创建 ros::NodeHandle 实例，用于与 ROS 系统进行通信。

ros::init(argc, argv, "offboard_node");
ros::NodeHandle nh;

1.2 订阅与发布

节点订阅了两个重要的话题：

• mavros/state：获取无人机的当前状态，如模式、解锁状态等。
• mavros/local_position/odom：获取无人机的当前位置和姿态信息。

同时，节点发布 mavros/setpoint_position/local 话题，用于向无人机发送目标位置。

ros::Subscriber state_sub = nh.subscribe("mavros/state", 10, state_cb);
ros::Subscriber odom_sub = nh.subscribe("mavros/local_position/odom", 10, odom_cb);
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);

1.3 服务客户端

节点创建了两个服务客户端，用于请求无人机模式切换和解锁：

ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");

2. 关键函数解析

2.1 odom_cb 回调函数

该函数用于处理无人机的里程计信息，更新当前位置和姿态，并在 need_q 为 true 时，将姿态四元数添加到 init_q 向量中。

void odom_cb(const nav_msgs::Odometry::ConstPtr &msg)
{
  current_pose.pose = msg->pose.pose;
  if (!need_q)
  {
    return;
  }
  Eigen::Quaterniond q = Eigen::Quaterniond(
      msg->pose.pose.orientation.w, msg->pose.pose.orientation.x,
      msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
  init_q.push_back(q);
}

2.2 state_cb 回调函数

处理无人机的状态信息，当无人机首次解锁时，记录机体坐标系到 ENU 坐标系的变换。

void state_cb(const mavros_msgs::State::ConstPtr &msg)
{
  current_state = *msg;
  if (!is_armed && msg->armed)
  {
    is_armed = true;
    body_to_enu_transform.header.stamp = ros::Time::now();
    body_to_enu_transform.header.frame_id = "enu";
    body_to_enu_transform.child_frame_id = "body";
    body_to_enu_transform.transform.translation.x = current_pose.pose.position.x;
    body_to_enu_transform.transform.translation.y = current_pose.pose.position.y;
    body_to_enu_transform.transform.translation.z = current_pose.pose.position.z;
    body_to_enu_transform.transform.rotation = current_pose.pose.orientation;
  }
}

2.3 get_avg_yaw 函数

计算一组四元数对应的平均偏航角。通过将偏航角转换为正弦和余弦值的平均值，再使用 atan2 函数得到最终的平均偏航角。

double get_avg_yaw(const std::vector<Eigen::Quaterniond> &quats)
{
  if (quats.empty())
  {
    ROS_WARN("Quaternion vector is empty. Returning 0 yaw.");
    return 0.0;
  }

  double sin_sum = 0.0;
  double cos_sum = 0.0;

  for (const auto &q : quats)
  {
    Eigen::Vector3d euler = q.toRotationMatrix().eulerAngles(0, 1, 2);
    double yaw = euler[2];

    sin_sum += std::sin(yaw);
    cos_sum += std::cos(yaw);
  }

  return std::atan2(sin_sum, cos_sum);
}

2.4 p_body_to_ENU 函数

使用 TF（Transform）库将目标点从机体坐标系转换到 ENU 坐标系。

std::vector<geometry_msgs::PoseStamped> p_body_to_ENU(const std::vector<geometry_msgs::PoseStamped> &target_points)
{
  std::vector<geometry_msgs::PoseStamped> transformed_points;
  for (const auto &target : target_points)
  {
    geometry_msgs::PoseStamped target_enu;
    try
    {
      target_enu = tf_buffer->transform(target, "enu", ros::Duration(1.0));
      ROS_INFO("Transformed target in ENU coordinates: x = %f, y = %f, z = %f",
               target_enu.pose.position.x,
               target_enu.pose.position.y,
               target_enu.pose.position.z);
      transformed_points.push_back(target_enu);
    }
    catch (tf2::TransformException &ex)
    {
      ROS_WARN("Transform failed: %s", ex.what());
      continue;
    }
  }
  return transformed_points;
}

2.5 is_close 函数

判断两个位置是否接近，通过计算两点之间的欧几里得距离，并与给定的阈值进行比较。

bool is_close(const geometry_msgs::PoseStamped &p1,
              const geometry_msgs::PoseStamped &p2, double tol)
{
  double dx = p1.pose.position.x - p2.pose.position.x;
  double dy = p1.pose.position.y - p2.pose.position.y;
  double dz = p1.pose.position.z - p2.pose.position.z;
  return std::sqrt(dx * dx + dy * dy + dz * dz) < tol;
}

2.6 create_target_positions 函数

创建一个目标点序列，包含三个不同高度的目标点。

std::vector<geometry_msgs::PoseStamped> create_target_positions()
{
  std::vector<geometry_msgs::PoseStamped> targets;
  geometry_msgs::PoseStamped pose;
  pose.pose.position.x = 0;
  pose.pose.position.y = 0;

  pose.pose.position.z = 1.2;
  targets.push_back(pose);
  pose.pose.position.z = 0.8;
  targets.push_back(pose);
  pose.pose.position.z = 0.0;
  targets.push_back(pose);
  return targets;
}

3. 算法流程

3.1 初始化阶段

1. 初始化 ROS 节点和相关对象。
2. 等待与无人机的连接。
3. 初始化目标路径。
4. 收集初始姿态四元数。

3.2 进入 OFFBOARD 模式

1. 发送一段时间的目标位置，以便无人机进入 OFFBOARD 模式。
2. 请求将无人机模式切换为 OFFBOARD。
3. 请求解锁无人机。

3.3 飞行控制阶段

1. 将目标点从机体坐标系转换到 ENU 坐标系。
2. 控制无人机依次飞向目标点，当接近目标点时，切换到下一个目标点。
3. 持续发布无人机的目标位置，直到所有目标点都到达。

3.4 结束阶段

释放动态分配的内存，退出程序。