CostmapTrajectoryChecker Class Reference

#include <costmap_trajectory_checker.h>

List of all members.

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 (bool update_map)
bool clearFootprint (const geometry_msgs::Pose2D &traj, bool update_map)
 CostmapTrajectoryChecker (const CostmapTrajectoryChecker &checker)
 Copy constructor.
 CostmapTrajectoryChecker (costmap_2d::Costmap2DROS *costmap_ros, std::string topic="")
 CostmapTrajectoryChecker (costmap_2d::Costmap2DROS *costmap_ros, std::string frame_id, std::vector< geometry_msgs::Point > footprint, std::string topic="")
 CostmapTrajectoryChecker ()
 Default 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="")
CostmapTrajectoryCheckeroperator= (const CostmapTrajectoryChecker &checker)
void setFootprint (double length, double width, double x_offset, double y_offset)
void setFootprint (const std::vector< geometry_msgs::Point > &footprint)
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:
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 ( bool  update_map  ) 

Definition at line 243 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.

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 
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 ( 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::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::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

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.

Used as the global frame id.

Definition at line 193 of file costmap_trajectory_checker.h.

Definition at line 194 of file costmap_trajectory_checker.h.

Definition at line 196 of file costmap_trajectory_checker.h.

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.

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.

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:
 All Classes Files Functions Variables


costmap_trajectory_checker
Author(s): Jonathan Scholz
autogenerated on Fri Jan 11 09:51:16 2013