A global body planning class for legged robots. More...

#include <global_body_planner.h>

Collaboration diagram for GlobalBodyPlanner:

[legend]

Public Member Functions
	GlobalBodyPlanner (ros::NodeHandle nh)
	Constructor for GlobalBodyPlanner Class.

bool	callPlanner ()
	Call the correct planning class and compute statistics.

void	spin ()
	Primary work function in class, called in node file for this component.

Private Member Functions
void	terrainMapCallback (const grid_map_msgs::GridMap::ConstPtr &msg)
	Callback function to handle new terrain map data.

void	robotStateCallback (const quad_msgs::RobotState::ConstPtr &msg)
	Callback function to handle new robot state data.

void	goalStateCallback (const geometry_msgs::PointStamped::ConstPtr &msg)
	Callback function to handle new goal state.

void	triggerReset ()
	Trigger a reset event.

int	initPlanner ()
	Initialize the planner by clearing out old plan data and setting the start state.

void	clearPlan ()
	Clear all data in plan member variables.

void	addStateAndGRFToMsg (double t, int plan_index, FullState body_state, GRF grf, int primitive_id, quad_msgs::RobotPlan &body_plan_msg)
	Update the body plan with the current plan.

void	publishPlan ()
	Publish the current body plan.

void	waitForData ()
	Wait until map and state messages have been received and processed.

void	getInitialPlan ()
	Call the planner repeatedly until startup_delay has been reached then return.

void	setStartState ()
	Set the start state to be used by the next planning call.

void	setGoalState ()
	Set the goal state to be used by the next planning call.

void	publishCurrentPlan ()
	Publish the current plan if updated.

Private Attributes
ros::Subscriber	terrain_map_sub_
	Subscriber for terrain map messages.

ros::Subscriber	robot_state_sub_
	Subscriber for robot state messages.

ros::Subscriber	goal_state_sub_
	Subscriber for goal state messages.

ros::Publisher	body_plan_pub_
	Publisher for body plan messages.

ros::Publisher	discrete_body_plan_pub_
	Publisher for discrete states in body plan messages.

ros::Publisher	tree_pub_
	Publisher for the planning tree.

std::string	terrain_map_topic_
	Topic name for terrain map (needed to ensure data has been received)

std::string	robot_state_topic_
	Topic name for robot state data (needed to ensure data has been received)

ros::NodeHandle	nh_
	Nodehandle to pub to and sub from.

double	update_rate_
	Update rate for sending and receiving data;.

int	num_calls_
	Number of times to call the planner.

double	max_planning_time_
	Max time to let the algorithm search.

std::string	map_frame_
	Handle for the map frame.

GlobalBodyPlan	newest_plan_
	Plan data for the most recently computed plan (may be suboptimal)

GlobalBodyPlan	current_plan_
	Plan data for the plan currently being executed.

FullState	start_state_
	Starting state for planner call.

FullState	goal_state_
	Goal state for planner.

FullState	robot_state_
	Current robot state.

geometry_msgs::PointStamped::ConstPtr	goal_state_msg_
	goal_state_msg_

double	replan_start_time_
	Starting time for planner call during replans relative to t_plan_[0].

double	committed_horizon_
	Horizon to commit to (replan from the next state after this horizon)

double	pos_error_threshold_
	Threshold of state error to trigger replanning.

bool	restart_flag_

std::vector< State >	state_sequence_
	Sequence of discrete states in the plan.

std::vector< Action >	action_sequence_
	Sequence of discrete actions in the plan.

std::vector< double >	cost_vector_
	Vector of cost instances in each planning call.

std::vector< double >	cost_vector_times_
	Vector of time instances of cost data for each planning call.

std::vector< double >	solve_time_info_
	Vector of solve times for each planning call.

std::vector< int >	vertices_generated_info_
	Vector of number of vertices for each planning call.

PlannerConfig	planner_config_
	Planner config.

double	reset_publish_delay_
	Delay after plan reset before publishing and refining.

bool	replanning_allowed_
	Boolean for whether replanning is allowed.

double	dt_
	Timestep for interpolation.

int	planner_status_
	ID for status of planner.

int	start_index_
	Index of active plan from which to begin refinement.

ros::Time	reset_time_
	Time at which reset began.

bool	publish_after_reset_delay_

ros::Time	global_plan_timestamp_
	Timestamp for t=0 of global plan.

Static Private Attributes
static const int	RESET = 0
	Planning status ID for resetting (start from scratch)

static const int	REFINE = 1
	Planning status ID for refining (optimize current plan)

Detailed Description

A global body planning class for legged robots.

GlobalBodyPlanner is a container for all of the logic utilized in the global body planning node. This algorithm requires an height map of the terrain as a GridMap message type, and will publish the global body plan as a BodyPlan message over a topic. It will also publish the discrete states used by the planner (from which the full path is interpolated).

Constructor & Destructor Documentation

◆ GlobalBodyPlanner()

GlobalBodyPlanner::GlobalBodyPlanner ( ros::NodeHandle nh )

Constructor for GlobalBodyPlanner Class.

Parameters

[in] nh Node handle

Returns: Constructed object of type GlobalBodyPlanner

Member Function Documentation

◆ addStateAndGRFToMsg()

void GlobalBodyPlanner::addStateAndGRFToMsg	(	double	t,
		int	plan_index,
		FullState	body_state,
		GRF	grf,
		int	primitive_id,
		quad_msgs::RobotPlan &	body_plan_msg
	)

private

Update the body plan with the current plan.

Parameters

[in]	t	Time of state in trajectory
[in]	body_state	Body state
[in]	grf	GRF applied to body
[in]	body_plan_msg	Body plan message

◆ callPlanner()

bool GlobalBodyPlanner::callPlanner ( )

Call the correct planning class and compute statistics.

Returns: Boolean for success of the planner

◆ goalStateCallback()

void GlobalBodyPlanner::goalStateCallback ( const geometry_msgs::PointStamped::ConstPtr & msg )

private

Callback function to handle new goal state.

Parameters

[in] msg the message contining the goal state

◆ initPlanner()

int GlobalBodyPlanner::initPlanner ( )

private

Initialize the planner by clearing out old plan data and setting the start state.

Returns: Index of the current plan from which to being the new plan (zero if fully replanning)

◆ robotStateCallback()

void GlobalBodyPlanner::robotStateCallback ( const quad_msgs::RobotState::ConstPtr & msg )

private

Callback function to handle new robot state data.

Parameters

[in] msg the message contining robot state data

◆ terrainMapCallback()

void GlobalBodyPlanner::terrainMapCallback ( const grid_map_msgs::GridMap::ConstPtr & msg )

private

Callback function to handle new terrain map data.

Parameters

[in] msg the message contining map data

Member Data Documentation

◆ publish_after_reset_delay_

bool GlobalBodyPlanner::publish_after_reset_delay_

private

Boolean for whether to publish the new plan after the reset delay has occured

◆ restart_flag_

bool GlobalBodyPlanner::restart_flag_

private

Flag to determine if the planner needs to restart planning from the robot state

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

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

Public Member Functions

Private Member Functions

Private Attributes

Static Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ GlobalBodyPlanner()

Member Function Documentation

◆ addStateAndGRFToMsg()

◆ callPlanner()

◆ goalStateCallback()

◆ initPlanner()

◆ robotStateCallback()

◆ terrainMapCallback()

Member Data Documentation

◆ publish_after_reset_delay_

◆ restart_flag_