feat: add visualisations to debug NO_PATH_FOUND #21
Conversation
Nisarg236
changed the title
| throw std::runtime_error("Start must be set before goal."); | ||
| } | ||
|
|
||
| const std::shared_ptr<costmap_2d::Costmap2DROS> costmap_2d_ros = _collision_checker->getCostmapROS(); |
There was a problem hiding this comment.
Q: can we use const here?
| const std::shared_ptr<costmap_2d::Costmap2DROS> costmap_2d_ros = _collision_checker->getCostmapROS(); | |
| const std::shared_ptr<const costmap_2d::Costmap2DROS> costmap_2d_ros = _collision_checker->getCostmapROS(); |
There was a problem hiding this comment.
This change is not required, I changed something here and then forgot to revert this.
But we can't change that to const because resetObstacleHeuristic doesnot accept that as parameter
| unsigned int map_x = static_cast<unsigned int>(this->pose.x + 0.5f); | ||
| unsigned int map_y = static_cast<unsigned int>(this->pose.y + 0.5f); | ||
| bool coordinates_in_bounds = (map_x < NodeHybrid::footprint_collision_map.info.width && | ||
| map_y < NodeHybrid::footprint_collision_map.info.height); | ||
|
|
||
| unsigned int index = map_y * NodeHybrid::footprint_collision_map.info.width + map_x; | ||
| bool index_valid = coordinates_in_bounds && (index < NodeHybrid::footprint_collision_map.data.size()); |
There was a problem hiding this comment.
Q: can't we use the costmap method here? (worldToMap, and in bounds check)
| if (index_valid) { | ||
| NodeHybrid::footprint_collision_map.data[index] = 100; // Mark as occupied | ||
| } |
There was a problem hiding this comment.
R: I think we should process this only if there is subscriber to the collision publisher
handle this in a cleaner way, i.e., do not add publisher checks all around
|
|
||
| void NodeHybrid::initializeFootprintCollisionMap(const std::shared_ptr<costmap_2d::Costmap2DROS>& costmap_ros) | ||
| { | ||
| const costmap_2d::Costmap2D* costmap = costmap_ros->getCostmap(); |
There was a problem hiding this comment.
Q/R: You are copying the whole global costmap here again? if yes, this might be very expensive in some cases (e.g. in large maps).
Isn't this the same problem we had before and you handle it by creating a new map with the desired size (the search space bounds), instead of using all the map?
AB#77603