Go to the documentation of this file.00001
00002
00003
00004
00005
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
00090 bool latch_xy_goal_tolerance_, xy_tolerance_latch_;
00091 bool rotating_to_goal_;
00092 };
00093
00094 }
00095 #endif