#include <costmap_trajectory_checker.h>

Public Member Functions
double	checkTrajectory (const std::vector< geometry_msgs::Pose2D > &traj, bool update_map=false, bool clear_footprint=false)
	Check validity of a trajectory against the provided costmap.
double	checkTrajectoryMonotonic (const std::vector< geometry_msgs::Pose2D > &traj, bool update_map=false, bool clear_footprint=false, unsigned int num_consec_points_in_collision=5)
	Check validity of a trajectory against the provided costmap.
double	checkTwist (const geometry_msgs::Twist &vel, unsigned int num_steps=10, double dt=0.2, bool update_map=false, bool clear_footprint=false)
	A wrapper for checkTrajectory and generateTrajectory that generates and checks a trajectory using the provided twist.
double	checkTwistMonotonic (const geometry_msgs::Twist &vel, unsigned int num_steps=10, double dt=0.2, bool update_map=false, bool clear_footprint=false)
	A wrapper for checkTrajectory and generateTrajectory that generates and checks a trajectory using the provided twist.
bool	clearFootprint (const geometry_msgs::Pose2D &traj, bool update_map)
bool	clearFootprint (bool update_map)
	CostmapTrajectoryChecker ()
	Default constructor.
	CostmapTrajectoryChecker (costmap_2d::Costmap2DROS *costmap_ros, std::string frame_id, std::vector< geometry_msgs::Point > footprint, std::string topic="")
	CostmapTrajectoryChecker (costmap_2d::Costmap2DROS *costmap_ros, std::string topic="")
	CostmapTrajectoryChecker (const CostmapTrajectoryChecker &checker)
	Copy constructor.
std::vector< geometry_msgs::Point >	getFootprint ()
std::string	getGlobalFrameID ()
std::string	getPubTopic ()
std::string	getRobotFrameID ()
void	initialize (costmap_2d::Costmap2DROS *costmap_ros, std::string topic="")
CostmapTrajectoryChecker &	operator= (const CostmapTrajectoryChecker &checker)
void	setFootprint (const std::vector< geometry_msgs::Point > &footprint)
void	setFootprint (double length, double width, double x_offset, double y_offset)
void	setGlobalFrameID (std::string frame_id)
	Getters and setters for frame_ids.
void	setPubTopic (std::string topic)
	Getter and setter for visualization topic.
void	setRobotFrameID (std::string frame_id)
	~CostmapTrajectoryChecker ()
	Destructor.
Static Public Member Functions
static void	generateTrajectory (const geometry_msgs::Pose2D &start_pose, const geometry_msgs::Twist &vel, unsigned int num_steps, double dt, std::vector< geometry_msgs::Pose2D > &traj)
	Generates a Pose2D trajectory by integrating provided velocity.
Private Member Functions
double	footprintCost (const geometry_msgs::Pose2D &pose)
	Returns the max cost in the costmap at the provided position.
void	getOrientedFootprint (const geometry_msgs::Pose2D &pose, std::vector< geometry_msgs::Point > &oriented_footprint)
bool	getRobotPose (geometry_msgs::Pose2D &pose_2d) const
	Retrieves the robot pose and converts to Pose2D representation.
void	publishTrajectory (const std::vector< geometry_msgs::Pose2D > &path)
Static Private Member Functions
static void	integratePose (const geometry_msgs::Twist &vel, double dt, geometry_msgs::Pose2D &pose)
static void	pose2DToPose (const geometry_msgs::Pose2D pose_2d, geometry_msgs::Pose &pose)
	Helper function for converting from geometry_msgs::Pose2D to geometry_msgs::Pose.
static void	pose2DToTF (const geometry_msgs::Pose2D pose_2d, tf::Pose &pose_tf)
	Helper function for converting from geometry_msgs::Pose2D to tf::pose.
static void	poseTFToPose2D (const tf::Pose &pose_tf, geometry_msgs::Pose2D &pose_2d)
	Helper function for converting from tf::pose to geometry_msgs::Pose2D Discards linear Z, Roll, and Pitch dimensions.
static void	poseToPose2D (const geometry_msgs::Pose pose, geometry_msgs::Pose2D &pose_2d)
	Helper function for converting from geometry_msgs::Pose to geometry_msgs::Pose2D.
Private Attributes
double	circumscribed_radius_
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.
std::vector< geometry_msgs::Point >	footprint_spec_
	The footprint specification of the robot.
std::string	global_frame_
	Used as the global frame id.
double	inflation_radius_
bool	initialized_
double	inscribed_radius_
ros::NodeHandle	nh_
	A local node handle.
std::string	robot_frame_
	Used as the base frame id of the robot.
tf::TransformListener	tl_
	Used for looking up robot pose.
ros::Publisher	traj_pub_
	A publisher for visualizing trajectories.
std::string	traj_topic_name_
	The topic name for publishing trajectories.
base_local_planner::WorldModel *	world_model_
	The world model to use for collision detection.

Detailed Description

Uses Pose2D rather than Pose messages because (a) it ensures proper input for our robots which can't fly, and (b)n because it's slightly more efficient when integrating velocity to not have to convert back and forth from a quaternion (though this is moot when publishing)

Definition at line 64 of file costmap_trajectory_checker.h.

Constructor & Destructor Documentation

CostmapTrajectoryChecker::CostmapTrajectoryChecker ( )

Default constructor.

Definition at line 46 of file costmap_trajectory_checker.cpp.

CostmapTrajectoryChecker::CostmapTrajectoryChecker	(	costmap_2d::Costmap2DROS *	costmap_ros,
		std::string	frame_id,
		std::vector< geometry_msgs::Point >	footprint,
		std::string	topic = `""`
	)

Parameters:

costmap_ros
frame_id
footprint

Returns:

Definition at line 56 of file costmap_trajectory_checker.cpp.

CostmapTrajectoryChecker::CostmapTrajectoryChecker	(	costmap_2d::Costmap2DROS *	costmap_ros,
		std::string	topic = `""`
	)

Barebones constructor. Gets footprint and frame_id from the costmap

Parameters:

costmap_ros

Returns:

Definition at line 50 of file costmap_trajectory_checker.cpp.

CostmapTrajectoryChecker::CostmapTrajectoryChecker ( const CostmapTrajectoryChecker & checker )

Copy constructor.

Definition at line 68 of file costmap_trajectory_checker.cpp.

CostmapTrajectoryChecker::~CostmapTrajectoryChecker ( )

Destructor.

Definition at line 89 of file costmap_trajectory_checker.cpp.

Member Function Documentation

double CostmapTrajectoryChecker::checkTrajectory	(	const std::vector< geometry_msgs::Pose2D > &	traj,
		bool	update_map = `false`,
		bool	clear_footprint = `false`
	)

Check validity of a trajectory against the provided costmap.

Returns:: The maximum cost on the trajectory, or -1 if LETHAL

Definition at line 190 of file costmap_trajectory_checker.cpp.

double CostmapTrajectoryChecker::checkTrajectoryMonotonic	(	const std::vector< geometry_msgs::Pose2D > &	traj,
		bool	update_map = `false`,
		bool	clear_footprint = `false`,
		unsigned int	num_consec_points_in_collision = `5`
	)

Check validity of a trajectory against the provided costmap.

Returns:: The maximum cost on the trajectory, or -1 if LETHAL

Definition at line 266 of file costmap_trajectory_checker.cpp.

double CostmapTrajectoryChecker::checkTwist	(	const geometry_msgs::Twist &	vel,
		unsigned int	num_steps = `10`,
		double	dt = `0.2`,
		bool	update_map = `false`,
		bool	clear_footprint = `false`
	)

A wrapper for checkTrajectory and generateTrajectory that generates and checks a trajectory using the provided twist.

Parameters:

vel	A Twist describing the robot's current velocity
num_steps	The length of the trajectory to generate
dt	Time between samples along the trajectory

Returns:: The maximum cost on the trajectory, or -1 if LETHAL

Definition at line 332 of file costmap_trajectory_checker.cpp.

double CostmapTrajectoryChecker::checkTwistMonotonic	(	const geometry_msgs::Twist &	vel,
		unsigned int	num_steps = `10`,
		double	dt = `0.2`,
		bool	update_map = `false`,
		bool	clear_footprint = `false`
	)

A wrapper for checkTrajectory and generateTrajectory that generates and checks a trajectory using the provided twist.

Parameters:

vel	A Twist describing the robot's current velocity
num_steps	The length of the trajectory to generate
dt	Time between samples along the trajectory

Returns:: The maximum cost on the trajectory, or -1 if LETHAL

Definition at line 321 of file costmap_trajectory_checker.cpp.

bool CostmapTrajectoryChecker::clearFootprint	(	const geometry_msgs::Pose2D &	traj,
		bool	update_map
	)

Definition at line 223 of file costmap_trajectory_checker.cpp.

bool CostmapTrajectoryChecker::clearFootprint ( bool update_map )

Definition at line 243 of file costmap_trajectory_checker.cpp.

double CostmapTrajectoryChecker::footprintCost ( const geometry_msgs::Pose2D & pose ) [private]

Returns the max cost in the costmap at the provided position.

Parameters:

x_i
y_i
theta_i

Returns:

Definition at line 356 of file costmap_trajectory_checker.cpp.

void CostmapTrajectoryChecker::generateTrajectory	(	const geometry_msgs::Pose2D &	start_pose,
		const geometry_msgs::Twist &	vel,
		unsigned int	num_steps,
		double	dt,
		std::vector< geometry_msgs::Pose2D > &	traj
	)		`[static]`

Generates a Pose2D trajectory by integrating provided velocity.

Parameters:

start_pose	The starting position of the robot, in world (not costmap) coordinates
vel
traj

Definition at line 174 of file costmap_trajectory_checker.cpp.

std::vector< geometry_msgs::Point > CostmapTrajectoryChecker::getFootprint ( )

Returns copy of footprint_spec_

Definition at line 136 of file costmap_trajectory_checker.cpp.

std::string CostmapTrajectoryChecker::getGlobalFrameID ( )

Definition at line 152 of file costmap_trajectory_checker.cpp.

void CostmapTrajectoryChecker::getOrientedFootprint	(	const geometry_msgs::Pose2D &	pose,
		std::vector< geometry_msgs::Point > &	oriented_footprint
	)		`[private]`

Definition at line 342 of file costmap_trajectory_checker.cpp.

std::string CostmapTrajectoryChecker::getPubTopic ( )

Definition at line 169 of file costmap_trajectory_checker.cpp.

std::string CostmapTrajectoryChecker::getRobotFrameID ( )

Definition at line 157 of file costmap_trajectory_checker.cpp.

bool CostmapTrajectoryChecker::getRobotPose ( geometry_msgs::Pose2D & pose_2d ) const [private]

Retrieves the robot pose and converts to Pose2D representation.

Parameters:

pose_2d Will be set to the pose of the robot in the global frame

Returns:: True if the pose was set successfully, false otherwise

Definition at line 415 of file costmap_trajectory_checker.cpp.

void CostmapTrajectoryChecker::initialize	(	costmap_2d::Costmap2DROS *	costmap_ros,
		std::string	topic = `""`
	)

Initialize

Parameters:

costmap_ros

Definition at line 91 of file costmap_trajectory_checker.cpp.

void CostmapTrajectoryChecker::integratePose	(	const geometry_msgs::Twist &	vel,
		double	dt,
		geometry_msgs::Pose2D &	pose
	)		`[static, private]`

Updates pose by applying provided twist for dt seconds

Parameters:

vel
dt
pose

Definition at line 438 of file costmap_trajectory_checker.cpp.

CostmapTrajectoryChecker & CostmapTrajectoryChecker::operator= ( const CostmapTrajectoryChecker & checker )

Definition at line 78 of file costmap_trajectory_checker.cpp.

void CostmapTrajectoryChecker::pose2DToPose	(	const geometry_msgs::Pose2D	pose_2d,
		geometry_msgs::Pose &	pose
	)		`[static, private]`

Helper function for converting from geometry_msgs::Pose2D to geometry_msgs::Pose.

Parameters:

pose_2d
pose

Definition at line 400 of file costmap_trajectory_checker.cpp.

void CostmapTrajectoryChecker::pose2DToTF	(	const geometry_msgs::Pose2D	pose_2d,
		tf::Pose &	pose_tf
	)		`[static, private]`

Helper function for converting from geometry_msgs::Pose2D to tf::pose.

Parameters:

pose_2d
pose_tf

Definition at line 392 of file costmap_trajectory_checker.cpp.

void CostmapTrajectoryChecker::poseTFToPose2D	(	const tf::Pose &	pose_tf,
		geometry_msgs::Pose2D &	pose_2d
	)		`[static, private]`

Helper function for converting from tf::pose to geometry_msgs::Pose2D Discards linear Z, Roll, and Pitch dimensions.

Parameters:

pose_tf	TF pose to convert
pose_2d	reference to converted 2D pose

Definition at line 375 of file costmap_trajectory_checker.cpp.

void CostmapTrajectoryChecker::poseToPose2D	(	const geometry_msgs::Pose	pose,
		geometry_msgs::Pose2D &	pose_2d
	)		`[static, private]`

Helper function for converting from geometry_msgs::Pose to geometry_msgs::Pose2D.

Parameters:

pose_2d
pose

Definition at line 408 of file costmap_trajectory_checker.cpp.

void CostmapTrajectoryChecker::publishTrajectory ( const std::vector< geometry_msgs::Pose2D > & path ) [private]

Definition at line 446 of file costmap_trajectory_checker.cpp.

void CostmapTrajectoryChecker::setFootprint ( const std::vector< geometry_msgs::Point > & footprint )

Set footprint spec of robot

Parameters:

footprint

Definition at line 118 of file costmap_trajectory_checker.cpp.

void CostmapTrajectoryChecker::setFootprint	(	double	length,
		double	width,
		double	x_offset,
		double	y_offset
	)

Shortcut for generating rectangular footprint from box dimensions

Parameters:

length
width
x_offset
y_offset

Definition at line 123 of file costmap_trajectory_checker.cpp.

void CostmapTrajectoryChecker::setGlobalFrameID ( std::string frame_id )

Getters and setters for frame_ids.

Definition at line 141 of file costmap_trajectory_checker.cpp.

void CostmapTrajectoryChecker::setPubTopic ( std::string topic )

Getter and setter for visualization topic.

Definition at line 162 of file costmap_trajectory_checker.cpp.

void CostmapTrajectoryChecker::setRobotFrameID ( std::string frame_id )

Definition at line 147 of file costmap_trajectory_checker.cpp.

Member Data Documentation

double CostmapTrajectoryChecker::circumscribed_radius_ [private]

Definition at line 194 of file costmap_trajectory_checker.h.

costmap_2d::Costmap2D CostmapTrajectoryChecker::costmap_ [private]

The costmap the controller will use.

Definition at line 186 of file costmap_trajectory_checker.h.

costmap_2d::Costmap2DROS* CostmapTrajectoryChecker::costmap_ros_ [private]

The ROS wrapper for the costmap the controller will use.

Definition at line 185 of file costmap_trajectory_checker.h.

std::vector<geometry_msgs::Point> CostmapTrajectoryChecker::footprint_spec_ [private]

The footprint specification of the robot.

Definition at line 195 of file costmap_trajectory_checker.h.

std::string CostmapTrajectoryChecker::global_frame_ [private]

Used as the global frame id.

Definition at line 193 of file costmap_trajectory_checker.h.

double CostmapTrajectoryChecker::inflation_radius_ [private]

Definition at line 194 of file costmap_trajectory_checker.h.

bool CostmapTrajectoryChecker::initialized_ [private]

Definition at line 196 of file costmap_trajectory_checker.h.

double CostmapTrajectoryChecker::inscribed_radius_ [private]

Definition at line 194 of file costmap_trajectory_checker.h.

ros::NodeHandle CostmapTrajectoryChecker::nh_ [private]

A local node handle.

Definition at line 188 of file costmap_trajectory_checker.h.

std::string CostmapTrajectoryChecker::robot_frame_ [private]

Used as the base frame id of the robot.

Definition at line 192 of file costmap_trajectory_checker.h.

tf::TransformListener CostmapTrajectoryChecker::tl_ [private]

Used for looking up robot pose.

Definition at line 183 of file costmap_trajectory_checker.h.

ros::Publisher CostmapTrajectoryChecker::traj_pub_ [private]

A publisher for visualizing trajectories.

Definition at line 189 of file costmap_trajectory_checker.h.

std::string CostmapTrajectoryChecker::traj_topic_name_ [private]

The topic name for publishing trajectories.

Definition at line 190 of file costmap_trajectory_checker.h.

base_local_planner::WorldModel* CostmapTrajectoryChecker::world_model_ [private]

The world model to use for collision detection.

Definition at line 184 of file costmap_trajectory_checker.h.

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

Public Member Functions

Static Public Member Functions

Private Member Functions

Static Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation