Public Member Functions | Static Public Member Functions | Private Member Functions | Static Private Member Functions | Private Attributes
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 (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::PointgetFootprint ()
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 (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::Costmap2DROScostmap_ros_
 The ROS wrapper for the costmap the controller will use.
std::vector< geometry_msgs::Pointfootprint_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::WorldModelworld_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

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.

Copy constructor.

Definition at line 68 of file costmap_trajectory_checker.cpp.

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:
velA Twist describing the robot's current velocity
num_stepsThe length of the trajectory to generate
dtTime 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:
velA Twist describing the robot's current velocity
num_stepsThe length of the trajectory to generate
dtTime 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.

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_poseThe starting position of the robot, in world (not costmap) coordinates
vel
traj

Definition at line 174 of file costmap_trajectory_checker.cpp.

Returns copy of footprint_spec_

Definition at line 136 of file costmap_trajectory_checker.cpp.

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.

Definition at line 169 of file costmap_trajectory_checker.cpp.

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_2dWill 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_tfTF pose to convert
pose_2dreference 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.

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

Definition at line 194 of file costmap_trajectory_checker.h.

The costmap the controller will use.

Definition at line 186 of file costmap_trajectory_checker.h.

The ROS wrapper for the costmap the controller will use.

Definition at line 185 of file costmap_trajectory_checker.h.

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.

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.

Used for looking up robot pose.

Definition at line 183 of file costmap_trajectory_checker.h.

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.

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:


costmap_trajectory_checker
Author(s): Jonathan Scholz
autogenerated on Tue Jan 7 2014 11:10:54