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 iri_diff_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.
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.
void	initialize (std::string name, tf::TransformListener tf, costmap_2d::Costmap2DROS costmap_ros)
	Constructs the ros wrapper.
bool	isGoalReached ()
	Check if the goal pose has been achieved.
double	scoreTrajectory (double vx_samp, double vy_samp, double vtheta_samp, bool update_map=true)
	Generate and score a single trajectory.
bool	setPlan (const std::vector< geometry_msgs::PoseStamped > &orig_global_plan)
	Set the plan that the controller is following.
	TrajectoryPlannerROS ()
	Default constructor for the ros wrapper.
	TrajectoryPlannerROS (std::string name, tf::TransformListener tf, costmap_2d::Costmap2DROS costmap_ros)
	Constructs the ros wrapper.
	~TrajectoryPlannerROS ()
	Destructor for the wrapper.
Private Member Functions
bool	change_max_velCallback (iri_diff_local_planner::change_max_vel::Request &req, iri_diff_local_planner::change_max_vel::Response &res)
std::vector< double >	loadYVels (ros::NodeHandle node)
void	odomCallback (const nav_msgs::Odometry::ConstPtr &msg)
void	reconfigureCB (IriDiffLocalPlannerConfig &config, uint32_t level)
	Callback for dynamic_reconfigure.
bool	rotateToGoal (const tf::Stamped< tf::Pose > &global_pose, const tf::Stamped< tf::Pose > &robot_vel, double goal_th, geometry_msgs::Twist &cmd_vel)
	Once a goal position is reached... rotate to the goal orientation.
double	sign (double x)
bool	stopWithAccLimits (const tf::Stamped< tf::Pose > &global_pose, const tf::Stamped< tf::Pose > &robot_vel, geometry_msgs::Twist &cmd_vel)
	Stop the robot taking into account acceleration limits.
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.
ros::ServiceServer	change_max_vel
costmap_2d::Costmap2D	costmap_
	The costmap the controller will use.
costmap_2d::Costmap2DROS *	costmap_ros_
	The ROS wrapper for the costmap the controller will use.
iri_diff_local_planner::IriDiffLocalPlannerConfig	default_config_
dynamic_reconfigure::Server < IriDiffLocalPlannerConfig > *	dsrv_
ros::Publisher	g_plan_pub_
std::string	global_frame_
	The frame in which the controller will run.
std::vector < geometry_msgs::PoseStamped >	global_plan_
double	inflation_radius_
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.
double	max_sensor_range_
	Keep track of the effective maximum range of our sensors.
double	max_vel_th_
double	max_vel_x_
double	min_in_place_vel_th_
double	min_vel_th_
boost::recursive_mutex	odom_lock_
ros::Subscriber	odom_sub_
bool	prune_plan_
std::string	robot_base_frame_
	Used as the base frame id of the robot.
double	rot_stopped_velocity_
bool	rotating_to_goal_
bool	setup_
double	sim_period_
TrajectoryPlanner *	tc_
	The trajectory controller.
tf::TransformListener *	tf_
	Used for transforming point clouds.
double	trans_stopped_velocity_
WorldModel *	world_model_
	The world model that the controller will use.
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 79 of file trajectory_planner_ros.h.

Constructor & Destructor Documentation

iri_diff_local_planner::TrajectoryPlannerROS::TrajectoryPlannerROS ( )

Default constructor for the ros wrapper.

Definition at line 71 of file trajectory_planner_ros.cpp.

iri_diff_local_planner::TrajectoryPlannerROS::TrajectoryPlannerROS	(	std::string	name,
		tf::TransformListener *	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 73 of file trajectory_planner_ros.cpp.

iri_diff_local_planner::TrajectoryPlannerROS::~TrajectoryPlannerROS ( )

Destructor for the wrapper.

Definition at line 271 of file trajectory_planner_ros.cpp.

Member Function Documentation

bool iri_diff_local_planner::TrajectoryPlannerROS::change_max_velCallback	(	iri_diff_local_planner::change_max_vel::Request &	req,
		iri_diff_local_planner::change_max_vel::Response &	res
	)		`[private]`

Definition at line 240 of file trajectory_planner_ros.cpp.

bool iri_diff_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 541 of file trajectory_planner_ros.cpp.

bool iri_diff_local_planner::TrajectoryPlannerROS::computeVelocityCommands ( geometry_msgs::Twist & cmd_vel ) [virtual]

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

Implements nav_core::BaseLocalPlanner.

Definition at line 376 of file trajectory_planner_ros.cpp.

void iri_diff_local_planner::TrajectoryPlannerROS::initialize	(	std::string	name,
		tf::TransformListener *	tf,
		costmap_2d::Costmap2DROS *	costmap_ros
	)		`[virtual]`

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

Implements nav_core::BaseLocalPlanner.

Definition at line 80 of file trajectory_planner_ros.cpp.

bool iri_diff_local_planner::TrajectoryPlannerROS::isGoalReached ( ) [virtual]

Check if the goal pose has been achieved.

Returns:: True if achieved, false otherwise

Implements nav_core::BaseLocalPlanner.

Definition at line 615 of file trajectory_planner_ros.cpp.

std::vector< double > iri_diff_local_planner::TrajectoryPlannerROS::loadYVels ( ros::NodeHandle node ) [private]

Definition at line 247 of file trajectory_planner_ros.cpp.

void iri_diff_local_planner::TrajectoryPlannerROS::odomCallback ( const nav_msgs::Odometry::ConstPtr & msg ) [private]

Definition at line 349 of file trajectory_planner_ros.cpp.

void iri_diff_local_planner::TrajectoryPlannerROS::reconfigureCB	(	IriDiffLocalPlannerConfig &	config,
		uint32_t	level
	)		`[private]`

Callback for dynamic_reconfigure.

Definition at line 56 of file trajectory_planner_ros.cpp.

bool iri_diff_local_planner::TrajectoryPlannerROS::rotateToGoal	(	const tf::Stamped< tf::Pose > &	global_pose,
		const tf::Stamped< tf::Pose > &	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 311 of file trajectory_planner_ros.cpp.

double iri_diff_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 578 of file trajectory_planner_ros.cpp.

bool iri_diff_local_planner::TrajectoryPlannerROS::setPlan ( const std::vector< geometry_msgs::PoseStamped > & orig_global_plan ) [virtual]

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

Implements nav_core::BaseLocalPlanner.

Definition at line 359 of file trajectory_planner_ros.cpp.

double iri_diff_local_planner::TrajectoryPlannerROS::sign ( double x ) [inline, private]

Definition at line 184 of file trajectory_planner_ros.h.

bool iri_diff_local_planner::TrajectoryPlannerROS::stopWithAccLimits	(	const tf::Stamped< tf::Pose > &	global_pose,
		const tf::Stamped< tf::Pose > &	robot_vel,
		geometry_msgs::Twist &	cmd_vel
	)		`[private]`