update global_planner,del detection code

Author: lanbaidiao

Modules/prometheus_case2_ugv/CMakeLists.txt Modules/global_planner_ugv/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 2.8.3)
-project(prometheus_case2_ugv)
+project(prometheus_global_planner_ugv)
 
 find_package(Eigen3 REQUIRED)
 find_package(PCL 1.7 REQUIRED)
@@ -39,16 +39,16 @@ add_library(occupy_map src/occupy_map.cpp)
 target_link_libraries(occupy_map ${PCL_LIBRARIES})
 add_library(A_star src/A_star.cpp)
 target_link_libraries(A_star occupy_map)
-add_library(case2_fsm_ugv src/case2_fsm_ugv.cpp)
-target_link_libraries(case2_fsm_ugv A_star)
-target_link_libraries(case2_fsm_ugv ${catkin_LIBRARIES})
-target_link_libraries(case2_fsm_ugv ${PCL_LIBRARIES})
-
-add_executable(case2_ugv_main src/case2_ugv_node.cpp)
-add_dependencies(case2_ugv_main prometheus_case2_ugv_gencpp)
-target_link_libraries(case2_ugv_main ${catkin_LIBRARIES})
-target_link_libraries(case2_ugv_main ${PCL_LIBRARIES})
-target_link_libraries(case2_ugv_main case2_fsm_ugv)
+add_library(global_planner_ugv src/global_planner_ugv.cpp)
+target_link_libraries(global_planner_ugv A_star)
+target_link_libraries(global_planner_ugv ${catkin_LIBRARIES})
+target_link_libraries(global_planner_ugv ${PCL_LIBRARIES})
+
+add_executable(global_planner_ugv_main src/global_planner_ugv_node.cpp)
+add_dependencies(global_planner_ugv_main prometheus_global_planner_ugv_gencpp)
+target_link_libraries(global_planner_ugv_main ${catkin_LIBRARIES})
+target_link_libraries(global_planner_ugv_main ${PCL_LIBRARIES})
+target_link_libraries(global_planner_ugv_main global_planner_ugv)
 
 
 ## Mark cpp header files for installation

Modules/prometheus_case2_ugv/config/advanced_param.xml Modules/global_planner_ugv/config/advanced_param.xml

@@ -17,7 +17,7 @@
 
 using namespace std;
 
-namespace prometheus_case2_ugv
+namespace global_planner_ugv
 {
 
 #define IN_CLOSE_SET 'a'

Modules/prometheus_case2_ugv/config/rviz_cxy_case2.rviz Modules/global_planner_ugv/config/rviz_cxy_case2.rviz

@@ -1,5 +1,5 @@
-#ifndef CASE2_FSM_UGV
-#define CASE2_FSM_UGV
+#ifndef GLOBAL_PLANNER_UGV
+#define GLOBAL_PLANNER_UGV
 
 #include <ros/ros.h>
 #include <boost/format.hpp>
@@ -23,7 +23,6 @@
 #include "prometheus_msgs/UGVCommand.h"
 #include "prometheus_msgs/ArucoInfo.h"
 #include "prometheus_msgs/Case2Result.h"
-#include "prometheus_msgs/Case3Result.h"
 #include "prometheus_msgs/StationCommand.h"
 #include "prometheus_msgs/MultiDetectionInfo.h"
 
@@ -38,10 +37,10 @@ using namespace std;
 
 #define MIN_DIS 0.1
 
-namespace prometheus_case2_ugv
+namespace global_planner_ugv
 {
 
-class Case2FSM_UGV
+class GlobalPlannerUGV
 {
 
 private:
@@ -59,24 +58,19 @@ class Case2FSM_UGV
     ros::Subscriber Gpointcloud_sub;
     ros::Subscriber Lpointcloud_sub;
     ros::Subscriber laserscan_sub;
-    // 【订阅】识别结果
-    ros::Subscriber detection_sub, detection_sub_real;
     // 【发布】控制指令
     ros::Publisher command_pub;
     // 【发布】规划路径
     ros::Publisher path_cmd_pub;
-    // 【发布】检测结果至其他无人车、无人车、地面站
-    ros::Publisher case2_result_pub;
     // 【定时器】主循环定时器、路径追踪定时器、目标追踪定时器
-    ros::Timer mainloop_timer, track_path_timer, object_tracking_timer, send_nei_odom_timer;
+    ros::Timer mainloop_timer, track_path_timer, send_nei_odom_timer;
     // 五种状态机
     enum EXEC_STATE
     {
       INIT,
       WAIT_GOAL,
       PLAN,
       PATH_TRACKING,
-      OBEJECT_TRACKING,
       RETURN,
       STOP
     };
@@ -85,7 +79,6 @@ class Case2FSM_UGV
     int swarm_num_ugv;     
     // 无人车名字                             
     string ugv_name;    
-    bool follow_target;
     float k_p,k_aoivd; 
     // 是否仿真模式
     bool sim_mode;    
@@ -153,36 +146,19 @@ class Case2FSM_UGV
     geometry_msgs::PoseStamped target_pos;
     // 上一条轨迹开始时间
     ros::Time tra_start_time;    
-
-    // 检测相关状态量
-    bool detected_by_myself;
-    bool detected_by_others;
-    
-    // 是否丢失目标
-    bool lost_object;
-    int num_count_vision_lost;
-    int num_count_vision_get;
-    // 目标位置
-    Eigen::Vector3d object_pos_body,object_pos_enu;
-    double dis_to_object;
-    // 检测范围
-    Eigen::Vector2d detection_range_x,detection_range_y;
 
     // 回调函数
     void goal_cb(const geometry_msgs::PoseStampedConstPtr& msg);
     void ugv_state_cb(const prometheus_msgs::UGVStateConstPtr &msg);
     void Gpointcloud_cb(const sensor_msgs::PointCloud2ConstPtr &msg);
     void Lpointcloud_cb(const sensor_msgs::PointCloud2ConstPtr &msg);
     void laser_cb(const sensor_msgs::LaserScanConstPtr &msg);
-    void detection_cb(const prometheus_msgs::ArucoInfoConstPtr &msg);
-    void detection_cb_real(const prometheus_msgs::MultiDetectionInfoConstPtr &msg);
     void cmd_cb(const prometheus_msgs::StationCommandConstPtr& msg);
     void nei_odom_cb(const nav_msgs::Odometry::ConstPtr& odom, int id);
 
     void send_nei_odom_cb(const ros::TimerEvent& e);
     void mainloop_cb(const ros::TimerEvent& e);
     void track_path_cb(const ros::TimerEvent& e);
-    void object_tracking_cb(const ros::TimerEvent& e);
 
     // 【获取当前时间函数】 单位：秒
     float get_time_in_sec(const ros::Time& begin_time);
@@ -195,10 +171,10 @@ class Case2FSM_UGV
 
 
 public:
-    Case2FSM_UGV(/* args */)
+    GlobalPlannerUGV(/* args */)
     {    
     }
-    ~Case2FSM_UGV()
+    ~GlobalPlannerUGV()
     {
     }
 

Modules/prometheus_case2_ugv/include/A_star.h Modules/global_planner_ugv/include/A_star.h

@@ -25,7 +25,7 @@
 #include "printf_utils.h"
 using namespace std;
 
-namespace prometheus_case2_ugv
+namespace global_planner_ugv
 {
 
 class Occupy_map

Modules/prometheus_case2_ugv/include/case2_fsm_ugv.h Modules/global_planner_ugv/include/global_planner_ugv.h

@@ -0,0 +1,39 @@
+<launch>
+	<arg name="swarm_num_ugv" default="1"/>
+	<!-- 参数 -->
+	<arg name="ugv_id" default="1"/>
+	<arg name="sim_mode" default="false"/>
+
+
+	<!-- 启动全局规划算法 -->
+    <node pkg="prometheus_global_planner_ugv" type="global_planner_ugv_main" name="global_planner_ugv_main_$(arg ugv_id)" output="screen">
+		<!-- 0 for 全局点云，1 for 局部点云，2 for laserscan  -->
+		<param name="global_planner_ugv/map_input_source" value="2" type="int"/>
+        <remap from="/ugv$(arg ugv_id)/prometheus/global_planner_ugv/global_pcl" to="/map_generator/global_cloud" />
+		<remap from="/ugv$(arg ugv_id)/prometheus/global_planner_ugv/local_pcl" to="/ugv$(arg ugv_id)/map_generator/local_cloud" />
+		<remap from="/ugv$(arg ugv_id)/prometheus/global_planner_ugv/laser_scan" to="/ugv$(arg ugv_id)/sensor/2Dlidar_scan" />
+		<param name="global_planner_ugv/swarm_num_ugv" value="$(arg swarm_num_ugv)"/>
+		<param name="global_planner_ugv/ugv_id" value="$(arg ugv_id)" />
+		<param name="global_planner_ugv/ugv_height" value="0.0" />
+		<param name="global_planner_ugv/sim_mode" value="$(arg sim_mode)" type="bool"/>
+		<!-- 手动给定目标点模式 -->
+		<param name="global_planner_ugv/manual_mode" value="true" type="bool"/>
+		<!-- 重规划间隔 -->
+		<param name="global_planner_ugv/replan_time" value="0.2" type="double"/>
+		<!-- path track间隔 -->
+		<param name="global_planner_ugv/track_frequency" value="0.1" type="double"/>
+		<!-- 地图参数 -->
+        <param name="map/origin_x" value="-20.0" type="double"/>
+        <param name="map/origin_y" value="-20.0" type="double"/>
+        <param name="map/map_size_x" value="40.0" type="double"/>
+        <param name="map/map_size_y" value="40.0" type="double"/>
+		<param name="map/queue_size" value="1" type="int"/>
+		<param name="map/border" value="true" type="bool"/>
+		<param name="map/resolution" value="0.2" type="double"/>
+		<param name="map/inflate" value="0.2" type="double"/>
+		<param name="global_planner_ugv/odom_inflate" value="0.3" type="double"/>
+		<!-- costmap -->
+		<param name="global_planner_ugv/cost_inflate" value="2" type="int"/>
+		<param name="astar/lambda_cost" value="10.0" type="double"/>
+    </node>
+</launch>

Modules/prometheus_case2_ugv/include/occupy_map.h Modules/global_planner_ugv/include/occupy_map.h

@@ -0,0 +1,3 @@
+<launch>
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find prometheus_global_planner_ugv)/launch/rviz_global_planner_ugv.rviz" required="true" />
+</launch>