PX4 通过 Vision 实现 Position、Altitude 和 Offboard 模式

news2025/2/22 16:20:09

本文通过 VINS-Fusion 的里程计信息为 PX4 提供视觉信息，从而达到视觉定高和定点的目的

主要工作为创建一个将 vins 里程计信息发布给 Mavros 的 /mavros/vision_pose/pose 话题

首先创建一个工作空间

mkdir -p ~/catkin_ws/src/vision_to_mavros/src/
cd ~/catkin_ws/src/vision_to_mavros/src/

然后创建一个名为 vision_to_mavros 的功能包

catkin_create_pkg vision_to_mavros roscpp std_msgs geometry_msgs mavros_msgs nav_msgs tf2_eigen tf

在 ~/catkin_ws/src/vision_to_mavros/src/vision_to_mavros/src/ 目录下创建一个 vision_to_mavros 节点，其内容如下

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Eigen>


Eigen::Vector3d p_vins;
Eigen::Quaterniond q_vins;


void vins_callback(const nav_msgs::Odometry::ConstPtr &msg)
{
    if(msg->header.frame_id == "world")
    {
        // // vins 的坐标轴方向, y 轴朝前, x 轴朝右, z 轴朝上
        // p_vins = Eigen::Vector3d(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z);

        // q_vins = Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);


        // vins 的坐标轴方向, y 轴朝前, x 轴朝右, z 轴朝上
        // 转化为 x 轴朝前, y 轴朝左, z 轴朝上
        p_vins = Eigen::Vector3d(msg->pose.pose.position.y, -msg->pose.pose.position.x, msg->pose.pose.position.z);

        q_vins = Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
        Eigen::AngleAxisd roll(M_PI/2,Eigen::Vector3d::UnitX()); // 绕 x 轴旋转 pi / 2
        Eigen::AngleAxisd pitch(0,Eigen::Vector3d::UnitY());
        Eigen::AngleAxisd yaw(0,Eigen::Vector3d::UnitZ());

        Eigen::Quaterniond _q_vins = roll * pitch * yaw;
        q_vins = q_vins * _q_vins;
    }
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "vins_to_mavros");
    ros::NodeHandle nh("~");

    ros::Subscriber slam_sub = nh.subscribe<nav_msgs::Odometry>("odom", 100, vins_callback);

    ros::Publisher vision_pub = nh.advertise<geometry_msgs::PoseStamped>("vision_pose", 10);


    // the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);

    ros::Time last_request = ros::Time::now();

    while(ros::ok()){
        geometry_msgs::PoseStamped vision;

        vision.pose.position.x = p_vins[0];
        vision.pose.position.y = p_vins[1];
        vision.pose.position.z = p_vins[2];

        vision.pose.orientation.x = q_vins.x();
        vision.pose.orientation.y = q_vins.y();
        vision.pose.orientation.z = q_vins.z();
        vision.pose.orientation.w = q_vins.w();

        vision.header.stamp = ros::Time::now();
        vision_pub.publish(vision);

        ROS_INFO("\nposition:\n   x: %.18f\n   y: %.18f\n   z: %.18f\norientation:\n   x: %.18f\n   y: %.18f\n   z: %.18f\n   w: %.18f", \
        p_vins[0],p_vins[1],p_vins[2],q_vins.x(),q_vins.y(),q_vins.z(),q_vins.w());

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

然后在 ~/catkin_ws/src/vision_to_mavros/src/vision_to_mavros/launch/ 下创建一个 launch 文件用于启动该节点，其内容如下

<launch>
    <node pkg="vision_to_mavros"  type="vins_to_mavros_node" name="vins_to_mavros" output="screen">
        <remap from="~vision_pose" to="/mavros/vision_pose/pose" />
        <remap from="~odom" to="/vins_estimator/odometry" />
    </node>
</launch>

最后配置 CMakeList.txt 文件，在相应位置添加两行

add_executable(vins_to_mavros_node src/vins_to_mavros.cpp)
target_link_libraries(vins_to_mavros_node
  ${catkin_LIBRARIES}
)

完成后编译该功能包

cd ~/catkin_ws/src/vision_to_mavros/
catkin_make

编译完成后启动 d435i 相机和 vins 节点，以及刚刚编译好的 vision_to_mavros 功能包

注意到 vision_to_mavros 节点已经开始打印位置信息了

输入 rostopic list 打印当前话题

lilabws001@lilabws001:~$ rostopic list
/camera/accel/imu_info
/camera/gyro/imu_info
/camera/imu
/camera/infra1/camera_info
/camera/infra1/image_rect_raw
/camera/infra1/image_rect_raw/compressed
/camera/infra1/image_rect_raw/compressed/parameter_descriptions
/camera/infra1/image_rect_raw/compressed/parameter_updates
/camera/infra1/image_rect_raw/compressedDepth
/camera/infra1/image_rect_raw/compressedDepth/parameter_descriptions
/camera/infra1/image_rect_raw/compressedDepth/parameter_updates
/camera/infra1/image_rect_raw/theora
/camera/infra1/image_rect_raw/theora/parameter_descriptions
/camera/infra1/image_rect_raw/theora/parameter_updates
/camera/infra1/metadata
/camera/infra2/camera_info
/camera/infra2/image_rect_raw
/camera/infra2/image_rect_raw/compressed
/camera/infra2/image_rect_raw/compressed/parameter_descriptions
/camera/infra2/image_rect_raw/compressed/parameter_updates
/camera/infra2/image_rect_raw/compressedDepth
/camera/infra2/image_rect_raw/compressedDepth/parameter_descriptions
/camera/infra2/image_rect_raw/compressedDepth/parameter_updates
/camera/infra2/image_rect_raw/theora
/camera/infra2/image_rect_raw/theora/parameter_descriptions
/camera/infra2/image_rect_raw/theora/parameter_updates
/camera/infra2/metadata
/camera/motion_module/parameter_descriptions
/camera/motion_module/parameter_updates
/camera/realsense2_camera_manager/bond
/camera/rgb_camera/parameter_descriptions
/camera/rgb_camera/parameter_updates
/camera/stereo_module/auto_exposure_roi/parameter_descriptions
/camera/stereo_module/auto_exposure_roi/parameter_updates
/camera/stereo_module/parameter_descriptions
/camera/stereo_module/parameter_updates
/diagnostics
/feature_tracker/feature
/mavros/vision_pose/pose
/rosout
/rosout_agg
/tf
/tf_static
/vins_estimator/camera_pose
/vins_estimator/camera_pose_right
/vins_estimator/camera_pose_visual
/vins_estimator/extrinsic
/vins_estimator/image_track
/vins_estimator/imu_propagate
/vins_estimator/key_poses
/vins_estimator/keyframe_point
/vins_estimator/keyframe_pose
/vins_estimator/margin_cloud
/vins_estimator/odometry
/vins_estimator/path
/vins_estimator/point_cloud
/vins_estimator/rectify_pose_left
/vins_estimator/rectify_pose_right

我们发现已经有 /mavros/vision_pose/pose 这个话题了

接下来，只需要将飞控与机载电脑连接，然后启动 mavros 节点