base_local_planner: latched_stop_rotate

Go to the documentation of this file.
00001 /*
00002  * latched_stop_rotate_controller.h
00003  *
00004  *  Created on: Apr 16, 2012
00005  *      Author: tkruse
00006  */
00007 
00008 #ifndef LATCHED_STOP_ROTATE_CONTROLLER_H_
00009 #define LATCHED_STOP_ROTATE_CONTROLLER_H_
00010 
00011 #include <string>
00012 
00013 #include <Eigen/Core>
00014 
00015 #include <tf/transform_datatypes.h>
00016 
00017 #include <base_local_planner/local_planner_util.h>
00018 #include <base_local_planner/odometry_helper_ros.h>
00019 
00020 namespace base_local_planner {
00021 
00022 class LatchedStopRotateController {
00023 public:
00024   LatchedStopRotateController(const std::string& name = "");
00025   virtual ~LatchedStopRotateController();
00026 
00027   bool isPositionReached(LocalPlannerUtil* planner_util,
00028       tf::Stamped<tf::Pose> global_pose);
00029 
00030   bool isGoalReached(LocalPlannerUtil* planner_util,
00031       OdometryHelperRos& odom_helper,
00032       tf::Stamped<tf::Pose> global_pose);
00033 
00034   void resetLatching() {
00035     xy_tolerance_latch_ = false;
00036   }
00037 
00045   bool stopWithAccLimits(const tf::Stamped<tf::Pose>& global_pose,
00046       const tf::Stamped<tf::Pose>& robot_vel,
00047       geometry_msgs::Twist& cmd_vel,
00048       Eigen::Vector3f acc_lim,
00049       double sim_period,
00050       boost::function<bool (Eigen::Vector3f pos,
00051                             Eigen::Vector3f vel,
00052                             Eigen::Vector3f vel_samples)> obstacle_check);
00053 
00062   bool rotateToGoal(const tf::Stamped<tf::Pose>& global_pose,
00063       const tf::Stamped<tf::Pose>& robot_vel,
00064       double goal_th,
00065       geometry_msgs::Twist& cmd_vel,
00066       Eigen::Vector3f acc_lim,
00067       double sim_period,
00068       base_local_planner::LocalPlannerLimits& limits,
00069       boost::function<bool (Eigen::Vector3f pos,
00070                             Eigen::Vector3f vel,
00071                             Eigen::Vector3f vel_samples)> obstacle_check);
00072 
00073   bool computeVelocityCommandsStopRotate(geometry_msgs::Twist& cmd_vel,
00074       Eigen::Vector3f acc_lim,
00075       double sim_period,
00076       LocalPlannerUtil* planner_util,
00077       OdometryHelperRos& odom_helper,
00078       tf::Stamped<tf::Pose> global_pose,
00079       boost::function<bool (Eigen::Vector3f pos,
00080                             Eigen::Vector3f vel,
00081                             Eigen::Vector3f vel_samples)> obstacle_check);
00082 
00083 private:
00084   inline double sign(double x){
00085     return x < 0.0 ? -1.0 : 1.0;
00086   }
00087 
00088 
00089   // whether to latch at all, and whether in this turn we have already been in goal area
00090   bool latch_xy_goal_tolerance_, xy_tolerance_latch_;
00091   bool rotating_to_goal_;
00092 };
00093 
00094 } /* namespace base_local_planner */
00095 #endif /* LATCHED_STOP_ROTATE_CONTROLLER_H_ */