From 4b8d1a63ba9efb0c9327640e1e47b16d1cd4d5e9 Mon Sep 17 00:00:00 2001 From: Yuxiao Hua <1628280289@qq.com> Date: Thu, 7 Mar 2024 00:15:59 +0800 Subject: [PATCH] Update --- include/global_planner.h | 3 --- include/occupy_map.h | 7 +------ src/global_planner.cpp | 10 ---------- src/occupy_map.cpp | 38 ++++---------------------------------- 4 files changed, 5 insertions(+), 53 deletions(-) diff --git a/include/global_planner.h b/include/global_planner.h index 8af6ba3..0c7fdda 100644 --- a/include/global_planner.h +++ b/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; diff --git a/include/occupy_map.h b/include/occupy_map.h index d5c9267..f06f2d3 100644 --- a/include/occupy_map.h +++ b/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 Ptr; }; diff --git a/src/global_planner.cpp b/src/global_planner.cpp index 7f82869..2782194 100644 --- a/src/global_planner.cpp +++ b/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; - } } } diff --git a/src/occupy_map.cpp b/src/occupy_map.cpp index 8fa280b..bbef935 100644 --- a/src/occupy_map.cpp +++ b/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,9 +73,6 @@ void Occupy_map::inflate_point_cloud(void) // 发布未膨胀点云 global_pcl_pub.publish(*global_env_); - //记录开始时间 - ros::Time time_start = ros::Time::now(); - // 转化为PCL的格式进行处理 pcl::PointCloud latest_global_cloud_; pcl::fromROSMsg(*global_env_, latest_global_cloud_); @@ -83,15 +80,7 @@ void Occupy_map::inflate_point_cloud(void) if ((int)latest_global_cloud_.points.size() == 0) {return;} - pcl::PointCloud 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)