A ROS wrapper for the trajectory controller that queries the param server to construct a controller. More...

#include <trajectory_planner_ros.h>

Inheritance diagram for base_local_planner::TrajectoryPlannerROS:

Public Member Functions
bool	checkTrajectory (double vx_samp, double vy_samp, double vtheta_samp, bool update_map=true)
	Generate and score a single trajectory. More...

bool	computeVelocityCommands (geometry_msgs::Twist &cmd_vel)
	Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base. More...

TrajectoryPlanner *	getPlanner () const
	Return the inner TrajectoryPlanner object. Only valid after initialize(). More...

void	initialize (std::string name, tf2_ros::Buffer tf, costmap_2d::Costmap2DROS costmap_ros)
	Constructs the ros wrapper. More...

bool	isGoalReached ()
	Check if the goal pose has been achieved. More...

bool	isInitialized ()

double	scoreTrajectory (double vx_samp, double vy_samp, double vtheta_samp, bool update_map=true)
	Generate and score a single trajectory. More...

bool	setPlan (const std::vector< geometry_msgs::PoseStamped > &orig_global_plan)
	Set the plan that the controller is following. More...

	TrajectoryPlannerROS ()
	Default constructor for the ros wrapper. More...

	TrajectoryPlannerROS (std::string name, tf2_ros::Buffer tf, costmap_2d::Costmap2DROS costmap_ros)
	Constructs the ros wrapper. More...

	~TrajectoryPlannerROS ()
	Destructor for the wrapper. More...

Private Member Functions
std::vector< double >	loadYVels (ros::NodeHandle node)

void	reconfigureCB (BaseLocalPlannerConfig &config, uint32_t level)
	Callback to update the local planner's parameters based on dynamic reconfigure. More...

bool	rotateToGoal (const geometry_msgs::PoseStamped &global_pose, const geometry_msgs::PoseStamped &robot_vel, double goal_th, geometry_msgs::Twist &cmd_vel)
	Once a goal position is reached... rotate to the goal orientation. More...

double	sign (double x)

bool	stopWithAccLimits (const geometry_msgs::PoseStamped &global_pose, const geometry_msgs::PoseStamped &robot_vel, geometry_msgs::Twist &cmd_vel)
	Stop the robot taking into account acceleration limits. More...

Private Attributes
double	acc_lim_theta_

double	acc_lim_x_

double	acc_lim_y_

nav_msgs::Odometry	base_odom_
	Used to get the velocity of the robot. More...

costmap_2d::Costmap2D *	costmap_
	The costmap the controller will use. More...

costmap_2d::Costmap2DROS *	costmap_ros_
	The ROS wrapper for the costmap the controller will use. More...

base_local_planner::BaseLocalPlannerConfig	default_config_

dynamic_reconfigure::Server< BaseLocalPlannerConfig > *	dsrv_

std::vector< geometry_msgs::Point >	footprint_spec_

ros::Publisher	g_plan_pub_

std::string	global_frame_
	The frame in which the controller will run. More...

std::vector< geometry_msgs::PoseStamped >	global_plan_

bool	initialized_

ros::Publisher	l_plan_pub_

bool	latch_xy_goal_tolerance_

MapGridVisualizer	map_viz_
	The map grid visualizer for outputting the potential field generated by the cost function. More...

double	max_sensor_range_
	Keep track of the effective maximum range of our sensors. More...

double	max_vel_th_

double	min_in_place_vel_th_

double	min_vel_th_

base_local_planner::OdometryHelperRos	odom_helper_

boost::recursive_mutex	odom_lock_

bool	prune_plan_

bool	reached_goal_

std::string	robot_base_frame_
	Used as the base frame id of the robot. More...

double	rot_stopped_velocity_

bool	rotating_to_goal_

bool	setup_

double	sim_period_

TrajectoryPlanner *	tc_
	The trajectory controller. More...

tf2_ros::Buffer *	tf_
	Used for transforming point clouds. More...

double	trans_stopped_velocity_

WorldModel *	world_model_
	The world model that the controller will use. More...

double	xy_goal_tolerance_

bool	xy_tolerance_latch_

double	yaw_goal_tolerance_

Detailed Description

A ROS wrapper for the trajectory controller that queries the param server to construct a controller.

Definition at line 113 of file trajectory_planner_ros.h.

Constructor & Destructor Documentation

◆ TrajectoryPlannerROS() [1/2]

base_local_planner::TrajectoryPlannerROS::TrajectoryPlannerROS ( )

Default constructor for the ros wrapper.

Definition at line 113 of file trajectory_planner_ros.cpp.

◆ TrajectoryPlannerROS() [2/2]

base_local_planner::TrajectoryPlannerROS::TrajectoryPlannerROS	(	std::string	name,
		tf2_ros::Buffer *	tf,
		costmap_2d::Costmap2DROS *	costmap_ros
	)

Constructs the ros wrapper.

Parameters

name	The name to give this instance of the trajectory planner
tf	A pointer to a transform listener
costmap	The cost map to use for assigning costs to trajectories

Definition at line 116 of file trajectory_planner_ros.cpp.

◆ ~TrajectoryPlannerROS()

base_local_planner::TrajectoryPlannerROS::~TrajectoryPlannerROS ( )

Destructor for the wrapper.

Definition at line 334 of file trajectory_planner_ros.cpp.

Member Function Documentation

◆ checkTrajectory()

bool base_local_planner::TrajectoryPlannerROS::checkTrajectory	(	double	vx_samp,
		double	vy_samp,
		double	vtheta_samp,
		bool	update_map = `true`
	)

Generate and score a single trajectory.

Parameters

vx_samp	The x velocity used to seed the trajectory
vy_samp	The y velocity used to seed the trajectory
vtheta_samp	The theta velocity used to seed the trajectory
update_map	Whether or not to update the map for the planner when computing the legality of the trajectory, this is useful to set to false if you're going to be doing a lot of trajectory checking over a short period of time

Returns: True if the trajectory is legal, false otherwise

Definition at line 590 of file trajectory_planner_ros.cpp.

◆ computeVelocityCommands()

bool base_local_planner::TrajectoryPlannerROS::computeVelocityCommands ( geometry_msgs::Twist & cmd_vel )

Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base.

Parameters

cmd_vel Will be filled with the velocity command to be passed to the robot base

Returns: True if a valid trajectory was found, false otherwise

Definition at line 434 of file trajectory_planner_ros.cpp.

◆ getPlanner()

TrajectoryPlanner* base_local_planner::TrajectoryPlannerROS::getPlanner ( ) const

inline

Return the inner TrajectoryPlanner object. Only valid after initialize().

Definition at line 196 of file trajectory_planner_ros.h.

◆ initialize()

void base_local_planner::TrajectoryPlannerROS::initialize	(	std::string	name,
		tf2_ros::Buffer *	tf,
		costmap_2d::Costmap2DROS *	costmap_ros
	)

Constructs the ros wrapper.

Parameters

name	The name to give this instance of the trajectory planner
tf	A pointer to a transform listener
costmap	The cost map to use for assigning costs to trajectories

Definition at line 123 of file trajectory_planner_ros.cpp.

◆ isGoalReached()

bool base_local_planner::TrajectoryPlannerROS::isGoalReached ( )

Check if the goal pose has been achieved.

Returns: True if achieved, false otherwise

Definition at line 648 of file trajectory_planner_ros.cpp.

◆ isInitialized()

bool base_local_planner::TrajectoryPlannerROS::isInitialized ( )

inline

Definition at line 191 of file trajectory_planner_ros.h.

◆ loadYVels()

std::vector< double > base_local_planner::TrajectoryPlannerROS::loadYVels ( ros::NodeHandle node )

private

Definition at line 310 of file trajectory_planner_ros.cpp.

◆ reconfigureCB()

void base_local_planner::TrajectoryPlannerROS::reconfigureCB	(	BaseLocalPlannerConfig &	config,
		uint32_t	level
	)

private

Callback to update the local planner's parameters based on dynamic reconfigure.

Definition at line 99 of file trajectory_planner_ros.cpp.

◆ rotateToGoal()

bool base_local_planner::TrajectoryPlannerROS::rotateToGoal	(	const geometry_msgs::PoseStamped &	global_pose,
		const geometry_msgs::PoseStamped &	robot_vel,
		double	goal_th,
		geometry_msgs::Twist &	cmd_vel
	)

private

Once a goal position is reached... rotate to the goal orientation.

Parameters

global_pose	The pose of the robot in the global frame
robot_vel	The velocity of the robot
goal_th	The desired th value for the goal
cmd_vel	The velocity commands to be filled

Returns: True if a valid trajectory was found, false otherwise

Definition at line 374 of file trajectory_planner_ros.cpp.

◆ scoreTrajectory()

double base_local_planner::TrajectoryPlannerROS::scoreTrajectory	(	double	vx_samp,
		double	vy_samp,
		double	vtheta_samp,
		bool	update_map = `true`
	)

Generate and score a single trajectory.

Parameters

vx_samp	The x velocity used to seed the trajectory
vy_samp	The y velocity used to seed the trajectory
vtheta_samp	The theta velocity used to seed the trajectory
update_map	Whether or not to update the map for the planner when computing the legality of the trajectory, this is useful to set to false if you're going to be doing a lot of trajectory checking over a short period of time

Returns: score of trajectory (double)

Definition at line 619 of file trajectory_planner_ros.cpp.

◆ setPlan()

bool base_local_planner::TrajectoryPlannerROS::setPlan ( const std::vector< geometry_msgs::PoseStamped > & orig_global_plan )

Set the plan that the controller is following.

Parameters

orig_global_plan The plan to pass to the controller

Returns: True if the plan was updated successfully, false otherwise

Definition at line 417 of file trajectory_planner_ros.cpp.

◆ sign()

double base_local_planner::TrajectoryPlannerROS::sign ( double x )

inlineprivate

Definition at line 225 of file trajectory_planner_ros.h.

◆ stopWithAccLimits()

bool base_local_planner::TrajectoryPlannerROS::stopWithAccLimits	(	const geometry_msgs::PoseStamped &	global_pose,
		const geometry_msgs::PoseStamped &	robot_vel,
		geometry_msgs::Twist &	cmd_vel
	)

private

Stop the robot taking into account acceleration limits.

Parameters

global_pose	The pose of the robot in the global frame
robot_vel	The velocity of the robot
cmd_vel	The velocity commands to be filled

Returns: True if a valid trajectory was found, false otherwise

Definition at line 345 of file trajectory_planner_ros.cpp.

Member Data Documentation

◆ acc_lim_theta_

double base_local_planner::TrajectoryPlannerROS::acc_lim_theta_

private

Definition at line 247 of file trajectory_planner_ros.h.

◆ acc_lim_x_

double base_local_planner::TrajectoryPlannerROS::acc_lim_x_

private

Definition at line 247 of file trajectory_planner_ros.h.

◆ acc_lim_y_

double base_local_planner::TrajectoryPlannerROS::acc_lim_y_

private

Definition at line 247 of file trajectory_planner_ros.h.

◆ base_odom_

nav_msgs::Odometry base_local_planner::TrajectoryPlannerROS::base_odom_

private

Used to get the velocity of the robot.

Definition at line 238 of file trajectory_planner_ros.h.

◆ costmap_

costmap_2d::Costmap2D* base_local_planner::TrajectoryPlannerROS::costmap_

private

The costmap the controller will use.

Definition at line 233 of file trajectory_planner_ros.h.

◆ costmap_ros_

costmap_2d::Costmap2DROS* base_local_planner::TrajectoryPlannerROS::costmap_ros_

private

The ROS wrapper for the costmap the controller will use.

Definition at line 232 of file trajectory_planner_ros.h.

◆ default_config_

base_local_planner::BaseLocalPlannerConfig base_local_planner::TrajectoryPlannerROS::default_config_

private

Definition at line 256 of file trajectory_planner_ros.h.

◆ dsrv_

dynamic_reconfigure::Server<BaseLocalPlannerConfig>* base_local_planner::TrajectoryPlannerROS::dsrv_

private

Definition at line 255 of file trajectory_planner_ros.h.

◆ footprint_spec_

std::vector<geometry_msgs::Point> base_local_planner::TrajectoryPlannerROS::footprint_spec_

private

Definition at line 263 of file trajectory_planner_ros.h.

◆ g_plan_pub_

ros::Publisher base_local_planner::TrajectoryPlannerROS::g_plan_pub_

private

Definition at line 253 of file trajectory_planner_ros.h.

◆ global_frame_

std::string base_local_planner::TrajectoryPlannerROS::global_frame_

private

The frame in which the controller will run.

Definition at line 236 of file trajectory_planner_ros.h.

◆ global_plan_

std::vector<geometry_msgs::PoseStamped> base_local_planner::TrajectoryPlannerROS::global_plan_

private

Definition at line 242 of file trajectory_planner_ros.h.

◆ initialized_

bool base_local_planner::TrajectoryPlannerROS::initialized_

private

Definition at line 260 of file trajectory_planner_ros.h.

◆ l_plan_pub_

ros::Publisher base_local_planner::TrajectoryPlannerROS::l_plan_pub_

private

Definition at line 253 of file trajectory_planner_ros.h.

◆ latch_xy_goal_tolerance_

bool base_local_planner::TrajectoryPlannerROS::latch_xy_goal_tolerance_

private

Definition at line 251 of file trajectory_planner_ros.h.

◆ map_viz_

MapGridVisualizer base_local_planner::TrajectoryPlannerROS::map_viz_

private

The map grid visualizer for outputting the potential field generated by the cost function.

Definition at line 234 of file trajectory_planner_ros.h.

◆ max_sensor_range_

double base_local_planner::TrajectoryPlannerROS::max_sensor_range_

private

Keep track of the effective maximum range of our sensors.

Definition at line 237 of file trajectory_planner_ros.h.

◆ max_vel_th_

double base_local_planner::TrajectoryPlannerROS::max_vel_th_

private

Definition at line 246 of file trajectory_planner_ros.h.

◆ min_in_place_vel_th_

double base_local_planner::TrajectoryPlannerROS::min_in_place_vel_th_

private

Definition at line 241 of file trajectory_planner_ros.h.

◆ min_vel_th_

double base_local_planner::TrajectoryPlannerROS::min_vel_th_

private

Definition at line 246 of file trajectory_planner_ros.h.

◆ odom_helper_

base_local_planner::OdometryHelperRos base_local_planner::TrajectoryPlannerROS::odom_helper_

private

Definition at line 261 of file trajectory_planner_ros.h.

◆ odom_lock_

boost::recursive_mutex base_local_planner::TrajectoryPlannerROS::odom_lock_

private

Definition at line 244 of file trajectory_planner_ros.h.

◆ prune_plan_

bool base_local_planner::TrajectoryPlannerROS::prune_plan_

private

Definition at line 243 of file trajectory_planner_ros.h.

◆ reached_goal_

bool base_local_planner::TrajectoryPlannerROS::reached_goal_

private

Definition at line 250 of file trajectory_planner_ros.h.

◆ robot_base_frame_

std::string base_local_planner::TrajectoryPlannerROS::robot_base_frame_

private

Used as the base frame id of the robot.

Definition at line 239 of file trajectory_planner_ros.h.

◆ rot_stopped_velocity_

double base_local_planner::TrajectoryPlannerROS::rot_stopped_velocity_

private

Definition at line 240 of file trajectory_planner_ros.h.

◆ rotating_to_goal_

bool base_local_planner::TrajectoryPlannerROS::rotating_to_goal_

private

Definition at line 249 of file trajectory_planner_ros.h.

◆ setup_

bool base_local_planner::TrajectoryPlannerROS::setup_

private

Definition at line 257 of file trajectory_planner_ros.h.

◆ sim_period_

double base_local_planner::TrajectoryPlannerROS::sim_period_

private

Definition at line 248 of file trajectory_planner_ros.h.

◆ tc_

TrajectoryPlanner* base_local_planner::TrajectoryPlannerROS::tc_

private

The trajectory controller.

Definition at line 230 of file trajectory_planner_ros.h.

◆ tf_

tf2_ros::Buffer* base_local_planner::TrajectoryPlannerROS::tf_

private

Used for transforming point clouds.

Definition at line 235 of file trajectory_planner_ros.h.

◆ trans_stopped_velocity_

double base_local_planner::TrajectoryPlannerROS::trans_stopped_velocity_

private

Definition at line 240 of file trajectory_planner_ros.h.

◆ world_model_

WorldModel* base_local_planner::TrajectoryPlannerROS::world_model_

private

The world model that the controller will use.

Definition at line 229 of file trajectory_planner_ros.h.

◆ xy_goal_tolerance_

double base_local_planner::TrajectoryPlannerROS::xy_goal_tolerance_

private

Definition at line 241 of file trajectory_planner_ros.h.

◆ xy_tolerance_latch_

bool base_local_planner::TrajectoryPlannerROS::xy_tolerance_latch_

private

Definition at line 251 of file trajectory_planner_ros.h.

◆ yaw_goal_tolerance_

double base_local_planner::TrajectoryPlannerROS::yaw_goal_tolerance_

private

Definition at line 241 of file trajectory_planner_ros.h.

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

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ TrajectoryPlannerROS() [1/2]

◆ TrajectoryPlannerROS() [2/2]

◆ ~TrajectoryPlannerROS()

Member Function Documentation

◆ checkTrajectory()

◆ computeVelocityCommands()

◆ getPlanner()

◆ initialize()

◆ isGoalReached()

◆ isInitialized()

◆ loadYVels()

◆ reconfigureCB()

◆ rotateToGoal()

◆ scoreTrajectory()

◆ setPlan()

◆ sign()

◆ stopWithAccLimits()

Member Data Documentation

◆ acc_lim_theta_

◆ acc_lim_x_

◆ acc_lim_y_

◆ base_odom_

◆ costmap_

◆ costmap_ros_

◆ default_config_

◆ dsrv_

◆ footprint_spec_

◆ g_plan_pub_

◆ global_frame_

◆ global_plan_

◆ initialized_

◆ l_plan_pub_

◆ latch_xy_goal_tolerance_

◆ map_viz_

◆ max_sensor_range_

◆ max_vel_th_

◆ min_in_place_vel_th_

◆ min_vel_th_

◆ odom_helper_

◆ odom_lock_

◆ prune_plan_

◆ reached_goal_

◆ robot_base_frame_

◆ rot_stopped_velocity_

◆ rotating_to_goal_

◆ setup_

◆ sim_period_

◆ tc_

◆ tf_

◆ trans_stopped_velocity_

◆ world_model_

◆ xy_goal_tolerance_

◆ xy_tolerance_latch_

◆ yaw_goal_tolerance_