Class GoalIntentExtractor

Class Documentation

class GoalIntentExtractor

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.

Public Functions

GoalIntentExtractor() = default

Constructor.

~GoalIntentExtractor() = default

Destructor.

void configure(rclcpp_lifecycle::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 &global_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

void 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

geometry_msgs::msg::PoseStamped 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

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.

Parameters:

goal – Action request goal

Returns:

start, goal node IDx

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.

Parameters:

  • inpute_route – Route to prune

  • goal – Action request goal

  • first_time – If this is the first time routing

Returns:

Pruned route

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.

Parameters:

start_pose – Starting pose of robot to prune using

geometry_msgs::msg::PoseStamped getStart()

gets the desired start pose

Returns:

PoseStamped of start pose

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 global_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_