Ego Plannerz中加入平滑功能

一、前言

本文基于原文章《在 ROS 中集成 PX4 位置平滑 PositionSmoothing 功能》进行衍生与复刻，文章将详细介绍所需的准备工作、代码修改步骤以及遇到的常见问题和解决方法。

二、准备工作

1. 拷贝所需文件

在开始之前，我们需要准备一些必要的文件并将其复制到项目的正确位置。具体步骤如下：

1.1 创建 include 文件夹

首先，进入目录 /Fast-Drone-250/src/realflight_modules/px4ctrl，并创建一个新的 include 文件夹：

mkdir include

1.2 拷贝相关文件

接下来，找到 PX4 源码目录中的一些必要文件，并将它们复制到新创建的 include 文件夹中：

• 在 PX4-Autopilot/src/lib 目录下找到以下文件夹，并将其拷贝至 /Fast-Drone-250/src/realflight_modules/px4ctrl/include 中：

  • mathlib
  • matrix
  • motion_planning
• 然后，进入 PX4-Autopilot/platforms/common/include 目录，将该目录下的所有文件拷贝至 /Fast-Drone-250/src/realflight_modules/px4ctrl/include。

2. 解决编译错误

引入 PX4 的库文件后，可能会遇到以下编译错误。接下来，我们将逐一解决这些问题。

2.1 错误 1： <px4_boardconfig.h> 文件缺失

在某些情况下，可能会出现找不到 <px4_boardconfig.h> 文件的错误。由于我们并不需要这个文件，并且在官方的包中也找不到该文件，所以我们可以通过注释掉相关代码来解决此问题。具体方法是，通过批量查找并注释掉所有涉及 <px4_boardconfig.h> 的代码行。

2.2 错误 2： <matrix/math.hpp> 文件路径错误

另一个常见的错误是路径设置错误，导致编译时找不到 <matrix/math.hpp> 文件。在官方版本中，路径为 matrix/math.hpp，但是在我们的环境中需要将其修改为 matrix/matrix/math.hpp。可以通过批量替换将所有 <matrix/math.hpp> 替换为 <matrix/matrix/math.hpp> 来解决此问题。

3. 项目文件结构

为了便于理解，下面展示了 include 文件夹的文件结构：

├── config
├── include
│   ├── mathlib
│   │   └── math
│   │       ├── filter
│   │       └── test
│   ├── matrix
│   │   ├── matrix
│   │   └── test
│   ├── motion_planning
│   └── px4_platform_common
│   └── px4_work_queue
├── launch
├── src
└── thrust_calibrate_scrips

三、代码修改

接下来，我们需要修改代码，以实现位置平滑功能的集成。请根据以下步骤进行操作：

1. 修改 PX4CtrlFSM.cpp

在文件 /Fast-Drone-250/src/realflight_modules/px4ctrl/src/src/PX4CtrlFSM.cpp 中，添加以下代码：

1.1 平滑点定义

在大约第 44 行之后，添加以下代码：

// 平滑点定义
PositionSmoothing::PositionSmoothingSetpoints setpoints;

1.2 修改 STEP3 和 STEP4 的代码

在大约第 313 行左右，找到以下代码并进行修改：

    // STEP3: solve and update new control commands
    if (rotor_low_speed_during_land) // used at the start of auto takeoff
    {
        motors_idling(imu_data, u);
    }
    else
    {
        PositionSmoothing::PositionSmoothingSetpoints setpoints;
        geometry_msgs::PoseStamped poseStamped;
        // 当前位置
        poseStamped.header = odom_data.msg.header;
        poseStamped.pose = odom_data.msg.pose.pose;
        // 目标位置
        matrix::Vector3f target_position(des.p.x(), des.p.y(), des.p.z());
        generateSmoothTrajectory(poseStamped, target_position, setpoints);
    }

    // STEP4: publish control commands to mavros
    publish_position_ctrl(setpoints, now_time);

2. 新增函数

在文件末尾新增以下函数，以便处理位置控制的发布和轨迹平滑生成：

2.1 发布位置控制命令

void PX4CtrlFSM::publish_position_ctrl(const PositionSmoothing::PositionSmoothingSetpoints& setpoints, const ros::Time &stamp)
{
    mavros_msgs::PositionTarget msg;
    msg.header.stamp = stamp;
    msg.header.frame_id = std::string("FCU");  // 坐标系为飞控坐标系

    // 设置位置控制模式
    msg.type_mask = mavros_msgs::PositionTarget::IGNORE_AFX |
                    mavros_msgs::PositionTarget::IGNORE_AFY |
                    mavros_msgs::PositionTarget::IGNORE_AFZ |
                    mavros_msgs::PositionTarget::IGNORE_YAW |
                    mavros_msgs::PositionTarget::IGNORE_YAW_RATE;

    // 设置位置
    msg.position.x = setpoints.position(0);
    msg.position.y = setpoints.position(1);
    msg.position.z = setpoints.position(2);

    // 设置速度
    // msg.velocity.x = setpoints.velocity(0);
    // msg.velocity.y = setpoints.velocity(1);
    // msg.velocity.z = setpoints.velocity(2);

    // // 设置加速度
    // msg.acceleration_or_force.x = setpoints.acceleration(0);
    // msg.acceleration_or_force.y = setpoints.acceleration(1);
    // msg.acceleration_or_force.z = setpoints.acceleration(2);

    // 发布控制命令
    ctrl_FCU_pub.publish(msg);
}

2.2 设置轨迹平滑器参数

// 设置轨迹平滑器参数
void PX4CtrlFSM::insetupPositionSmoothg() {
    // 创建一个 Vector3f 对象用于最大加加速度
Eigen::Vector3f max_jerk(4.0f, 4.0f, 4.0f); // 加速度xyz
position_smoothing.setMaxJerk(max_jerk.x());// 使用 Vector3f 对象
// position_smoothing.setMaxJerk(4.0f);              // 最大加加速度 (m/s³)
position_smoothing.setMaxAccelerationXY(2.0f);    // 最大水平加速度 (m/s²)
position_smoothing.setMaxAccelerationZ(3.0f);     // 最大垂直加速度 (m/s²)
position_smoothing.setMaxVelocityXY(5.0f);        // 最大水平速度 (m/s)
position_smoothing.setMaxVelocityZ(3.0f);         // 最大垂直速度 (m/s)
position_smoothing.setHorizontalTrajectoryGain(0.8f);   // 水平轨迹增益：
position_smoothing.setMaxAllowedHorizontalError(2.0f); // 最大允许的水平误差 (m)：
position_smoothing.setVerticalAcceptanceRadius(0.5f); // 垂直接受半径 (m)：
position_smoothing.setCruiseSpeed(3.0f); // 巡航速度 (m/s)：
position_smoothing.setTargetAcceptanceRadius(0.5f); // 目标接受半径 (m)：
}

2.3 初始化轨迹生成器

// 在生成轨迹前，初始化轨迹生成器的状态
void PX4CtrlFSM::initializeTrajectoryGenerator(const geometry_msgs::PoseStamped& current_pose) {
    Vector3f current_position(
        current_pose.pose.position.x,
        current_pose.pose.position.y,
        current_pose.pose.position.z
    );

    Vector3f current_local_velocity(
        odom_data.msg.twist.twist.linear.x,
        odom_data.msg.twist.twist.linear.y,
        odom_data.msg.twist.twist.linear.z
    );


    Vector3f zero_acceleration(0.0f, 0.0f, 0.0f);

    // 重置轨迹生成器状态
    position_smoothing.reset(zero_acceleration, current_local_velocity, current_position);
}

2.4 生成平滑轨迹

// 生成平滑轨迹
void  PX4CtrlFSM::generateSmoothTrajectory(const geometry_msgs::PoseStamped& current_pose,
    const Vector3f& target_position,
    PositionSmoothing::PositionSmoothingSetpoints& setpoints // 修改为引用类型输出参数
   ) {
// 计算时间间隔

ros::Time current_time = ros::Time::now();
float delta_time = (current_time - last_generation_time).toSec();
last_generation_time = current_time;

// ROS_INFO("Delta time: %.4f", delta_time);  // 打印时间差，确认它是否有变化

// 创建当前位置向量
Vector3f current_pos(current_pose.pose.position.x,
current_pose.pose.position.y,
current_pose.pose.position.z);

// ROS_INFO("Smooth Current Position: [%.2f, %.2f, %.2f]", current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z);
// ROS_INFO("Smooth Target Position: [%.2f, %.2f, %.2f]", target_position(0), target_position(1), target_position(2));

// 创建预馈速度（这里设为0）
Vector3f feedforward_velocity(0.0f, 0.0f, 0.0f);



// 生成平滑轨迹
position_smoothing.generateSetpoints(
    current_pos, //当前位置
    target_position,// 目标位置
    feedforward_velocity, // 目标速度
    delta_time,  // 时间步长
    false, // 不强制零速度
    setpoints // 输出的平滑轨迹点
);
//返回平滑后的XYZ位置position，速度velocity，加速度acceleration等信息
// ROS_INFO("New Smoothed Position: [%.2f, %.2f, %.2f]", setpoints.position(0), setpoints.position(1), setpoints.position(2));

}

3. 修改头文件 PX4CtrlFSM.h

在头文件 src/realflight_modules/px4ctrl/src/PX4CtrlFSM.h 中，加入以下包含的头文件和私有成员：

3.1 包含新头文件

#include <mavros_msgs/PositionTarget.h>
#include "motion_planning/PositionSmoothing.hpp"
#include "motion_planning/VelocitySmoothing.hpp"

3.2 增加私有成员和函数声明

PositionSmoothing position_smoothing;
ros::Time last_generation_time;
void publish_position_ctrl(const PositionSmoothing::PositionSmoothingSetpoints& setpoints, const ros::Time &stamp);
void generateSmoothTrajectory(const geometry_msgs::PoseStamped& current_pose, const Vector3f& target_position, PositionSmoothing::PositionSmoothingSetpoints& setpoints);
void initializeTrajectoryGenerator(const geometry_msgs::PoseStamped& current_pose);
void insetupPositionSmoothg();

4. 修改 CMakeLists.txt

在文件 src/realflight_modules/px4ctrl/CMakeLists.txt 中，加入新的源文件：

add_executable(px4ctrl_node
  src/px4ctrl_node.cpp
  src/PX4CtrlFSM.cpp
  src/PX4CtrlParam.cpp
  src/controller.cpp
  src/input.cpp
  include/motion_planning/PositionSmoothing.cpp
  include/motion_planning/VelocitySmoothing.cpp
)

5. 修改 px4Ctrl_node.cpp

在文件 src/realflight_modules/px4ctrl/src/px4Ctrl_node.cpp 中，修改以下内容，大约在78行左右：

    // fsm.ctrl_FCU_pub = nh.advertise<mavros_msgs::AttitudeTarget>("/mavros/setpoint_raw/attitude", 10);
    fsm.ctrl_FCU_pub = nh.advertise<mavros_msgs::PoseStamped>("/mavros/setpoint_position/local", 10);

6. 解决编译问题

在编译时，如果遇到 math::negate(T) 没有返回值的问题，可以通过修改 /Fast-Drone-250/src/realflight_modules/px4ctrl/include/mathlib/math/Functions.hpp 文件的 negate258 行 函数，确保有返回值。修改如下：

template<>
constexpr int16_t negate(int16_t value)
{
    return (value == INT16_MAX) ? INT16_MIN :
           (value == INT16_MIN) ? INT16_MAX :
           -value;
}

四、编译与测试

在完成上述所有步骤后，可以尝试编译项目。如果编译过程中遇到问题，可以根据具体的错误提示进行相应的调整。