nav2_route::GoalIntentExtractor Class Reference

Extracts the start and goal nodes in the graph to use for the routing request from the action request. This may use additional information about the environment or simply find the corresponding nodes specified in the request. More...

#include <nav2_route/include/nav2_route/goal_intent_extractor.hpp>

Collaboration diagram for nav2_route::GoalIntentExtractor:

Public Member Functions
	GoalIntentExtractor ()=default
	Constructor.
	~GoalIntentExtractor ()=default
	Destructor.
void	configure (nav2_util::LifecycleNode::SharedPtr node, Graph &graph, GraphToIDMap *id_to_graph_map, std::shared_ptr< tf2_ros::Buffer > tf, std::shared_ptr< nav2_costmap_2d::CostmapSubscriber > costmap_subscriber, const std::string &route_frame, const std::string &base_frame)
	Configure extractor. More...
void	setGraph (Graph &graph, GraphToIDMap *id_to_graph_map)
	Sets a new graph when updated. More...
geometry_msgs::msg::PoseStamped	transformPose (geometry_msgs::msg::PoseStamped &pose, const std::string &frame_id)
	Transforms a pose into the route frame. More...
template<typename GoalT >
NodeExtents	findStartandGoal (const std::shared_ptr< const GoalT > goal)
	Main API to find the start and goal graph IDX (not IDs) for routing. More...
template<typename GoalT >
Route	pruneStartandGoal (const Route &input_route, const std::shared_ptr< const GoalT > goal, ReroutingState &rerouting_info)
	Prune the start and end nodes in a route if the start or goal poses, respectively, are already along the route to avoid backtracking. More...
void	overrideStart (const geometry_msgs::msg::PoseStamped &start_pose)
	Override the start pose for use in pruning if it is externally overridden Usually by the rerouting logic. More...
geometry_msgs::msg::PoseStamped	getStart ()
	gets the desired start pose More...

Protected Attributes
rclcpp::Logger	logger_ {rclcpp::get_logger("GoalIntentExtractor")}
std::shared_ptr< NodeSpatialTree >	node_spatial_tree_
GraphToIDMap *	id_to_graph_map_
Graph *	graph_
std::shared_ptr< tf2_ros::Buffer >	tf_
std::shared_ptr< nav2_costmap_2d::CostmapSubscriber >	costmap_subscriber_
std::string	route_frame_
std::string	base_frame_
geometry_msgs::msg::PoseStamped	start_
geometry_msgs::msg::PoseStamped	goal_
bool	prune_goal_
bool	enable_search_
int	max_nn_search_iterations_
float	max_dist_from_edge_
float	min_dist_from_goal_
float	min_dist_from_start_

Detailed Description

Extracts the start and goal nodes in the graph to use for the routing request from the action request. This may use additional information about the environment or simply find the corresponding nodes specified in the request.

Definition at line 44 of file goal_intent_extractor.hpp.

Member Function Documentation

configure()

void nav2_route::GoalIntentExtractor::configure	(	nav2_util::LifecycleNode::SharedPtr	node,
		Graph &	graph,
		GraphToIDMap *	id_to_graph_map,
		std::shared_ptr< tf2_ros::Buffer >	tf,
		std::shared_ptr< nav2_costmap_2d::CostmapSubscriber >	costmap_subscriber,
		const std::string &	route_frame,
		const std::string &	base_frame
	)

Configure extractor.

Parameters

node	Node to use to create any interface needed
graph	Graph to populate kD tree using
id_to_graph_map	Remapping vector to correlate nodeIDs
tf	TF buffer for transformations
costmap_subscriber	Costmap subscriber to use for traversability
route_frame	Planning frame
base_frame	Robot reference frame

Definition at line 26 of file goal_intent_extractor.cpp.

findStartandGoal()

template<typename GoalT >

NodeExtents nav2_route::GoalIntentExtractor::findStartandGoal

(

const std::shared_ptr< const GoalT >

goal

)

Main API to find the start and goal graph IDX (not IDs) for routing.

Parameters

goal	Action request goal

Returns

start, goal node IDx

Definition at line 106 of file goal_intent_extractor.cpp.

References nav2_route::GoalIntentSearch::BreadthFirstSearch::getClosestNodeIdx(), nav2_route::GoalIntentSearch::LoSCollisionChecker::isInCollision(), nav2_route::GoalIntentSearch::BreadthFirstSearch::search(), transformPose(), and nav2_route::GoalIntentSearch::LoSCollisionChecker::worldToMap().

Here is the call graph for this function:

getStart()

geometry_msgs::msg::PoseStamped nav2_route::GoalIntentExtractor::getStart

(

)

gets the desired start pose

Returns

PoseStamped of start pose

Definition at line 295 of file goal_intent_extractor.cpp.

overrideStart()

void nav2_route::GoalIntentExtractor::overrideStart

(

const geometry_msgs::msg::PoseStamped &

start_pose

)

Override the start pose for use in pruning if it is externally overridden Usually by the rerouting logic.

Parameters

start_pose

Starting pose of robot to prune using

Definition at line 98 of file goal_intent_extractor.cpp.

pruneStartandGoal()

template<typename GoalT >

Route nav2_route::GoalIntentExtractor::pruneStartandGoal	(	const Route &	input_route,
		const std::shared_ptr< const GoalT >	goal,
		ReroutingState &	rerouting_info
	)

Prune the start and end nodes in a route if the start or goal poses, respectively, are already along the route to avoid backtracking.

Parameters

inpute_route	Route to prune
goal	Action request goal
first_time	If this is the first time routing

Returns

Pruned route

Definition at line 226 of file goal_intent_extractor.cpp.

setGraph()

void nav2_route::GoalIntentExtractor::setGraph	(	Graph &	graph,
		GraphToIDMap *	id_to_graph_map
	)

Sets a new graph when updated.

Parameters

graph	Graph to populate kD tree using
graph	id_to_graph_map to get graph IDX for node IDs

Definition at line 75 of file goal_intent_extractor.cpp.

transformPose()

geometry_msgs::msg::PoseStamped nav2_route::GoalIntentExtractor::transformPose	(	geometry_msgs::msg::PoseStamped &	pose,
		const std::string &	frame_id
	)

Transforms a pose into the route frame.

Parameters

pose	Pose to transform (e.g. start, goal)
frame_id	Frame to transform to

Definition at line 82 of file goal_intent_extractor.cpp.

Referenced by findStartandGoal().

Here is the caller graph for this function:

---

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

• nav2_route/include/nav2_route/goal_intent_extractor.hpp
• nav2_route/src/goal_intent_extractor.cpp