Skip to content

代码问题请教 #21

@roseupram

Description

@roseupram

非常感谢作者的工作,我在运行其他仿真场景时代码一直出现下面的问题

[ INFO] [1740363994.144297827, 205.782000000]: **Planning iteration  193**
[ INFO] [1740363994.144329168, 205.782000000]: call for the path to explore..
[ WARN] [1740363994.220186055, 205.858000000]: tourpoints is empty, planning finish..
[ INFO] [1740363994.220411738, 205.858000000]: the path get.
[ WARN] [1740363994.220465119, 205.858000000]: this iteration get an empty path ,need next planning iteration
[ INFO] [1740363994.220484993, 205.858000000]: **Planning iteration  194**
[ INFO] [1740363994.220506291, 205.858000000]: call for the path to explore..
[ WARN] [1740363994.262083570, 205.900000000]: tourpoints is empty, planning finish..

debug看到检测frontier的代码一直没有进入 if (isFrontier(changedCellCode)) 这里,请问是否可以提供一点查找问题的思路

    void Ufomap::findPlaneLocalFrontier() {
        local_frontier_cells_.clear();
        for (const auto &changedCellCode: changed_cell_codes_) {
            if (frontier_depth_ == changedCellCode.getDepth() && history_frontier_cells_.count(changedCellCode)==0) {  
                ufo::map::Point3 point = map_.toCoord(changedCellCode.toKey());
                if (isInExplorationArea(point.x(),point.y()) &&
                    point.z() > current_robot_pose_.z() + robot_bottom_ &&
                    point.z() < current_robot_pose_.z() + robot_height_) {
                    odom_mutex_.lock();
                    ufo::math::Vector3 current = current_robot_pose_.translation();
                    odom_mutex_.unlock();
                    if (point.distanceXY(current) > 0.6) {    
                        if (isFrontier(changedCellCode)) {
                            local_frontier_cells_.insert(changedCellCode);
                            history_frontier_cells_.insert(changedCellCode);
                        }
                    }
                }

            }
        }
    }

这是rviz里的样子
Image

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions