A class that implements RRT sampling-based planning. More...

#include <rrt.h>

Inheritance diagram for RRT:

Public Member Functions
	RRT ()
	Constructor for RRT.

	~RRT ()
	Destructor for RRT.

int	attemptConnect (const State &s_existing, const State &s, double t_s, StateActionResult &result, const PlannerConfig &planner_config, int direction)

int	attemptConnect (const State &s_existing, const State &s, StateActionResult &result, const PlannerConfig &planner_config, int direction)

virtual int	extend (PlannerClass &T, const State &s, const PlannerConfig &planner_config, int direction, ros::Publisher &tree_pub)

std::vector< int >	pathFromStart (PlannerClass &T, int idx)
	Get the path from the root vertex to the specified one.

void	printPath (PlannerClass &T, std::vector< int > path)
	Print the states in the specified path via stdout.

void	getStatistics (double &plan_time, int &vertices_generated, double &plan_length, double &path_duration, double &dist_to_goal)
	Get the statistics for the planner solve.

bool	newConfig (State s, State s_near, StateActionResult &result, const PlannerConfig &planner_config, int direction, ros::Publisher &tree_pub)
	Generate a new state that can be connected to the tree and is as close as possible to the specified state.

std::vector< State >	getStateSequence (PlannerClass &T, std::vector< int > path)
	Get the states along the specified path of vertex indices.

std::vector< Action >	getActionSequence (PlannerClass &T, std::vector< int > path)
	Get the actions along the specified path of vertex indices (assumes that actions are synched with the states to which they lead)

Protected Attributes
const double	prob_goal_thresh = 0.05
	Probability with which the goal is sampled (if running vanilla RRT)

bool	goal_found = false
	Boolean for if the goal has been reached.

std::chrono::duration< double >	elapsed_total_
	Total time elapsed in this planner call.

std::chrono::duration< double >	elapsed_to_first_

int	success_ = 0
	Integer checking if a solution was computed in 5 seconds.

int	num_vertices_
	Number of vertices in the tree.

double	path_length_
	Path quality in meters.

double	path_duration_
	The duration of the path in seconds.

double	dist_to_goal_
	Distance from the final state to the goal (m)

visualization_msgs::MarkerArray	tree_viz_msg_
	Message for tree visualization.

Detailed Description

A class that implements RRT sampling-based planning.

This class builds an RRT using the PlannerClass as a data structure to maintain the tree as well as utility methods to process and debug.

Constructor & Destructor Documentation

◆ RRT()

RRT::RRT ( )

Constructor for RRT.

Returns: Constructed object of type RRT

Member Function Documentation

◆ attemptConnect() [1/2]

int RRT::attemptConnect	(	const State &	s_existing,
		const State &	s,
		double	t_s,
		StateActionResult &	result,
		const PlannerConfig &	planner_config,
		int	direction
	)

Attempt to connect two states with specified stance time, and return a new state if the full connection is not possible

Parameters

[in]	s_existing	The state that is already in the tree and closest to the specified state
[in]	s	The state to extend the tree towards
[in]	t_s	The stance time for this connection
[out]	result	Result of the newConfig operation
[in]	terrain	Height map of the terrain
[in]	direction	Direction of the dynamics (either FORWARD or REVERSE)

Returns: Int describing the result of the attempt (TRAPPED, ADVANCED, or REACHED)

◆ attemptConnect() [2/2]

int RRT::attemptConnect	(	const State &	s_existing,
		const State &	s,
		StateActionResult &	result,
		const PlannerConfig &	planner_config,
		int	direction
	)

Attempt to connect two states, and return a new state if the full connection is not possible. Internally computes stance time

Parameters

[in]	s	The state to extend the tree towards
[out]	result	Result of the newConfig operation
[in]	terrain	Height map of the terrain
[in]	direction	Direction of the dynamics (either FORWARD or REVERSE)

Returns: Int describing the result of the attempt (TRAPPED, ADVANCED, or REACHED)

◆ extend()

int RRT::extend	(	PlannerClass &	T,
		const State &	s,
		const PlannerConfig &	planner_config,
		int	direction,
		ros::Publisher &	tree_pub
	)

virtual

Extend the tree towards the desired state

Parameters

[in]	T	The PlannerClass instance containing the tree
[in]	s	The state to extend the tree towards
[in]	terrain	Height map of the terrain
[in]	direction	The direction with which to peform the extension (FORWARD to go away from the root vertex, REVERSE to go towards it)

◆ getActionSequence()

std::vector< Action > RRT::getActionSequence	(	PlannerClass &	T,
		std::vector< int >	path
	)

Get the actions along the specified path of vertex indices (assumes that actions are synched with the states to which they lead)

Parameters

[in]	T	The PlannerClass instance containing the tree
[in]	path	Vector of vertices in the path

Returns: The sequence of actions in the path

◆ getStateSequence()

std::vector< State > RRT::getStateSequence	(	PlannerClass &	T,
		std::vector< int >	path
	)

Get the states along the specified path of vertex indices.

Parameters

[in]	T	The PlannerClass instance containing the tree
[in]	path	Vector of vertices in the path

Returns: The sequence of states in the path

◆ getStatistics()

void RRT::getStatistics	(	double &	plan_time,
		int &	vertices_generated,
		double &	plan_length,
		double &	path_duration,
		double &	dist_to_goal
	)

Get the statistics for the planner solve.

Parameters

[out]	plan_time	The total time spent in the planner
[out]	vertices_generated	Number of vertices generated in the tree
[out]	plan_length	The length of the path in meters
[out]	path_duration	The duration of the path in seconds
[out]	dist_to_goal	Distance from the final state to the goal (0 if solved)

◆ newConfig()

bool RRT::newConfig	(	State	s,
		State	s_near,
		StateActionResult &	result,
		const PlannerConfig &	planner_config,
		int	direction,
		ros::Publisher &	tree_pub
	)

Generate a new state that can be connected to the tree and is as close as possible to the specified state.

Parameters

[in]	s	Specific state to move towards
[in]	s_near	Closest state in the tree to the specified state
[out]	result	Result of the newConfig operation
[in]	planner_config	Planner configuration parameters
[in]	direction	Direction of the dynamics (either FORWARD or REVERSE)

Returns: Boolean if the new state got closer to the specified state than any other in the tree

◆ pathFromStart()

std::vector< int > RRT::pathFromStart	(	PlannerClass &	T,
		int	idx
	)

Get the path from the root vertex to the specified one.

Parameters

[in]	T	The PlannerClass instance containing the tree
[in]	idx	Index of the desired vertex

Returns: Vector of vertex indices on the path from the root of the tree to the desired

◆ printPath()

void RRT::printPath	(	PlannerClass &	T,
		std::vector< int >	path
	)

Print the states in the specified path via stdout.

Parameters

[in]	T	The PlannerClass instance containing the tree
[in]	path	Vector of vertices in the path

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

global_body_planner/include/global_body_planner/rrt.h
global_body_planner/src/rrt.cpp

Public Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ RRT()

Member Function Documentation

◆ attemptConnect() [1/2]

◆ attemptConnect() [2/2]

◆ extend()

◆ getActionSequence()

◆ getStateSequence()

◆ getStatistics()

◆ newConfig()

◆ pathFromStart()

◆ printPath()