Update

include/global_planner.h

@@ -50,7 +50,6 @@ class Global_Planner
     // 根据不同的输入（激光雷达输入、相机输入等）生成occupymap
 
     // 订阅无人机状态、目标点、传感器数据（生成地图）
-    ros::Subscriber goal_sub;
     ros::Subscriber drone_state_sub;
     // 支持2维激光雷达、3维激光雷达、D435i等实体传感器
     // 支持直接输入全局已知点云
@@ -86,9 +85,7 @@ class Global_Planner
     enum EXEC_STATE
     {
         WAIT_GOAL,
-        PLANNING,
         TRACKING,
-        LANDING,
     };
     EXEC_STATE exec_state;
 

include/occupy_map.h

@@ -41,7 +41,6 @@ class Occupy_map
         // 地图分辨率
         double resolution_, inv_resolution_;
         // 膨胀参数
-        double inflate_;
         bool debug_mode;
         // 地图原点,地图尺寸
         Eigen::Vector3d origin_, map_size_3d_, min_range_, max_range_;
@@ -54,7 +53,7 @@ class Occupy_map
         void show_gpcl_marker(visualization_msgs::Marker &m, int id, Eigen::Vector4d color);
 
         // 发布点云用于rviz显示
-        ros::Publisher global_pcl_pub, inflate_pcl_pub;
+        ros::Publisher global_pcl_pub;
 
         //初始化
         void init(ros::NodeHandle& nh);
@@ -64,8 +63,6 @@ class Occupy_map
         void map_update_lpcl(const sensor_msgs::PointCloud2ConstPtr & local_point, const nav_msgs::Odometry & odom);
         // 地图更新函数 - 输入：二维激光雷达
         void map_update_laser(const sensor_msgs::LaserScanConstPtr & local_point, const nav_msgs::Odometry & odom);
-        // 地图膨胀
-        void inflate_point_cloud(void);
         // 判断当前点是否在地图内
         bool isInMap(Eigen::Vector3d pos);
         // 设置占据
@@ -78,8 +75,6 @@ class Occupy_map
         int getOccupancy(Eigen::Vector3d pos);
         // 根据索引返回占据状态
         int getOccupancy(Eigen::Vector3i id);
-        // 检查安全
-        bool check_safety(Eigen::Vector3d& pos, double check_distance/*, Eigen::Vector3d& map_point*/);
         // 定义该类的指针
         typedef std::shared_ptr<Occupy_map> Ptr;
 };

src/global_planner.cpp

@@ -203,16 +203,6 @@ void Global_Planner::mainloop_cb(const ros::TimerEvent& e)
 
             break;
         }
-        case LANDING:
-        {
-            Command_Now.header.stamp = ros::Time::now();
-            Command_Now.Mode         = prometheus_msgs::ControlCommand::Land;
-            Command_Now.Command_ID   = Command_Now.Command_ID + 1;
-            Command_Now.source = NODE_NAME;
-
-            command_pub.publish(Command_Now);
-            break;
-        }
     }
 }
 

src/occupy_map.cpp

@@ -10,9 +10,9 @@ void Occupy_map::init(ros::NodeHandle& nh)
     nh.param("map/origin_y", origin_(1), 0.0);
     nh.param("map/origin_z", origin_(2), 0.0);
     // 地图实际尺寸，单位：米
-    nh.param("map/map_size_x", map_size_3d_(0), 10.0);
-    nh.param("map/map_size_y", map_size_3d_(1), 10.0);
-    nh.param("map/map_size_z", map_size_3d_(2), 5.0);
+    nh.param("map/map_size_x", map_size_3d_(0), 6.0);
+    nh.param("map/map_size_y", map_size_3d_(1), 6.0);
+    nh.param("map/map_size_z", map_size_3d_(2), 3.0);
     // 地图分辨率，单位：米
     nh.param("map/resolution", resolution_,  0.05);
 
@@ -73,25 +73,14 @@ void Occupy_map::inflate_point_cloud(void)
     // 发布未膨胀点云
     global_pcl_pub.publish(*global_env_);
 
-    //记录开始时间
-    ros::Time time_start = ros::Time::now();
-
     // 转化为PCL的格式进行处理
     pcl::PointCloud<pcl::PointXYZ> latest_global_cloud_;
     pcl::fromROSMsg(*global_env_, latest_global_cloud_);
 
     if ((int)latest_global_cloud_.points.size() == 0)  
     {return;}
 
-    pcl::PointCloud<pcl::PointXYZ> cloud_inflate_vis_;
-    cloud_inflate_vis_.clear();
-
-    // 膨胀格子数 = 膨胀距离/分辨率
-    // ceil返回大于或者等于指定表达式的最小整数
-    const int ifn = ceil(inflate_ * inv_resolution_);
-
-    pcl::PointXYZ pt_inf;
-    Eigen::Vector3d p3d, p3d_inf;
+    Eigen::Vector3d p3d;
 
     // 遍历全局点云中的所有点
     for (size_t i = 0; i < latest_global_cloud_.points.size(); ++i) 
@@ -108,25 +97,6 @@ void Occupy_map::inflate_point_cloud(void)
         }
     }
 
-    cloud_inflate_vis_.header.frame_id = "world";
-
-    // 转化为ros msg发布
-    sensor_msgs::PointCloud2 map_inflate_vis;
-    pcl::toROSMsg(cloud_inflate_vis_, map_inflate_vis);
-
-    inflate_pcl_pub.publish(map_inflate_vis);
-
-    static int exec_num=0;
-    exec_num++;
-
-    // 此处改为根据循环时间计算的数值
-    if(exec_num == 20)
-    {
-        // 膨胀地图效率与地图大小有关（有点久，Astar更新频率是多久呢？ 怎么才能提高膨胀效率呢？）
-        printf("inflate global point take %f [s].\n",   (ros::Time::now()-time_start).toSec());
-        exec_num=0;
-    }  
-
 }
 
 void Occupy_map::setOccupancy(Eigen::Vector3d pos, int occ)