Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include <dwb_plugins/simple_goal_checker.h>
00036 #include <pluginlib/class_list_macros.h>
00037 #include <angles/angles.h>
00038
00039 namespace dwb_plugins
00040 {
00041
00042 SimpleGoalChecker::SimpleGoalChecker() :
00043 xy_goal_tolerance_(0.25), yaw_goal_tolerance_(0.25), stateful_(true), check_xy_(true), xy_goal_tolerance_sq_(0.0625)
00044 {
00045 }
00046
00047 void SimpleGoalChecker::initialize(const ros::NodeHandle& nh)
00048 {
00049 nh.param("xy_goal_tolerance", xy_goal_tolerance_, 0.25);
00050 nh.param("yaw_goal_tolerance", yaw_goal_tolerance_, 0.25);
00051 nh.param("stateful", stateful_, true);
00052 xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
00053 }
00054
00055 void SimpleGoalChecker::reset()
00056 {
00057 check_xy_ = true;
00058 }
00059
00060 bool SimpleGoalChecker::isGoalReached(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose,
00061 const nav_2d_msgs::Twist2D& velocity)
00062 {
00063 if (check_xy_)
00064 {
00065 double dx = query_pose.x - goal_pose.x,
00066 dy = query_pose.y - goal_pose.y;
00067 if (dx * dx + dy * dy > xy_goal_tolerance_sq_)
00068 {
00069 return false;
00070 }
00071
00072
00073 if (stateful_)
00074 {
00075 check_xy_ = false;
00076 }
00077 }
00078 double dyaw = angles::shortest_angular_distance(query_pose.theta, goal_pose.theta);
00079 return fabs(dyaw) < yaw_goal_tolerance_;
00080 }
00081
00082 }
00083
00084 PLUGINLIB_EXPORT_CLASS(dwb_plugins::SimpleGoalChecker, dwb_local_planner::GoalChecker)