#include <cart_local_planner.h>
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 > ¤t_plan, const int ¤t_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_ |
Definition at line 80 of file cart_local_planner.h.
enum cart_local_planner::CartLocalPlanner::CONTROL_MODE [protected] |
Reimplemented in cart_local_planner::FixedFrontCartPlanner, and cart_local_planner::HolonomicCartPlanner.
Definition at line 201 of file cart_local_planner.h.
enum cart_local_planner::CartLocalPlanner::FILTER_OPTIONS [protected] |
Reimplemented in cart_local_planner::FixedFrontCartPlanner, and cart_local_planner::HolonomicCartPlanner.
Definition at line 195 of file cart_local_planner.h.
cart_local_planner::CartLocalPlanner::CartLocalPlanner | ( | ) |
Definition at line 135 of file cart_local_planner.cpp.
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
Definition at line 568 of file cart_local_planner.cpp.
bool cart_local_planner::CartLocalPlanner::checkTwistsMonotonic | ( | ) | [protected, virtual] |
Check twists for collision in costmap
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
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
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 |
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 */
nav_msgs::Odometry cart_local_planner::CartLocalPlanner::base_odom_ [protected] |
Definition at line 219 of file cart_local_planner.h.
CostmapTrajectoryChecker cart_local_planner::CartLocalPlanner::cart_collision_checker_ [protected] |
Definition at line 215 of file cart_local_planner.h.
tf::Stamped<tf::Pose> cart_local_planner::CartLocalPlanner::cart_pose_actual_ [protected] |
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.
ros::Publisher cart_local_planner::CartLocalPlanner::cart_pose_pub_ [protected] |
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.
ros::Publisher cart_local_planner::CartLocalPlanner::cart_twist_pub_ [protected] |
Publisher for controlling cart velocity
Definition at line 254 of file cart_local_planner.h.
enum CONTROL_MODE cart_local_planner::CartLocalPlanner::control_mode_ [protected] |
Reimplemented in cart_local_planner::FixedFrontCartPlanner, and cart_local_planner::HolonomicCartPlanner.
Definition at line 206 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.
unsigned int cart_local_planner::CartLocalPlanner::current_waypoint_ [protected] |
Waypoint counter
Definition at line 245 of file cart_local_planner.h.
bool cart_local_planner::CartLocalPlanner::debug_print_ [protected] |
Definition at line 263 of file cart_local_planner.h.
double cart_local_planner::CartLocalPlanner::dt_ [protected] |
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.
ros::Time cart_local_planner::CartLocalPlanner::goal_reached_time_ [protected] |
Definition at line 220 of file cart_local_planner.h.
bool cart_local_planner::CartLocalPlanner::initialized_ [protected] |
Definition at line 240 of file cart_local_planner.h.
boost::mutex cart_local_planner::CartLocalPlanner::invalid_pose_mutex_ [protected] |
Definition at line 222 of file cart_local_planner.h.
ros::Subscriber cart_local_planner::CartLocalPlanner::invalid_pose_sub_ [protected] |
Definition at line 218 of file cart_local_planner.h.
double cart_local_planner::CartLocalPlanner::K_rot_base_ [protected] |
P-gains for the base
Definition at line 241 of file cart_local_planner.h.
double cart_local_planner::CartLocalPlanner::K_rot_cart_ [protected] |
P-gains for the cart
Definition at line 242 of file cart_local_planner.h.
double cart_local_planner::CartLocalPlanner::K_trans_base_ [protected] |
Definition at line 241 of file cart_local_planner.h.
double cart_local_planner::CartLocalPlanner::K_trans_cart_ [protected] |
Definition at line 242 of file cart_local_planner.h.
ros::Time cart_local_planner::CartLocalPlanner::last_invalid_pose_time_ [protected] |
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.
int cart_local_planner::CartLocalPlanner::num_traj_steps_ [protected] |
Trajectory length
Definition at line 243 of file cart_local_planner.h.
boost::mutex cart_local_planner::CartLocalPlanner::odom_lock_ [protected] |
Definition at line 222 of file cart_local_planner.h.
ros::Subscriber cart_local_planner::CartLocalPlanner::odom_sub_ [protected] |
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.
ros::Publisher cart_local_planner::CartLocalPlanner::pose2D_pub_ [protected] |
Definition at line 259 of file cart_local_planner.h.
CostmapTrajectoryChecker cart_local_planner::CartLocalPlanner::robot_collision_checker_ [protected] |
Definition at line 214 of file cart_local_planner.h.
tf::Stamped<tf::Pose> cart_local_planner::CartLocalPlanner::robot_pose_actual_ [protected] |
Robot pose in fixed frame
Definition at line 225 of file cart_local_planner.h.
tf::Pose cart_local_planner::CartLocalPlanner::robot_pose_error_ [protected] |
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.
double cart_local_planner::CartLocalPlanner::rot_stopped_velocity_ [protected] |
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.
bool cart_local_planner::CartLocalPlanner::subscribe_sbpl_plan_ [protected] |
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.
double cart_local_planner::CartLocalPlanner::tolerance_rot_ [protected] |
Definition at line 246 of file cart_local_planner.h.
double cart_local_planner::CartLocalPlanner::tolerance_timeout_ [protected] |
Goal threshold tolerances
Definition at line 246 of file cart_local_planner.h.
double cart_local_planner::CartLocalPlanner::tolerance_trans_ [protected] |
Definition at line 246 of file cart_local_planner.h.
double cart_local_planner::CartLocalPlanner::trans_stopped_velocity_ [protected] |
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.
geometry_msgs::Twist cart_local_planner::CartLocalPlanner::twist_base_max_ [protected] |
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.
geometry_msgs::Twist cart_local_planner::CartLocalPlanner::twist_cart_max_ [protected] |
Definition at line 260 of file cart_local_planner.h.
ros::Publisher cart_local_planner::CartLocalPlanner::vel_pub_ [protected] |
Definition at line 217 of file cart_local_planner.h.