latched_stop_rotate_controller.h
Go to the documentation of this file.
1 /*
2  * latched_stop_rotate_controller.h
3  *
4  * Created on: Apr 16, 2012
5  * Author: tkruse
6  */
7 
8 #ifndef LATCHED_STOP_ROTATE_CONTROLLER_H_
9 #define LATCHED_STOP_ROTATE_CONTROLLER_H_
10 
11 #include <string>
12 
13 #include <Eigen/Core>
14 
15 #include <tf/transform_datatypes.h>
16 
19 
20 namespace base_local_planner {
21 
23 public:
26 
27  void initialize(const std::string& name = "");
28 
29  bool isPositionReached(LocalPlannerUtil* planner_util,
30  tf::Stamped<tf::Pose> global_pose);
31 
32  bool isGoalReached(LocalPlannerUtil* planner_util,
33  OdometryHelperRos& odom_helper,
34  tf::Stamped<tf::Pose> global_pose);
35 
36  void resetLatching() {
37  xy_tolerance_latch_ = false;
38  }
39 
47  bool stopWithAccLimits(const tf::Stamped<tf::Pose>& global_pose,
48  const tf::Stamped<tf::Pose>& robot_vel,
49  geometry_msgs::Twist& cmd_vel,
50  Eigen::Vector3f acc_lim,
51  double sim_period,
52  boost::function<bool (Eigen::Vector3f pos,
53  Eigen::Vector3f vel,
54  Eigen::Vector3f vel_samples)> obstacle_check);
55 
64  bool rotateToGoal(const tf::Stamped<tf::Pose>& global_pose,
65  const tf::Stamped<tf::Pose>& robot_vel,
66  double goal_th,
67  geometry_msgs::Twist& cmd_vel,
68  Eigen::Vector3f acc_lim,
69  double sim_period,
71  boost::function<bool (Eigen::Vector3f pos,
72  Eigen::Vector3f vel,
73  Eigen::Vector3f vel_samples)> obstacle_check);
74 
75  bool computeVelocityCommandsStopRotate(geometry_msgs::Twist& cmd_vel,
76  Eigen::Vector3f acc_lim,
77  double sim_period,
78  LocalPlannerUtil* planner_util,
79  OdometryHelperRos& odom_helper,
80  tf::Stamped<tf::Pose> global_pose,
81  boost::function<bool (Eigen::Vector3f pos,
82  Eigen::Vector3f vel,
83  Eigen::Vector3f vel_samples)> obstacle_check);
84 
85 private:
86  inline double sign(double x){
87  return x < 0.0 ? -1.0 : 1.0;
88  }
89 
90 
91  // whether to latch at all, and whether in this turn we have already been in goal area
94 };
95 
96 } /* namespace base_local_planner */
97 #endif /* LATCHED_STOP_ROTATE_CONTROLLER_H_ */
bool isGoalReached(LocalPlannerUtil *planner_util, OdometryHelperRos &odom_helper, tf::Stamped< tf::Pose > global_pose)
bool computeVelocityCommandsStopRotate(geometry_msgs::Twist &cmd_vel, Eigen::Vector3f acc_lim, double sim_period, LocalPlannerUtil *planner_util, OdometryHelperRos &odom_helper, tf::Stamped< tf::Pose > global_pose, boost::function< bool(Eigen::Vector3f pos, Eigen::Vector3f vel, Eigen::Vector3f vel_samples)> obstacle_check)
bool stopWithAccLimits(const tf::Stamped< tf::Pose > &global_pose, const tf::Stamped< tf::Pose > &robot_vel, geometry_msgs::Twist &cmd_vel, Eigen::Vector3f acc_lim, double sim_period, boost::function< bool(Eigen::Vector3f pos, Eigen::Vector3f vel, Eigen::Vector3f vel_samples)> obstacle_check)
Stop the robot taking into account acceleration limits.
bool rotateToGoal(const tf::Stamped< tf::Pose > &global_pose, const tf::Stamped< tf::Pose > &robot_vel, double goal_th, geometry_msgs::Twist &cmd_vel, Eigen::Vector3f acc_lim, double sim_period, base_local_planner::LocalPlannerLimits &limits, boost::function< bool(Eigen::Vector3f pos, Eigen::Vector3f vel, Eigen::Vector3f vel_samples)> obstacle_check)
Once a goal position is reached... rotate to the goal orientation.
Helper class implementing infrastructure code many local planner implementations may need...
bool isPositionReached(LocalPlannerUtil *planner_util, tf::Stamped< tf::Pose > global_pose)


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:49