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 <Eigen/Core>
00012
00013 #include <tf/transform_datatypes.h>
00014
00015 #include <base_local_planner/local_planner_util.h>
00016 #include <base_local_planner/odometry_helper_ros.h>
00017
00018 namespace base_local_planner {
00019
00020 class LatchedStopRotateController {
00021 public:
00022 LatchedStopRotateController();
00023 virtual ~LatchedStopRotateController();
00024
00025 bool isPositionReached(LocalPlannerUtil* planner_util,
00026 tf::Stamped<tf::Pose> global_pose);
00027
00028 bool isGoalReached(LocalPlannerUtil* planner_util,
00029 OdometryHelperRos& odom_helper,
00030 tf::Stamped<tf::Pose> global_pose);
00031
00032 void resetLatching() {
00033 xy_tolerance_latch_ = false;
00034 }
00035
00043 bool stopWithAccLimits(const tf::Stamped<tf::Pose>& global_pose,
00044 const tf::Stamped<tf::Pose>& robot_vel,
00045 geometry_msgs::Twist& cmd_vel,
00046 Eigen::Vector3f acc_lim,
00047 double sim_period,
00048 boost::function<bool (Eigen::Vector3f pos,
00049 Eigen::Vector3f vel,
00050 Eigen::Vector3f vel_samples)> obstacle_check);
00051
00060 bool rotateToGoal(const tf::Stamped<tf::Pose>& global_pose,
00061 const tf::Stamped<tf::Pose>& robot_vel,
00062 double goal_th,
00063 geometry_msgs::Twist& cmd_vel,
00064 Eigen::Vector3f acc_lim,
00065 double sim_period,
00066 base_local_planner::LocalPlannerLimits& limits,
00067 boost::function<bool (Eigen::Vector3f pos,
00068 Eigen::Vector3f vel,
00069 Eigen::Vector3f vel_samples)> obstacle_check);
00070
00071 bool computeVelocityCommandsStopRotate(geometry_msgs::Twist& cmd_vel,
00072 Eigen::Vector3f acc_lim,
00073 double sim_period,
00074 LocalPlannerUtil* planner_util,
00075 OdometryHelperRos& odom_helper,
00076 tf::Stamped<tf::Pose> global_pose,
00077 boost::function<bool (Eigen::Vector3f pos,
00078 Eigen::Vector3f vel,
00079 Eigen::Vector3f vel_samples)> obstacle_check);
00080
00081 private:
00082 inline double sign(double x){
00083 return x < 0.0 ? -1.0 : 1.0;
00084 }
00085
00086
00087
00088 bool latch_xy_goal_tolerance_, xy_tolerance_latch_;
00089 bool rotating_to_goal_;
00090 };
00091
00092 }
00093 #endif