Merge pull request #48 from HuaYuXiao/test-mavros

[v4.1.1] [bug fix] prevent unnecessary subscriber when using EKF2 fusion
HuaYuXiao · Sep 7, 2024 · 3dada3c · 3dada3c
2 parents 6090089 + cbf5e56
commit 3dada3c
Show file tree

Hide file tree

Showing 8 changed files with 153 additions and 104 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -9,6 +9,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - [CRUCIAL, bug fix] prevent performing leaked command (skip iteration if current task's done)
 - [new feature]: `Return`, `Stabilized`, `Acro`, `Rattitude`, `Altitude`, `Position`
 - [bug fix] switch to `Hold` once reach desired `Takeoff` height
+- [bug fix] prevent unnecessary subscriber when using EKF2 fusion
 
 ## v4.1.0 - 2024.08.24
 - [new feature] check whether `Armed` before `AUTO.TAKEOFF`

diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -41,8 +41,8 @@ include_directories(
 add_executable(px4ctrl_control src/PX4CtrlFSM.cpp)
 target_link_libraries(px4ctrl_control ${catkin_LIBRARIES})
 
-add_executable(px4ctrl_vision src/px4ctrl_vision.cpp)
-target_link_libraries(px4ctrl_vision ${catkin_LIBRARIES})
+add_executable(ekf2_fusion src/ekf2_fusion.cpp)
+target_link_libraries(ekf2_fusion ${catkin_LIBRARIES})
 
 add_executable(px4ctrl_terminal src/px4ctrl_terminal.cpp)
 target_link_libraries(px4ctrl_terminal ${catkin_LIBRARIES})

diff --git a/README.md b/README.md
@@ -8,12 +8,12 @@
 
 A package to control multi-copters using PX4-Autopilot platform. Implement following functions: 
 
-1. Process location data from SLAM, Gazebo, MoCap, etc. Publish to `/mavros/vision_pose/pose`
+1. Process EKF2 fusion from SLAM, Gazebo, MoCap, etc. Publish to `/mavros/vision_pose/pose`
 2. Control drone to `Arm`, `Takeoff`, `Offboard`, `Move`, `Hold`, `Land`, Return, etc. Publish to `/mavros/setpoint_raw/local`
 3. Process control command from path-planner, with format `quadrotor_msgs::PositionCommand`
 4. Interact with a terminal to control drone, with format `easondrone_msgs::ControlCommand`
 
-![Snipaste_2024-08-25_21-44-35.png](doc/Snipaste_2024-08-25_21-44-35.png)
+![screenshot](doc/Snipaste_2024-08-25_21-44-35.png)
 
 Visit this [yuque doc](https://www.yuque.com/g/easonhua/nx9k7f/xuv0pnk5yxk9qw3v/collaborator/join?token=V4SM11MTCXNawO7w&source=doc_collaborator#) for detailed information.
 
@@ -30,6 +30,8 @@ cd ~/easondrone_ws && catkin_make --source module/px4ctrl --build module/px4ctrl
 roslaunch px4ctrl px4ctrl.launch
 ```
 
+![rosgraph.png](doc/rosgraph.png)
+
 ## Acknowledgement
 
 Thanks for following packages:

diff --git a/doc/rosgraph.png b/doc/rosgraph.png
diff --git a/include/px4ctrl_node.h b/include/px4ctrl_node.h
@@ -74,7 +74,7 @@ namespace PX4CtrlFSM{
     double odom_roll_, odom_pitch_, odom_yaw_;
     geometry_msgs::PoseStamped vision_pose_;
 
-    int input_source;
+    int ekf2_source_;
     string LIO_topic_, T265_topic_, Gazebo_topic_, VIO_topic_;
     string object_name;
     std::string subject_name;
@@ -160,7 +160,7 @@ namespace PX4CtrlFSM{
     }
 
     void VICON_cb(const geometry_msgs::TransformStamped::ConstPtr& msg){
-        if (input_source != 6){
+        if (ekf2_source_ != 6){
             return;
         }
 
@@ -174,7 +174,7 @@ namespace PX4CtrlFSM{
     }
 
     void Gazebo_cb(const nav_msgs::Odometry::ConstPtr &msg){
-        if (input_source != 2){
+        if (ekf2_source_ != 2){
             return;
         }
 
@@ -188,7 +188,7 @@ namespace PX4CtrlFSM{
     }
 
     void T265_cb(const nav_msgs::Odometry::ConstPtr &msg){
-        if (input_source != 3){
+        if (ekf2_source_ != 3){
             return;
         }
 
@@ -204,7 +204,7 @@ namespace PX4CtrlFSM{
     }
 
     void optitrack_cb(const geometry_msgs::PoseStamped::ConstPtr &msg){
-        if (input_source != 0){
+        if (ekf2_source_ != 0){
             return;
         }
 
@@ -218,7 +218,7 @@ namespace PX4CtrlFSM{
     }
 
     void LIO_cb(const nav_msgs::Odometry::ConstPtr &msg){
-        if (input_source != 5){
+        if (ekf2_source_ != 5){
             return;
         }
 
@@ -234,7 +234,7 @@ namespace PX4CtrlFSM{
     }
 
     void VIO_cb(const nav_msgs::Odometry::ConstPtr &msg){
-        if (input_source != 1){
+        if (ekf2_source_ != 1){
             return;
         }
 

diff --git a/launch/px4ctrl.launch b/launch/px4ctrl.launch
@@ -1,21 +1,21 @@
 <launch>
-    <node pkg="px4ctrl" type="px4ctrl_control" name="px4ctrl_control"
-          output="screen" launch-prefix="gnome-terminal --"/>
-
-    <node pkg="px4ctrl" type="px4ctrl_vision" name="px4ctrl_vision" output="screen">
-        <!-- 定位数据输入源: 6 - VICON, 2 - Gazebo, 3 - T265, 5 - LIO, 0 - optitrack, 1 - VIO -->
-        <param name="input_source" value="2"/>
+    <node pkg="px4ctrl" type="ekf2_fusion" name="ekf2_fusion">
+        <!-- 定位数据输入源: 2 Gazebo, 1 VIO, 4 VICON, 3 T265, 5 LIO, 0 OptiTrack -->
+        <param name="EKF2_source" value="2"/>
         <param name="LIO_topic" value="/Odometry"/>
         <param name="Gazebo_topic" value="/gazebo/groundtruth/odom"/>
         <param name="T265_topic" value="/t265/odom/sample"/>
         <param name="VIO_topic" value="/vins_estimator/odometry"/>
     </node>
 
+    <node pkg="px4ctrl" type="px4ctrl_control" name="px4ctrl_control"
+          output="screen" launch-prefix="gnome-terminal --"/>
+
     <node pkg="px4ctrl" type="px4ctrl_terminal" name="px4ctrl_terminal"
           output="screen" launch-prefix="gnome-terminal --"/>
 
     <node pkg="px4ctrl" type="px4ctrl_navigate.py" name="px4ctrl_navigate"
           output="screen" launch-prefix="gnome-terminal --"/>
 
-    <node pkg="px4ctrl" type="joy_node" name="joy_node" output="screen"/>
+    <node pkg="px4ctrl" type="joy_node" name="joy_node"/>
 </launch>
diff --git a/src/ekf2_fusion.cpp b/src/ekf2_fusion.cpp
@@ -0,0 +1,132 @@
+/*
+ * Maintainer: Eason Hua
+ * Update Time: 2024.09.07
+ * [email protected]
+ */
+
+#include "px4ctrl_node.h"
+
+using namespace PX4CtrlFSM;
+
+/****** 主函数 ******/
+int main(int argc, char **argv){
+    ros::init(argc, argv, "ekf2_fusion");
+    ros::NodeHandle nh("~");
+
+    // 定位数据输入源
+    nh.param<int>("ekf2_source", ekf2_source_, 2);
+
+    switch (ekf2_source_){
+        case 0:{
+            cout << "[px4ctrl_vision] Using OptiTrack as input source." << endl;
+
+            // 动作捕捉设备中设定的刚体名字
+            nh.param<string>("object_name", object_name, "p450");
+
+            // 【订阅】optitrack估计位置
+            optitrack_sub = nh.subscribe<geometry_msgs::PoseStamped>
+                    ("/vrpn_client_node/"+ object_name + "/pose", 100, optitrack_cb);
+
+            break;
+        }
+
+        case 1:{
+            cout << "[px4ctrl_vision] Using VIO as input source." << endl;
+
+            nh.param<string>("VIO_topic", VIO_topic_, "/vins_estimator/odometry");
+
+            // subscribe to odometry from LIO
+            VIO_sub_ = nh.subscribe<nav_msgs::Odometry>
+                    (VIO_topic_, 100, VIO_cb);
+
+            break;
+        }
+
+        case 2:{
+            cout << "[px4ctrl_vision] Using Gazebo as input source." << endl;
+
+            nh.param<string>("Gazebo_topic", Gazebo_topic_, "/gazebo/ground_truth/odom");
+
+            // 【订阅】gazebo仿真真值
+            Gazebo_sub_ = nh.subscribe<nav_msgs::Odometry>
+                    (Gazebo_topic_, 100, Gazebo_cb);
+
+            break;
+        }
+
+        case 3:{
+            cout << "[px4ctrl_vision] Using T265 as input source." << endl;
+
+            nh.param<string>("T265_topic", T265_topic_, "/t265/odom/sample");
+
+            // 【订阅】t265估计位置
+            T265_sub_ = nh.subscribe<nav_msgs::Odometry>
+                    (T265_topic_, 100, T265_cb);
+
+            break;
+        }
+
+        case 4: {
+            cout << "[px4ctrl_vision] Using VICON as input source." << endl;
+
+            nh.param<string>("subject_name", subject_name, "p450");
+            nh.param<string>("segment_name", segment_name, "p450");
+
+            // VICON
+            VICON_sub_ = nh.subscribe<geometry_msgs::TransformStamped>
+                    ("/vicon/" + subject_name + "/" + segment_name, 1000, VICON_cb);
+
+            break;
+        }
+
+        case 5:{
+            cout << "[px4ctrl_vision] Using LIO as input source." << endl;
+
+            nh.param<string>("LIO_topic", LIO_topic_, "/Odometry");
+
+            // subscribe to odometry from LIO
+            LIO_sub_ = nh.subscribe<nav_msgs::Odometry>
+                    (LIO_topic_, 100, LIO_cb);
+
+            break;
+        }
+
+        default:
+            cout << "[px4ctrl_vision] Invalid input source! Exiting..." << endl;
+            return -1;
+    }
+
+    // 【发布】无人机位置和偏航角 坐标系 ENU系
+    // 本话题要发送飞控(通过mavros_extras/src/plugins/vision_pose_pose_estimate.cpp发送),
+    // 对应Mavlink消息为VISION_POSITION_ESTIMATE(#102),
+    // 对应的飞控中的uORB消息为vehicle_vision_pose_position.msg 及 vehicle_vision_pose_attitude.msg
+    vision_pose_pub_ = nh.advertise<geometry_msgs::PoseStamped>
+            ("/mavros/vision_pose/pose", 10);
+    odom_out_pub_ = nh.advertise<nav_msgs::Odometry>
+            ("/mavros/odometry/out", 10);
+
+    vision_pose_.header.stamp = ros::Time::now();
+    vision_pose_.header.frame_id = "world";
+
+    odom_out_.header.stamp = ros::Time::now();
+    odom_out_.header.frame_id = "world";
+
+    // frequency to publish
+    ros::Rate rate(50.0);
+
+    cout << "[px4ctrl_vision] estimator initialized! " << endl;
+
+    /****** Main Loop ******/
+    while (ros::ok()){
+        // 回调一次 更新传感器状态
+        ros::spinOnce();
+
+        // 将采集的机载设备的定位信息及偏航角信息发送至飞控
+        vision_pose_.header.stamp = ros::Time::now();
+        vision_pose_pub_.publish(vision_pose_);
+
+        rate.sleep();
+    }
+
+    return 0;
+}
diff --git a/src/px4ctrl_vision.cpp b/src/px4ctrl_vision.cpp