From dba2ffa1a7ce878708f8f18fa0c1417824ded799 Mon Sep 17 00:00:00 2001 From: 7799 <2158192875@qq.com> Date: Wed, 14 Jun 2023 11:33:43 +0800 Subject: [PATCH] =?UTF-8?q?=E9=81=BF=E9=9A=9C=E5=8F=8AQt=E5=9B=BE=E5=83=8F?= =?UTF-8?q?=E7=AE=80=E5=8D=95=E5=A4=84=E7=90=86=E2=80=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/79xlx/A_star.cpp | 375 --------------------------------------- src/79xlx/Qt_0.cpp | 19 -- src/79xlx/Qt_1.cpp | 43 ----- src/79xlx/occupy_map.cpp | 292 ------------------------------ 4 files changed, 729 deletions(-) delete mode 100644 src/79xlx/A_star.cpp delete mode 100644 src/79xlx/Qt_0.cpp delete mode 100644 src/79xlx/Qt_1.cpp delete mode 100644 src/79xlx/occupy_map.cpp diff --git a/src/79xlx/A_star.cpp b/src/79xlx/A_star.cpp deleted file mode 100644 index 8a391af..0000000 --- a/src/79xlx/A_star.cpp +++ /dev/null @@ -1,375 +0,0 @@ -#include "A_star.h" - -using namespace std; -using namespace Eigen; - -namespace Global_Planning -{ - -Astar::~Astar() -{ - for (int i = 0; i < max_search_num; i++) - { - // delete表示释放堆内存 - delete path_node_pool_[i]; - } -} - -void Astar::init(ros::NodeHandle& nh) -{ - // 2d参数 - nh.param("global_planner/is_2D", is_2D, 0); // 1代表2D平面规划及搜索,0代表3D - nh.param("global_planner/2D_fly_height", fly_height, 1.5); // 2D规划时,定高高度 - // 规划搜索相关参数 - nh.param("astar/lambda_heu", lambda_heu_, 2.0); // 加速引导参数 - nh.param("astar/allocate_num", max_search_num, 100000); //最大搜索节点数 - // 地图参数 - nh.param("map/resolution", resolution_, 0.2); // 地图分辨率 - - tie_breaker_ = 1.0 + 1.0 / max_search_num; - - this->inv_resolution_ = 1.0 / resolution_; - - has_global_point = false; - path_node_pool_.resize(max_search_num); - - // 新建 - for (int i = 0; i < max_search_num; i++) - { - path_node_pool_[i] = new Node; - } - - use_node_num_ = 0; - iter_num_ = 0; - - // 初始化占据地图 - Occupy_map_ptr.reset(new Occupy_map); - Occupy_map_ptr->init(nh); - - // 读取地图参数 - origin_ = Occupy_map_ptr->min_range_; - map_size_3d_ = Occupy_map_ptr->max_range_ - Occupy_map_ptr->min_range_; -} - -void Astar::reset() -{ - // 重置与搜索相关的变量 - expanded_nodes_.clear(); - path_nodes_.clear(); - - std::priority_queue, NodeComparator0> empty_queue; - open_set_.swap(empty_queue); - - for (int i = 0; i < use_node_num_; i++) - { - NodePtr node = path_node_pool_[i]; - node->parent = NULL; - node->node_state = NOT_EXPAND; - } - - use_node_num_ = 0; - iter_num_ = 0; -} - -// 搜索函数，输入为：起始点及终点 -int Astar::search(Eigen::Vector3d start_pt, Eigen::Vector3d end_pt) -{ - // 首先检查目标点是否可到达 - if(Occupy_map_ptr->getOccupancy(end_pt)) - { - pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "Astar can't find path: goal point is occupied."); - return NO_PATH; - } - - // 计时 - ros::Time time_astar_start = ros::Time::now(); - - goal_pos = end_pt; - Eigen::Vector3i end_index = posToIndex(end_pt); - - // 初始化,将起始点设为第一个路径点 - NodePtr cur_node = path_node_pool_[0]; - cur_node->parent = NULL; - cur_node->position = start_pt; - cur_node->index = posToIndex(start_pt); - cur_node->g_score = 0.0; - cur_node->f_score = lambda_heu_ * getEuclHeu(cur_node->position, end_pt); - cur_node->node_state = IN_OPEN_SET; - - // 将当前点推入open set - open_set_.push(cur_node); - // 迭代次数+1 - use_node_num_ += 1; - // 记录当前为已扩展 - expanded_nodes_.insert(cur_node->index, cur_node); - - NodePtr terminate_node = NULL; - - // 搜索主循环 - while (!open_set_.empty()) - { - // 获取f_score最低的点 - cur_node = open_set_.top(); - - // 判断终止条件 - bool reach_end = abs(cur_node->index(0) - end_index(0)) <= 1 && - abs(cur_node->index(1) - end_index(1)) <= 1 && - abs(cur_node->index(2) - end_index(2)) <= 1; - - if (reach_end) - { - // 将当前点设为终止点，并往回形成路径 - terminate_node = cur_node; - retrievePath(terminate_node); - - // 时间一般很短，远远小于膨胀点云的时间 - printf("Astar take time %f s. \n", (ros::Time::now()-time_astar_start).toSec()); - - return REACH_END; - } - - /* ---------- pop node and add to close set ---------- */ - open_set_.pop(); - // 将当前点推入close set - cur_node->node_state = IN_CLOSE_SET; // in expand set - iter_num_ += 1; - - /* ---------- init neighbor expansion ---------- */ - Eigen::Vector3d cur_pos = cur_node->position; - Eigen::Vector3d expand_node_pos; - - vector inputs; - Eigen::Vector3d d_pos; - - /* ---------- expansion loop ---------- */ - // 扩展： 3*3*3 - 1 = 26种可能 - for (double dx = -resolution_; dx <= resolution_ + 1e-3; dx += resolution_) - { - for (double dy = -resolution_; dy <= resolution_ + 1e-3; dy += resolution_) - { - for (double dz = -resolution_; dz <= resolution_ + 1e-3; dz += resolution_) - { - - d_pos << dx, dy, dz; - // 对于2d情况，不扩展z轴 - if (is_2D == 1) - { - d_pos(2) = 0.0; - } - - // 跳过自己那个格子 - if (d_pos.norm() < 1e-3) - { - continue; - } - - // 扩展节点的位置 - expand_node_pos = cur_pos + d_pos; - - // 确认该点在地图范围内 - if(!Occupy_map_ptr->isInMap(expand_node_pos)) - { - continue; - } - - // 计算扩展节点的index - Eigen::Vector3i d_pos_id; - d_pos_id << int(dx/resolution_), int(dy/resolution_), int(dz/resolution_); - Eigen::Vector3i expand_node_id = d_pos_id + cur_node->index; - - //检查当前扩展的点是否在close set中，如果是则跳过 - NodePtr expand_node = expanded_nodes_.find(expand_node_id); - if (expand_node != NULL && expand_node->node_state == IN_CLOSE_SET) - { - continue; - } - - // 检查当前扩展点是否被占据,如果是则跳过 - bool is_occupy = Occupy_map_ptr->getOccupancy(expand_node_pos); - if (is_occupy) - { - continue; - } - - // 如果能通过上述检查则 - double tmp_g_score, tmp_f_score; - tmp_g_score = d_pos.squaredNorm() + cur_node->g_score; - tmp_f_score = tmp_g_score + lambda_heu_ * getEuclHeu(expand_node_pos, end_pt); - - // 如果扩展的当前节点为NULL，即未扩展过 - if (expand_node == NULL) - { - expand_node = path_node_pool_[use_node_num_]; - expand_node->index = expand_node_id; - expand_node->position = expand_node_pos; - expand_node->f_score = tmp_f_score; - expand_node->g_score = tmp_g_score; - expand_node->parent = cur_node; - expand_node->node_state = IN_OPEN_SET; - - open_set_.push(expand_node); - expanded_nodes_.insert(expand_node_id, expand_node); - - use_node_num_ += 1; - // 超过最大搜索次数 - if (use_node_num_ == max_search_num) - { - pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "Astar can't find path: reach the max_search_num.\n"); - return NO_PATH; - } - } - // 如果当前节点已被扩展过，则更新其状态 - else if (expand_node->node_state == IN_OPEN_SET) - { - if (tmp_g_score < expand_node->g_score) - { - // expand_node->index = expand_node_id; - expand_node->position = expand_node_pos; - expand_node->f_score = tmp_f_score; - expand_node->g_score = tmp_g_score; - expand_node->parent = cur_node; - } - } - } - } - } - - } - - // 搜索完所有可行点，即使没达到最大搜索次数，也没有找到路径 - // 这种一般是因为无人机周围被占据，或者无人机与目标点之间无可通行路径造成的 - pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "max_search_num: open set empty."); - return NO_PATH; -} - -// 由最终点往回生成路径 -void Astar::retrievePath(NodePtr end_node) -{ - NodePtr cur_node = end_node; - path_nodes_.push_back(cur_node); - - while (cur_node->parent != NULL) - { - cur_node = cur_node->parent; - path_nodes_.push_back(cur_node); - } - - // 反转顺序 - reverse(path_nodes_.begin(), path_nodes_.end()); - // 生成路径 -} - -std::vector Astar::getPath() -{ - vector path; - for (uint i = 0; i < path_nodes_.size(); ++i) - { - path.push_back(path_nodes_[i]->position); - } - path.push_back(goal_pos); - return path; -} - -nav_msgs::Path Astar::get_ros_path() -{ - nav_msgs::Path path; - - path.header.frame_id = "world"; - path.header.stamp = ros::Time::now(); - path.poses.clear(); - - geometry_msgs::PoseStamped path_i_pose; - for (uint i=0; iposition[0]; - path_i_pose.pose.position.y = path_nodes_[i]->position[1]; - path_i_pose.pose.position.z = path_nodes_[i]->position[2]; - path.poses.push_back(path_i_pose); - } - - path_i_pose .header.frame_id = "world"; - path_i_pose.pose.position.x = goal_pos[0]; - path_i_pose.pose.position.y = goal_pos[1]; - path_i_pose.pose.position.z = goal_pos[2]; - path.poses.push_back(path_i_pose); - - return path; -} - -double Astar::getDiagHeu(Eigen::Vector3d x1, Eigen::Vector3d x2) -{ - double dx = fabs(x1(0) - x2(0)); - double dy = fabs(x1(1) - x2(1)); - double dz = fabs(x1(2) - x2(2)); - - double h = 0; - - int diag = min(min(dx, dy), dz); - dx -= diag; - dy -= diag; - dz -= diag; - - if (dx < 1e-4) - { - h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dy, dz) + 1.0 * abs(dy - dz); - } - if (dy < 1e-4) - { - h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dx, dz) + 1.0 * abs(dx - dz); - } - if (dz < 1e-4) - { - h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dx, dy) + 1.0 * abs(dx - dy); - } - - return tie_breaker_ * h; -} - - -double Astar::getManhHeu(Eigen::Vector3d x1, Eigen::Vector3d x2) -{ - double dx = fabs(x1(0) - x2(0)); - double dy = fabs(x1(1) - x2(1)); - double dz = fabs(x1(2) - x2(2)); - - return tie_breaker_ * (dx + dy + dz); -} - -double Astar::getEuclHeu(Eigen::Vector3d x1, Eigen::Vector3d x2) -{ - return tie_breaker_ * (x2 - x1).norm(); -} - -std::vector Astar::getVisitedNodes() -{ - vector visited; - visited.assign(path_node_pool_.begin(), path_node_pool_.begin() + use_node_num_ - 1); - return visited; -} - -Eigen::Vector3i Astar::posToIndex(Eigen::Vector3d pt) -{ - Vector3i idx ; - idx << floor((pt(0) - origin_(0)) * inv_resolution_), floor((pt(1) - origin_(1)) * inv_resolution_), - floor((pt(2) - origin_(2)) * inv_resolution_); - - return idx; -} - -void Astar::indexToPos(Eigen::Vector3i id, Eigen::Vector3d &pos) -{ - for (int i = 0; i < 3; ++i) - pos(i) = (id(i) + 0.5) * resolution_ + origin_(i); -} - -// 检查cur_pos是否安全 -bool Astar::check_safety(Eigen::Vector3d &cur_pos, double safe_distance) -{ - bool is_safety; - is_safety = Occupy_map_ptr->check_safety(cur_pos, safe_distance); - return is_safety; -} - - -} \ No newline at end of file diff --git a/src/79xlx/Qt_0.cpp b/src/79xlx/Qt_0.cpp deleted file mode 100644 index 6b5dcca..0000000 --- a/src/79xlx/Qt_0.cpp +++ /dev/null @@ -1,19 +0,0 @@ -QPixmap pixmap("image.png"); // 加载图像 -QPainter painter(&pixmap); // 创建绘图对象 - -// 指定矩形范围（这里以左上角为起点，宽高为100像素的矩形为例） -int x = 0; -int y = 0; -int width = 100; -int height = 100; - -// 创建颜色对象（这里使用Qt中预定义的红色） -QColor color(Qt::red); - -// 填充指定范围内的像素 -painter.fillRect(x, y, width, height, color); - -// 显示图像 -QLabel label; -label.setPixmap(pixmap); -label.show(); diff --git a/src/79xlx/Qt_1.cpp b/src/79xlx/Qt_1.cpp deleted file mode 100644 index fc4c686..0000000 --- a/src/79xlx/Qt_1.cpp +++ /dev/null @@ -1,43 +0,0 @@ -QPixmap pixmap("image.png"); // 加载图像 -QPainter painter(&pixmap); // 创建绘图对象 - -// 指定矩形范围（这里以左上角为起点，宽高为100像素的矩形为例） -int x = 0; -int y = 0; -int width = 100; -int height = 100; - -// 创建画笔对象（这里使用Qt中预定义的红色） -QPen pen(Qt::red); -pen.setWidth(2); // 设置边框宽度 - -// 绘制矩形框 -painter.setPen(pen); -painter.drawRect(x, y, width, height); - -// 显示图像 -QLabel label; -label.setPixmap(pixmap); -label.show(); - - - - -// 指定矩形范围（这里以左上角为起点，宽高为100像素的矩形为例） -int x = 0; -int y = 0; -int width = 100; -int height = 100; - -// 获取像素大小 -QSize size = pixmap.size(); // 获取图像尺寸 -double pixelSizeX = 1.0 / size.width(); // 计算x方向每个像素在实际场景中所代表的长度 -double pixelSizeY = 1.0 / size.height(); // 计算y方向每个像素在实际场景中所代表的长度 - -// 计算实际场景中的长度 -int pixelCount = width * height; // 计算指定范围内的像素数量 -double lengthX = pixelCount * pixelSizeX; // 计算x方向实际场景中的长度 -double lengthY = pixelCount * pixelSizeY; // 计算y方向实际场景中的长度 - -qDebug() << "实际场景中的长度（x方向）：" << lengthX; -qDebug() << "实际场景中的长度（y方向）：" << lengthY; diff --git a/src/79xlx/occupy_map.cpp b/src/79xlx/occupy_map.cpp deleted file mode 100644 index ec86045..0000000 --- a/src/79xlx/occupy_map.cpp +++ /dev/null @@ -1,292 +0,0 @@ -#include - -namespace Global_Planning -{ -// 初始化函数 -void Occupy_map::init(ros::NodeHandle& nh) -{ - // TRUE代表2D平面规划及搜索,FALSE代表3D - nh.param("global_planner/is_2D", is_2D, true); - // 2D规划时,定高高度 - nh.param("global_planner/fly_height_2D", fly_height_2D, 1.0); - // 地图原点 - nh.param("map/origin_x", origin_(0), -5.0); - nh.param("map/origin_y", origin_(1), -5.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/resolution", resolution_, 0.2); - // 地图膨胀距离，单位：米 - nh.param("map/inflate", inflate_, 0.3); - - // 发布 地图rviz显示 - global_pcl_pub = nh.advertise("/prometheus/planning/global_pcl", 10); - // 发布膨胀后的点云 - inflate_pcl_pub = nh.advertise("/prometheus/planning/global_inflate_pcl", 1); - - // 发布二维占据图？ - // 发布膨胀后的二维占据图？ - - this->inv_resolution_ = 1.0 / resolution_; - for (int i = 0; i < 3; ++i) - { - // 占据图尺寸 = 地图尺寸 / 分辨率 - grid_size_(i) = ceil(map_size_3d_(i) / resolution_); - } - - // 占据容器的大小 = 占据图尺寸 x*y*z - occupancy_buffer_.resize(grid_size_(0) * grid_size_(1) * grid_size_(2)); - fill(occupancy_buffer_.begin(), occupancy_buffer_.end(), 0.0); - - min_range_ = origin_; - max_range_ = origin_ + map_size_3d_; - - // 对于二维情况，重新限制点云高度 - if(is_2D == true) - { - min_range_(2) = fly_height_2D - resolution_; - max_range_(2) = fly_height_2D + resolution_; - } -} - -// 地图更新函数 - 输入：全局点云 -void Occupy_map::map_update_gpcl(const sensor_msgs::PointCloud2ConstPtr & global_point) -{ - has_global_point = true; - global_env_ = global_point; -} - -// 地图更新函数 - 输入：局部点云 -void Occupy_map::map_update_lpcl(const sensor_msgs::PointCloud2ConstPtr & local_point, const nav_msgs::Odometry & odom) -{ - has_global_point = true; -// 将传递过来的局部点云转为全局点云 -} - -// 地图更新函数 - 输入：laser -void Occupy_map::map_update_laser(const sensor_msgs::LaserScanConstPtr & local_point, const nav_msgs::Odometry & odom) -{ - has_global_point = true; -// 将传递过来的数据转为全局点云 -} - -// 当global_planning节点接收到点云消息更新时，进行设置点云指针并膨胀 -// Astar规划路径时，采用的是此处膨胀后的点云（setOccupancy只在本函数中使用） -void Occupy_map::inflate_point_cloud(void) -{ - if(!has_global_point) - { - pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "Occupy_map [inflate point cloud]: don't have global point, can't inflate!\n"); - return; - } - - // 发布未膨胀点云 - 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_); - - //printf("time 1 take %f [s].\n", (ros::Time::now()-time_start).toSec()); - - 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; - - // 遍历全局点云中的所有点 - for (size_t i = 0; i < latest_global_cloud_.points.size(); ++i) - { - // 取出第i个点 - p3d(0) = latest_global_cloud_.points[i].x; - p3d(1) = latest_global_cloud_.points[i].y; - p3d(2) = latest_global_cloud_.points[i].z; - - // 若取出的点不在地图内（膨胀时只考虑地图范围内的点） - if(!isInMap(p3d)) - { - continue; - } - - // 根据膨胀距离，膨胀该点 - for (int x = -ifn; x <= ifn; ++x) - for (int y = -ifn; y <= ifn; ++y) - for (int z = -ifn; z <= ifn; ++z) - { - // 为什么Z轴膨胀一半呢？ z 轴其实可以不膨胀 - p3d_inf(0) = pt_inf.x = p3d(0) + x * resolution_; - p3d_inf(1) = pt_inf.y = p3d(1) + y * resolution_; - p3d_inf(2) = pt_inf.z = p3d(2) + 0.5 * z * resolution_; - - // 若膨胀的点不在地图内（膨胀时只考虑地图范围内的点） - if(!isInMap(p3d_inf)) - { - continue; - } - - cloud_inflate_vis_.push_back(pt_inf); - - // 设置膨胀后的点被占据 - this->setOccupancy(p3d_inf, 1); - } - } - - 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) -{ - if (occ != 1 && occ != 0) - { - pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "occ value error!\n"); - return; - } - - if (!isInMap(pos)) - { - return; - } - - Eigen::Vector3i id; - posToIndex(pos, id); - - // 设置为占据/不占据 索引是如何索引的？ [三维地图 变 二维数组] - // 假设10*10*10米，分辨率1米，buffer大小为 1000 （即每一个占格都对应一个buffer索引） - // [0.1,0.1,0.1] 对应索引为[0,0,0]， buffer索引为 0 - // [9.9,9.9,9.9] 对应索引为[9,9,9]， buffer索引为 900+90+9 = 999 - occupancy_buffer_[id(0) * grid_size_(1) * grid_size_(2) + id(1) * grid_size_(2) + id(2)] = occ; -} - -bool Occupy_map::isInMap(Eigen::Vector3d pos) -{ - // min_range就是原点，max_range就是原点+地图尺寸 - // 比如设置0,0,0为原点，[0,0,0]点会被判断为不在地图里 - //　同时　对于２Ｄ情况，超出飞行高度的数据也会认为不在地图内部 - if (pos(0) < min_range_(0) + 1e-4 || pos(1) < min_range_(1) + 1e-4 || pos(2) < min_range_(2) + 1e-4) - { - return false; - } - - if (pos(0) > max_range_(0) - 1e-4 || pos(1) > max_range_(1) - 1e-4 || pos(2) > max_range_(2) - 1e-4) - { - return false; - } - - return true; -} - -bool Occupy_map::check_safety(Eigen::Vector3d& pos, double check_distance) -{ - if(!isInMap(pos)) - { - // 当前位置点不在地图内 - pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "[check_safety]: the odom point is not in map\n"); - return 0; - } - Eigen::Vector3i id; - posToIndex(pos, id); - Eigen::Vector3i id_occ; - Eigen::Vector3d pos_occ; - - int check_dist_xy = int(check_distance/resolution_); - int check_dist_z=0; - int cnt=0; - for(int ix=-check_dist_xy; ix<=check_dist_xy; ix++){ - for(int iy=-check_dist_xy; iy<=check_dist_xy; iy++){ - for(int iz=-check_dist_z; iz<=check_dist_z; iz++){ - id_occ(0) = id(0)+ix; - id_occ(1) = id(1)+iy; - id_occ(2) = id(2)+iz; - indexToPos(id_occ, pos_occ); - if(!isInMap(pos_occ)){ - // printf("[check_safety]: current odom is near the boundary of the map\n"); - // pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, "[check_safety]: current odom is near the boundary of the map\n"); - return 0; - } - if(getOccupancy(id_occ)){ - // printf("[check_safety]: current state is dagerous, the pos [%d, %d, %d], is occupied\n", ix, iy, iz); - cnt++; - } - } - } - } - if(cnt>5){ - return 0; - } - return 1; - -} - -void Occupy_map::posToIndex(Eigen::Vector3d pos, Eigen::Vector3i &id) -{ - for (int i = 0; i < 3; ++i) - { - id(i) = floor((pos(i) - origin_(i)) * inv_resolution_); - } - -} - -void Occupy_map::indexToPos(Eigen::Vector3i id, Eigen::Vector3d &pos) -{ - for (int i = 0; i < 3; ++i) - { - pos(i) = (id(i) + 0.5) * resolution_ + origin_(i); - } -} - -int Occupy_map::getOccupancy(Eigen::Vector3d pos) -{ - if (!isInMap(pos)) - { - return -1; - } - - Eigen::Vector3i id; - posToIndex(pos, id); - - return occupancy_buffer_[id(0) * grid_size_(1) * grid_size_(2) + id(1) * grid_size_(2) + id(2)]; -} - -int Occupy_map::getOccupancy(Eigen::Vector3i id) -{ - if (id(0) < 0 || id(0) >= grid_size_(0) || id(1) < 0 || id(1) >= grid_size_(1) || id(2) < 0 || - id(2) >= grid_size_(2)) - { - return -1; - } - - return occupancy_buffer_[id(0) * grid_size_(1) * grid_size_(2) + id(1) * grid_size_(2) + id(2)]; -} -}