ugv_estimetor add gps location_sourcce

kenny67 · Nov 23, 2023 · 8d5207d · 8d5207d
1 parent 9968f57
commit 8d5207d
Show file tree

Hide file tree

Showing 8 changed files with 71 additions and 30 deletions.
diff --git a/Modules/swarm_control b/Modules/swarm_control
diff --git a/Modules/ugv_control/include/ugv_controller.h b/Modules/ugv_control/include/ugv_controller.h
@@ -98,7 +98,6 @@ class UGV_controller
         Test = 5
     };
 
-
 private:
     void ugv_command_cb(const prometheus_msgs::UGVCommand::ConstPtr& msg);
     void ugv_state_cb(const prometheus_msgs::UGVState::ConstPtr& msg);

diff --git a/Modules/ugv_control/include/ugv_estimator.h b/Modules/ugv_control/include/ugv_estimator.h
@@ -40,6 +40,7 @@ class UGV_estimator
     int ugv_id;                                           // 无人车编号
     string ugv_name;                            // 无人车名字(话题前缀)
     bool sim_mode;
+    int location_source;     //无人车定位来源
     prometheus_msgs::UGVState ugv_state;    // 无人车状态
     prometheus_msgs::UGVCommand ugv_command;
     nav_msgs::Odometry ugv_odom;                // 无人车odom
@@ -62,28 +63,27 @@ class UGV_estimator
     ros::Subscriber mocap_pos_sub;
     ros::Subscriber mocap_vel_sub;
     ros::Subscriber battery_sub;
+    ros::Subscriber gps_pos_sub;
 
     // 发布话题
     ros::Publisher ugv_state_pub;
     ros::Publisher ugv_odom_pub;
     ros::Publisher ugv_mesh_pub;
     ros::Publisher trajectory_pub;
     ros::Publisher mocap_matlab_pub;
-    
-    enum MatlabUGVState
+
+    enum UGVLocationSource
     {
-        HOLD = 0,
-        Direct_Control_BODY = 1,
-        Direct_Control_ENU = 2,
-        Point_Control = 3,
-        Path_Control = 4,
-        Test = 5
+        MOCAP = 0,
+        GPS = 1
     };
 
 
+
 private:
     void mocap_pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg);
     void mocap_vel_cb(const geometry_msgs::TwistStamped::ConstPtr &msg);
+    void gps_pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg);
     void battery_cb(const std_msgs::Float32::ConstPtr &msg);
     void gazebo_cb(const nav_msgs::Odometry::ConstPtr &msg);
     float get_time_in_sec(const ros::Time& begin_time);

diff --git a/Modules/ugv_control/launch/ugv_control_lingao.yaml b/Modules/ugv_control/launch/ugv_control_lingao.yaml
@@ -8,10 +8,10 @@ ugv_control:
   k_i : 0.01
   d_t : 0.01  
 ##  跑圆参数
-  circle_radius : 5.0
-  linear_vel : 1.0
+circle_radius : 1.5
+linear_vel : 1.0
 ##  最大速度
-  max_vel : 2.0
+max_vel : 2.0
 
 ##　地理围栏
 geo_fence:

diff --git a/Modules/ugv_control/launch/ugv_control_main.launch b/Modules/ugv_control/launch/ugv_control_main.launch
@@ -1,13 +1,16 @@
 <launch>
 	<arg name="ugv_id" default="1"/>
     <arg name="sim_mode" default="false"/>
+    <!--无人车定位源 0 mocap 1 gps -->
+    <arg name="location_source" default="0"/>
     <arg name="flag_printf" default="true"/>
     <arg name="ugv_control/enable_external_control" value="true" />
 
     <!-- 启动ugv_control_main -->
 	<node pkg="prometheus_ugv_control" type="ugv_control_main" name="ugv_control_main_$(arg ugv_id)" output="screen">
 		<param name="ugv_id" value="$(arg ugv_id)" />
         <param name="sim_mode" value="$(arg sim_mode)" />
+        <param name="location_source" value="$(arg location_source)" />
 		<param name="flag_printf" value="$(arg flag_printf)" />
 		<param name="ugv_control/enable_external_control" value="$(arg ugv_control/enable_external_control)" />
 		<rosparam command="load" file="$(find prometheus_ugv_control)/launch/ugv_control_lingao.yaml" />

diff --git a/Modules/ugv_control/launch/ugv_control_wheel.yaml b/Modules/ugv_control/launch/ugv_control_wheel.yaml
@@ -9,10 +9,10 @@ ugv_control:
   d_t : 0.01
 
 ##  跑圆参数
-  circle_radius : 5.0
-  linear_vel : 1.0
+circle_radius : 1.5
+linear_vel : 1.0
 ##  最大速度
-  max_vel : 1.0
+max_vel : 0.5
 
 ##　地理围栏
 geo_fence:

diff --git a/Modules/ugv_control/src/ugv_controller.cpp b/Modules/ugv_control/src/ugv_controller.cpp
@@ -215,6 +215,8 @@ void UGV_controller::mainloop()
                 this->cmd_vel.angular.x = 0.0;
                 this->cmd_vel.angular.y = 0.0;
                 this->cmd_vel.angular.z = this->k_yaw*this->error_yaw;
+
+                VelLimit();
             }else
             {
                 // 先调整yaw
@@ -225,8 +227,6 @@ void UGV_controller::mainloop()
                 this->cmd_vel.angular.y = 0.0;
                 this->cmd_vel.angular.z = this->k_yaw*this->error_yaw;
             }
-
-            VelLimit();
             ReachTargetPoint();
             this->cmd_pub.publish(this->cmd_vel);
             break;
@@ -243,7 +243,7 @@ void UGV_controller::mainloop()
                 enu_x = this->k_p_path*(this->Command_Now.position_ref[0] - this->pos_ugv[0]);
                 enu_y = this->k_p_path*(this->Command_Now.position_ref[1] - this->pos_ugv[1]);
                 // cal vel_avoid_nei
-                //add_apf_vel();
+                add_apf_vel();
                 enu_x = enu_x + this->vel_avoid_nei[0];
                 enu_y = enu_y + this->vel_avoid_nei[1];
                 float body_x, body_y;
@@ -268,6 +268,7 @@ void UGV_controller::mainloop()
             }
 
             VelLimit();
+            ReachTargetPoint();
             this->cmd_pub.publish(this->cmd_vel);
 
             break;
@@ -306,7 +307,6 @@ void UGV_controller::mainloop()
             }
 
             VelLimit();
-
             this->cmd_pub.publish(this->cmd_vel);       
             test_time = test_time + 0.05;       //20Hz
             break;

diff --git a/Modules/ugv_control/src/ugv_estimator.cpp b/Modules/ugv_control/src/ugv_estimator.cpp
@@ -4,10 +4,10 @@
 UGV_estimator::UGV_estimator(ros::NodeHandle& nh)
 {
      // 读取参数
-    nh.param("ugv_id", this->ugv_id, 0);
-    nh.param("sim_mode", this->sim_mode, false);
-    nh.param("mesh_resource", this->mesh_resource, std::string("package://prometheus_ugv_control/meshes/car.dae"));
-
+    nh.param<int>("ugv_id", this->ugv_id, 0);
+    nh.param<bool>("sim_mode", this->sim_mode, false);
+    nh.param<string>("mesh_resource", this->mesh_resource, std::string("package://prometheus_ugv_control/meshes/car.dae"));
+    nh.param<int>("location_source", this->location_source, UGVLocationSource::MOCAP);
 
     this->ugv_name = "/ugv" + std::to_string(this->ugv_id);
 
@@ -17,12 +17,21 @@ UGV_estimator::UGV_estimator(ros::NodeHandle& nh)
         this->gazebo_odom_sub = nh.subscribe<nav_msgs::Odometry>(this->ugv_name + "/odom", 1, &UGV_estimator::gazebo_cb, this);
     }else
     {
-        // 【订阅】mocap估计位置
-        this->mocap_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>("/vrpn_client_node"+ this->ugv_name + "/pose", 1, &UGV_estimator::mocap_pos_cb, this);
-
-        // 【订阅】mocap估计速度
-        this->mocap_vel_sub = nh.subscribe<geometry_msgs::TwistStamped>("/vrpn_client_node"+ this->ugv_name + "/twist", 1, &UGV_estimator::mocap_vel_cb, this);
-
+        if (this->location_source == UGVLocationSource::MOCAP)
+        {
+            // 【订阅】mocap估计位置
+            this->mocap_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>("/vrpn_client_node"+ this->ugv_name + "/pose", 1, &UGV_estimator::mocap_pos_cb, this);
+
+            // 【订阅】mocap估计速度
+            this->mocap_vel_sub = nh.subscribe<geometry_msgs::TwistStamped>("/vrpn_client_node"+ this->ugv_name + "/twist", 1, &UGV_estimator::mocap_vel_cb, this);
+
+        }
+        else if (this->location_source == UGVLocationSource::GPS)
+        {
+            // 【订阅】飞控本地位置数据
+            this->gps_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>(ugv_name + "/mavros/local_position/pose", 1, &UGV_estimator::gps_pos_cb, this);
+        }
+
         // 【订阅】电池状态(无人车底板电压)
         this->battery_sub = nh.subscribe<std_msgs::Float32>(this->ugv_name + "/PowerVoltage", 1, &UGV_estimator::battery_cb, this);
 
@@ -119,6 +128,36 @@ void UGV_estimator::mocap_pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg
 
     this->mocap_matlab_pub.publish(pose_msg);
 }
+void UGV_estimator::gps_pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
+{
+    // ugv_state赋值 - 位置
+    this->ugv_state.position[0] = msg->pose.position.x;
+    this->ugv_state.position[1] = msg->pose.position.y;
+    this->ugv_state.position[2] = msg->pose.position.z;
+
+    //计算速度
+    // now_time = get_time_in_sec(ros::Time::now());
+    // dt = now_time - last_time;
+    this->dt  = 0.01;
+    this->ugv_state.velocity[0] = (ugv_state.position[0] - last_position_x) / dt;
+    this->ugv_state.velocity[1] = (ugv_state.position[1] - last_position_y) / dt;
+    this->ugv_state.velocity[2] = (ugv_state.position[2] - last_position_z) / dt;
+
+    //保存当前信息为上一时刻
+    this->last_position_x = this->ugv_state.position[0];
+    this->last_position_y = this->ugv_state.position[1];
+    this->last_position_z = this->ugv_state.position[2];
+    // last_time = now_time;
+
+    // ugv_state赋值 - 四元数
+    this->ugv_state.attitude_q = msg->pose.orientation;
+    // ugv_state赋值 - 欧拉角
+    Eigen::Quaterniond q_gps = Eigen::Quaterniond(msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z);
+    Eigen::Vector3d euler_gps = quaternion_to_euler(q_gps);
+    this->ugv_state.attitude[0] = euler_gps[0];
+    this->ugv_state.attitude[1] = euler_gps[1];
+    this->ugv_state.attitude[2] = euler_gps[2];
+}
 
 void UGV_estimator::mocap_vel_cb(const geometry_msgs::TwistStamped::ConstPtr &msg)
 {