Class AStarSearcher

Inheritance Relationships

Base Type

Class Documentation

class AStarSearcher : public GraphSearcher<cv::Mat>

Public Functions

nav_msgs::msg::OccupancyGrid update_grid(const nav_msgs::msg::OccupancyGrid &occ_grid, const Point2i &drone_pose, double safety_distance)

Update the occupancy grid.

Parameters:
  • occ_grid – occupancy grid

  • drone_pose – drone pose in cell coordinates

  • safety_distance – safety distance in meters

cv::Point2i cellToPixel(Point2i cell, int rows, int cols)

Convert cell coordinates to pixel coordinates.

Parameters:
  • cell – cell coordinates

  • rows – number of rows

  • cols – number of columns

cv::Point2i cellToPixel(Point2i cell, cv::Mat map)

Convert cell coordinates to pixel coordinates.

Parameters:
  • cell – cell coordinates

  • map – map

cv::Point2i cellToPixel(Point2i cell, nav_msgs::msg::MapMetaData map_info)

Convert cell coordinates to pixel coordinates.

Parameters:
  • cell – cell coordinates

  • map_info – map metadata

Point2i pixelToCell(cv::Point2i pixel, nav_msgs::msg::MapMetaData map_info)

Convert pixel coordinates to cell coordinates.

Parameters:
  • pixel – pixel coordinates

  • map_info – map metadata

cv::Mat gridToImg(nav_msgs::msg::OccupancyGrid occ_grid, double thresh = 30, bool unknown_as_free = false)

Occupancy grid to binary image

Parameters:
  • occ_grid – occupancy grid

  • thresh – threshold value

  • unknown_as_free – if true, unknown cells are considered free

Returns:

: binary image

nav_msgs::msg::OccupancyGrid imgToGrid(const cv::Mat img, const std_msgs::msg::Header &header, double grid_resolution)

Binary image to occupancy grid

Parameters:
  • img – binary image

  • header – header of the occupancy grid

  • grid_resolution – resolution of the occupancy grid

Returns:

: occupancy grid

Protected Functions

virtual double calc_h_cost(Point2i current, Point2i end) override
virtual double calc_g_cost(Point2i current) override
virtual int hash_key(Point2i point) override
virtual bool cell_in_limits(Point2i point) override
virtual bool cell_occuppied(Point2i point) override

Protected Attributes

bool use_heuristic_ = true