Nav2 Navigation Stack - rolling  main
ROS 2 Navigation Stack
Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
nav2_costmap_2d::CostmapTopicCollisionChecker Class Reference

Using a costmap via a ros topic, this object is used to find if robot poses are in collision with the costmap environment. More...

#include <nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp>

Collaboration diagram for nav2_costmap_2d::CostmapTopicCollisionChecker:
Collaboration graph
[legend]

Public Member Functions

 CostmapTopicCollisionChecker (CostmapSubscriber &costmap_sub, FootprintSubscriber &footprint_sub, std::string name="collision_checker")
 A constructor.
 
 CostmapTopicCollisionChecker (CostmapSubscriber &costmap_sub, std::string footprint_string, std::string name="collision_checker")
 Alternative constructor with a footprint string instead of a subscriber. It needs to be defined relative to the robot frame.
 
 ~CostmapTopicCollisionChecker ()=default
 A destructor.
 
double scorePose (const geometry_msgs::msg::Pose &pose, bool fetch_costmap_and_footprint=true)
 Returns the obstacle footprint score for a particular pose. More...
 
bool isCollisionFree (const geometry_msgs::msg::Pose &pose, bool fetch_costmap_and_footprint=true)
 Returns if a pose is collision free. More...
 

Protected Member Functions

Footprint getFootprint (const geometry_msgs::msg::Pose &pose, bool fetch_latest_footprint=true)
 Get a footprint at a set pose. More...
 

Protected Attributes

std::string name_
 
CostmapSubscribercostmap_sub_
 
FootprintSubscriberfootprint_sub_ = nullptr
 
FootprintCollisionChecker< std::shared_ptr< Costmap2D > > collision_checker_
 
rclcpp::Clock::SharedPtr clock_
 
Footprint footprint_
 
std::string footprint_string_
 

Detailed Description

Using a costmap via a ros topic, this object is used to find if robot poses are in collision with the costmap environment.

Definition at line 40 of file costmap_topic_collision_checker.hpp.

Member Function Documentation

◆ getFootprint()

Footprint nav2_costmap_2d::CostmapTopicCollisionChecker::getFootprint ( const geometry_msgs::msg::Pose &  pose,
bool  fetch_latest_footprint = true 
)
protected

Get a footprint at a set pose.

Parameters
posePose to get footprint at
fetch_latest_footprintDefaults to true. When checking with multiple poses at once, footprint should be fetched in the first check but fetching can be skipped in consequent checks for speedup

Definition at line 102 of file costmap_topic_collision_checker.cpp.

References nav2_costmap_2d::FootprintSubscriber::getFootprintInRobotFrame(), and nav2_costmap_2d::transformFootprint().

Referenced by scorePose().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ isCollisionFree()

bool nav2_costmap_2d::CostmapTopicCollisionChecker::isCollisionFree ( const geometry_msgs::msg::Pose &  pose,
bool  fetch_costmap_and_footprint = true 
)

Returns if a pose is collision free.

Parameters
posePose to check collision at
fetch_costmap_and_footprintDefaults to true. When checking with multiple poses at once, data should be fetched in the first check but fetching can be skipped in consequent checks for speedup

Definition at line 60 of file costmap_topic_collision_checker.cpp.

References scorePose().

Here is the call graph for this function:

◆ scorePose()

double nav2_costmap_2d::CostmapTopicCollisionChecker::scorePose ( const geometry_msgs::msg::Pose &  pose,
bool  fetch_costmap_and_footprint = true 
)

Returns the obstacle footprint score for a particular pose.

Parameters
posePose to get score at
fetch_costmap_and_footprintDefaults to true. When checking with multiple poses at once, data should be fetched in the first check but fetching can be skipped in consequent checks for speedup

Definition at line 81 of file costmap_topic_collision_checker.cpp.

References nav2_costmap_2d::CostmapSubscriber::getCostmap(), and getFootprint().

Referenced by isCollisionFree().

Here is the call graph for this function:
Here is the caller graph for this function:

The documentation for this class was generated from the following files: