/** * returns true if we have passed the goal position. * Meaning we might have overshot on the position beyond tolerance, yet still return true. * Also goal orientation might not yet be true */ boolLatchedStopRotateController::isPositionReached(LocalPlannerUtil* planner_util, tf::Stamped<tf::Pose> global_pose) { double xy_goal_tolerance = planner_util->getCurrentLimits().xy_goal_tolerance;
// 认为 global goal 是 global plan的最后一个点 // 显然是把全局路径的最后一个点赋值给 goal_pose tf::Stamped<tf::Pose> goal_pose; if ( ! planner_util->getGoal(goal_pose)) { returnfalse; } double goal_x = goal_pose.getOrigin().getX(); double goal_y = goal_pose.getOrigin().getY();
// check to see if we've reached the goal position if ( (latch_xy_goal_tolerance_ && xy_tolerance_latch_) || base_local_planner::getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance) { xy_tolerance_latch_ = true; returntrue; } returnfalse; }
else { tf::Stamped<tf::Pose> robot_vel; odom_helper_.getRobotVel(robot_vel); nav_msgs::Odometry base_odom; odom_helper_.getOdom(base_odom); // if we're not stopped yet... we want to stop... // taking into account the acceleration limits of the robot
/* 这里是机器人在继续运动的情况 rotating_to_goal_ 在构造函数里是 false stopped函数: 如果里程计的角速度小于 rot_stopped_vel,且线速度小于 trans_stopped_vel, 则返回 true,认为已经停止 */ if ( ! rotating_to_goal_ && !base_local_planner::stopped( base_odom, limits.rot_stopped_vel, limits.trans_stopped_vel) ) { if ( ! stopWithAccLimits( global_pose, robot_vel, cmd_vel, acc_lim, sim_period, obstacle_check) ) { ROS_INFO("Error when stopping."); returnfalse; } ROS_DEBUG("Stopping..."); } //if we're stopped... then we want to rotate to goal else { //set this so that we know its OK to be moving rotating_to_goal_ = true; if ( ! rotateToGoal( global_pose, robot_vel, goal_th, cmd_vel, acc_lim, sim_period, limits, obstacle_check) ) { ROS_INFO("Error when rotating."); returnfalse; } ROS_DEBUG("Rotating..."); } } returntrue; }