cart_local_planner::CartLocalPlanner Class Reference

#include <cart_local_planner.h>

Inheritance diagram for cart_local_planner::CartLocalPlanner:
Inheritance graph
[legend]

List of all members.

Public Member Functions

 CartLocalPlanner ()
bool computeVelocityCommands (geometry_msgs::Twist &cmd_vel)
void initialize (std::string name, tf::TransformListener *tf, costmap_2d::Costmap2DROS *costmap_ros)
bool isGoalReached ()
bool setPlan (const std::vector< geometry_msgs::PoseStamped > &global_plan)

Protected Types

enum  CONTROL_MODE { REGULAR }
enum  FILTER_OPTIONS { GLOBAL_SCALING = 0x1, CART_ERR_SCALING = GLOBAL_SCALING << 1, ALL = 0xffff }

Protected Member Functions

double baseTwistFromError ()
 * Sets twist_base_ based on robot_pose_error_, returns twist magnitude
double cartTwistFromError ()
 * Sets twist_cart_ based on cart_pose_error_, returns twist magnitutde
bool checkTrajectoryMonotonic (const std::vector< unsigned int > &indices)
virtual bool checkTwists ()
virtual bool checkTwistsMonotonic ()
virtual void controlModeAction ()
virtual void filterTwistsCombined (int filter_options)
void freeze ()
 * set zero twist to arms and base */
bool getNextFewWaypointsIndices (const std::vector< geometry_msgs::PoseStamped > &current_plan, const int &current_waypoint_index, const int &max_num_waypoints, const double &max_translation, const double &max_rotation, std::vector< unsigned int > &waypoint_indices)
virtual void initialization_extras ()
 A place to implement any additional initialization required for the planner. Gets called at the end of initialize().
void invalidPoseCallback (const std_msgs::Empty::ConstPtr &msg)
 * callback for invalid pose notification */
geometry_msgs::Twist mapBaseTwistToCart (const geometry_msgs::Twist &twist_base)
 * map twist_base to cart frame, thanks to KDL's frame*twist operator */
void odomCallback (const nav_msgs::Odometry::ConstPtr &msg)
 * callback for base controller odometry */
void publishDebugPose (tf::Pose &p)
void publishDebugPose (geometry_msgs::Pose &p)
void publishDebugTwist (geometry_msgs::Twist &t)
int setCartGoalFromWaypoint (double min_dist, double min_x_component)
void setCartPoseGoal (const tf::Stamped< tf::Pose > &goal)
virtual void setControlMode ()
 Sets the internal control mode for the computeVelocityCommands switch based on the current state of the robot and the cart.
virtual void setGoalPoses ()
void setRobotPoseGoal (const tf::Stamped< tf::Pose > &goal)
void setYawFromVec (const tf::Pose &pose1, const tf::Pose &pose2, tf::Pose &res)
 * makes a new pose with yaw set from xy coordinate deltas (atan) */
std::vector< geometry_msgs::Point > transformFootprint (const geometry_msgs::PolygonStamped &poly) const
 * converts poly to footprint in the costmap global frame */
bool transformGlobalPlan (const tf::TransformListener &tf, const std::vector< geometry_msgs::PoseStamped > &global_plan, const costmap_2d::Costmap2DROS &costmap, const std::string &global_frame, std::vector< geometry_msgs::PoseStamped > &transformed_plan)
 * Transforms global_plan to plan in global frame, and does pruning */

Static Protected Member Functions

static void scaleTwist2D (geometry_msgs::Twist &t, double scale)
 * scale twist by constant factor */

Protected Attributes

nav_msgs::Odometry base_odom_
CostmapTrajectoryChecker cart_collision_checker_
tf::Stamped< tf::Pose > cart_pose_actual_
geometry_msgs::Pose2D cart_pose_error_
tf::Stamped< tf::Pose > cart_pose_goal_
ros::Publisher cart_pose_pub_
pose_range_2D cart_range
ros::Publisher cart_twist_pub_
enum CONTROL_MODE control_mode_
costmap_2d::Costmap2DROS * costmap_ros_
unsigned int current_waypoint_
bool debug_print_
double dt_
std::vector
< geometry_msgs::PoseStamped > 
global_plan_
ros::Time goal_reached_time_
bool initialized_
boost::mutex invalid_pose_mutex_
ros::Subscriber invalid_pose_sub_
double K_rot_base_
double K_rot_cart_
double K_trans_base_
double K_trans_cart_
ros::Time last_invalid_pose_time_
ros::NodeHandle nh_
int num_traj_steps_
boost::mutex odom_lock_
ros::Subscriber odom_sub_
std::vector
< geometry_msgs::PoseStamped > 
original_global_plan_
ros::Publisher pose2D_pub_
CostmapTrajectoryChecker robot_collision_checker_
tf::Stamped< tf::Pose > robot_pose_actual_
tf::Pose robot_pose_error_
tf::Stamped< tf::Pose > robot_pose_goal_
double rot_stopped_velocity_
boost::shared_ptr
< cart_local_planner::SBPLSubscriber
< cart_pushing_msgs::RobotCartPath > > 
sbpl_subscriber_
bool subscribe_sbpl_plan_
tf::TransformBroadcaster tb_
tf::TransformListener * tf_
double tolerance_rot_
double tolerance_timeout_
double tolerance_trans_
double trans_stopped_velocity_
geometry_msgs::Twist * twist_base_
geometry_msgs::Twist twist_base_max_
geometry_msgs::TwistStamped twist_cart_
geometry_msgs::Twist twist_cart_max_
ros::Publisher vel_pub_

Detailed Description

Definition at line 80 of file cart_local_planner.h.


Member Enumeration Documentation

Enumerator:
REGULAR 

Reimplemented in cart_local_planner::FixedFrontCartPlanner, and cart_local_planner::HolonomicCartPlanner.

Definition at line 201 of file cart_local_planner.h.

Enumerator:
GLOBAL_SCALING 
CART_ERR_SCALING 
ALL 

Reimplemented in cart_local_planner::FixedFrontCartPlanner, and cart_local_planner::HolonomicCartPlanner.

Definition at line 195 of file cart_local_planner.h.


Constructor & Destructor Documentation

cart_local_planner::CartLocalPlanner::CartLocalPlanner (  ) 

Definition at line 135 of file cart_local_planner.cpp.


Member Function Documentation

double cart_local_planner::CartLocalPlanner::baseTwistFromError (  )  [protected]

* Sets twist_base_ based on robot_pose_error_, returns twist magnitude

Definition at line 695 of file cart_local_planner.cpp.

double cart_local_planner::CartLocalPlanner::cartTwistFromError (  )  [protected]

* Sets twist_cart_ based on cart_pose_error_, returns twist magnitutde

Definition at line 704 of file cart_local_planner.cpp.

bool cart_local_planner::CartLocalPlanner::checkTrajectoryMonotonic ( const std::vector< unsigned int > &  indices  )  [protected]

Definition at line 353 of file cart_local_planner.cpp.

bool cart_local_planner::CartLocalPlanner::checkTwists (  )  [protected, virtual]

Check twists for collision in costmap

Returns:
True if not in collision

Definition at line 568 of file cart_local_planner.cpp.

bool cart_local_planner::CartLocalPlanner::checkTwistsMonotonic (  )  [protected, virtual]

Check twists for collision in costmap

Returns:
True if not in collision

Definition at line 602 of file cart_local_planner.cpp.

bool cart_local_planner::CartLocalPlanner::computeVelocityCommands ( geometry_msgs::Twist &  cmd_vel  ) 
void cart_local_planner::CartLocalPlanner::controlModeAction (  )  [protected, virtual]

Implements what to do during computeVelocityCommands for each control mode. This provides a greater degree of control than the default supported by the base_local_planner API, for problems such as rotating in place or evading collisions that the global planner doesn't know about (e.g. cart collisions).

Reimplemented in cart_local_planner::FixedFrontCartPlanner, and cart_local_planner::HolonomicCartPlanner.

Definition at line 458 of file cart_local_planner.cpp.

void cart_local_planner::CartLocalPlanner::filterTwistsCombined ( int  filter_options  )  [protected, virtual]

Applies requested operations on the robot and cart twists

Parameters:
filter_options An int containing OR'ed together FILTER_OPTIONS

1: Scale everything by its max;

2: Scale base vel based on cart error (turns out to be a very handy trick for keeping the cart on track - this is basically a gaussian)

Reimplemented in cart_local_planner::FixedFrontCartPlanner, and cart_local_planner::HolonomicCartPlanner.

Definition at line 521 of file cart_local_planner.cpp.

void cart_local_planner::CartLocalPlanner::freeze (  )  [protected]

* set zero twist to arms and base */

Definition at line 841 of file cart_local_planner.cpp.

bool cart_local_planner::CartLocalPlanner::getNextFewWaypointsIndices ( const std::vector< geometry_msgs::PoseStamped > &  current_plan,
const int &  current_waypoint_index,
const int &  max_num_waypoints,
const double &  max_translation,
const double &  max_rotation,
std::vector< unsigned int > &  waypoint_indices 
) [protected]

Definition at line 403 of file cart_local_planner.cpp.

void cart_local_planner::CartLocalPlanner::initialization_extras (  )  [protected, virtual]

A place to implement any additional initialization required for the planner. Gets called at the end of initialize().

Reimplemented in cart_local_planner::FixedFrontCartPlanner, and cart_local_planner::HolonomicCartPlanner.

Definition at line 238 of file cart_local_planner.cpp.

void cart_local_planner::CartLocalPlanner::initialize ( std::string  name,
tf::TransformListener *  tf,
costmap_2d::Costmap2DROS *  costmap_ros 
)

Definition at line 147 of file cart_local_planner.cpp.

void cart_local_planner::CartLocalPlanner::invalidPoseCallback ( const std_msgs::Empty::ConstPtr &  msg  )  [protected]

* callback for invalid pose notification */

Definition at line 909 of file cart_local_planner.cpp.

bool cart_local_planner::CartLocalPlanner::isGoalReached (  ) 

Definition at line 915 of file cart_local_planner.cpp.

geometry_msgs::Twist cart_local_planner::CartLocalPlanner::mapBaseTwistToCart ( const geometry_msgs::Twist &  twist_base  )  [protected]

* map twist_base to cart frame, thanks to KDL's frame*twist operator */

void cart_local_planner::CartLocalPlanner::odomCallback ( const nav_msgs::Odometry::ConstPtr &  msg  )  [protected]

* callback for base controller odometry */

Definition at line 901 of file cart_local_planner.cpp.

void cart_local_planner::CartLocalPlanner::publishDebugPose ( tf::Pose &  p  )  [protected]

Definition at line 893 of file cart_local_planner.cpp.

void cart_local_planner::CartLocalPlanner::publishDebugPose ( geometry_msgs::Pose &  p  )  [protected]
void cart_local_planner::CartLocalPlanner::publishDebugTwist ( geometry_msgs::Twist &  t  )  [protected]
static void cart_local_planner::CartLocalPlanner::scaleTwist2D ( geometry_msgs::Twist &  t,
double  scale 
) [static, protected]

* scale twist by constant factor */

int cart_local_planner::CartLocalPlanner::setCartGoalFromWaypoint ( double  min_dist,
double  min_x_component 
) [protected]

Picks a future waypoint on the global plan for the cart based on the distance from the robot

Parameters:
min_dist The minimum distance a future waypoint must be in order to qualify as a cart goal pose
min_x_component The amount of min_dist that must be in the X direction
Returns:
The waypoint number chosen to set the target cart pose, or -1 on failure

Definition at line 644 of file cart_local_planner.cpp.

void cart_local_planner::CartLocalPlanner::setCartPoseGoal ( const tf::Stamped< tf::Pose > &  goal  )  [protected]

Definition at line 443 of file cart_local_planner.cpp.

void cart_local_planner::CartLocalPlanner::setControlMode (  )  [protected, virtual]

Sets the internal control mode for the computeVelocityCommands switch based on the current state of the robot and the cart.

Reimplemented in cart_local_planner::FixedFrontCartPlanner, and cart_local_planner::HolonomicCartPlanner.

Definition at line 453 of file cart_local_planner.cpp.

void cart_local_planner::CartLocalPlanner::setGoalPoses (  )  [protected, virtual]

Sets goal poses for the cart and the robot given current state and way-point

Definition at line 482 of file cart_local_planner.cpp.

bool cart_local_planner::CartLocalPlanner::setPlan ( const std::vector< geometry_msgs::PoseStamped > &  global_plan  ) 
void cart_local_planner::CartLocalPlanner::setRobotPoseGoal ( const tf::Stamped< tf::Pose > &  goal  )  [protected]

Definition at line 437 of file cart_local_planner.cpp.

void cart_local_planner::CartLocalPlanner::setYawFromVec ( const tf::Pose &  pose1,
const tf::Pose &  pose2,
tf::Pose &  res 
) [protected]

* makes a new pose with yaw set from xy coordinate deltas (atan) */

Definition at line 716 of file cart_local_planner.cpp.

std::vector<geometry_msgs::Point> cart_local_planner::CartLocalPlanner::transformFootprint ( const geometry_msgs::PolygonStamped &  poly  )  const [protected]

* converts poly to footprint in the costmap global frame */

bool cart_local_planner::CartLocalPlanner::transformGlobalPlan ( const tf::TransformListener &  tf,
const std::vector< geometry_msgs::PoseStamped > &  global_plan,
const costmap_2d::Costmap2DROS &  costmap,
const std::string &  global_frame,
std::vector< geometry_msgs::PoseStamped > &  transformed_plan 
) [protected]

* Transforms global_plan to plan in global frame, and does pruning */


Member Data Documentation

nav_msgs::Odometry cart_local_planner::CartLocalPlanner::base_odom_ [protected]

Definition at line 219 of file cart_local_planner.h.

Definition at line 215 of file cart_local_planner.h.

Cart pose in robot frame

Definition at line 226 of file cart_local_planner.h.

geometry_msgs::Pose2D cart_local_planner::CartLocalPlanner::cart_pose_error_ [protected]

Distance from actual cart pose to goal pose

Definition at line 234 of file cart_local_planner.h.

tf::Stamped<tf::Pose> cart_local_planner::CartLocalPlanner::cart_pose_goal_ [protected]

Cart pose in robot frame

Definition at line 230 of file cart_local_planner.h.

Publisher for controlling cart position

Definition at line 253 of file cart_local_planner.h.

Definition at line 257 of file cart_local_planner.h.

Publisher for controlling cart velocity

Definition at line 254 of file cart_local_planner.h.

costmap_2d::Costmap2DROS* cart_local_planner::CartLocalPlanner::costmap_ros_ [protected]

Definition at line 212 of file cart_local_planner.h.

Waypoint counter

Definition at line 245 of file cart_local_planner.h.

Definition at line 263 of file cart_local_planner.h.

Trajectory sample time-interval

Definition at line 244 of file cart_local_planner.h.

std::vector<geometry_msgs::PoseStamped> cart_local_planner::CartLocalPlanner::global_plan_ [protected]

Definition at line 221 of file cart_local_planner.h.

Definition at line 220 of file cart_local_planner.h.

Definition at line 240 of file cart_local_planner.h.

Definition at line 222 of file cart_local_planner.h.

Definition at line 218 of file cart_local_planner.h.

P-gains for the base

Definition at line 241 of file cart_local_planner.h.

P-gains for the cart

Definition at line 242 of file cart_local_planner.h.

Definition at line 241 of file cart_local_planner.h.

Definition at line 242 of file cart_local_planner.h.

Definition at line 267 of file cart_local_planner.h.

ros::NodeHandle cart_local_planner::CartLocalPlanner::nh_ [protected]

Definition at line 210 of file cart_local_planner.h.

Trajectory length

Definition at line 243 of file cart_local_planner.h.

Definition at line 222 of file cart_local_planner.h.

Definition at line 218 of file cart_local_planner.h.

std::vector<geometry_msgs::PoseStamped> cart_local_planner::CartLocalPlanner::original_global_plan_ [protected]

Definition at line 270 of file cart_local_planner.h.

Definition at line 259 of file cart_local_planner.h.

Definition at line 214 of file cart_local_planner.h.

Robot pose in fixed frame

Definition at line 225 of file cart_local_planner.h.

Robot goal pose in current robot frame

Definition at line 233 of file cart_local_planner.h.

tf::Stamped<tf::Pose> cart_local_planner::CartLocalPlanner::robot_pose_goal_ [protected]

Robot pose in fixed frame

Definition at line 229 of file cart_local_planner.h.

Used to check if base is stopped at goal

Definition at line 247 of file cart_local_planner.h.

boost::shared_ptr<cart_local_planner::SBPLSubscriber<cart_pushing_msgs::RobotCartPath> > cart_local_planner::CartLocalPlanner::sbpl_subscriber_ [protected]

Definition at line 269 of file cart_local_planner.h.

Definition at line 250 of file cart_local_planner.h.

tf::TransformBroadcaster cart_local_planner::CartLocalPlanner::tb_ [protected]

Definition at line 209 of file cart_local_planner.h.

tf::TransformListener* cart_local_planner::CartLocalPlanner::tf_ [protected]

Definition at line 208 of file cart_local_planner.h.

Definition at line 246 of file cart_local_planner.h.

Goal threshold tolerances

Definition at line 246 of file cart_local_planner.h.

Definition at line 246 of file cart_local_planner.h.

Definition at line 247 of file cart_local_planner.h.

geometry_msgs::Twist* cart_local_planner::CartLocalPlanner::twist_base_ [protected]

Pointer to cmd_vel, the Twist for returning to move_base

Definition at line 237 of file cart_local_planner.h.

Definition at line 260 of file cart_local_planner.h.

geometry_msgs::TwistStamped cart_local_planner::CartLocalPlanner::twist_cart_ [protected]

TwistStamped for writing to cart command_vel topic

Definition at line 238 of file cart_local_planner.h.

Definition at line 260 of file cart_local_planner.h.

Definition at line 217 of file cart_local_planner.h.


The documentation for this class was generated from the following files:
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator


cart_local_planner
Author(s): Jonathan Scholz
autogenerated on Fri Jan 11 09:55:10 2013