local_planner/src/nodes/local_planner.cpp
// stop in front of an obstacle at a distance defined by the variable
// keep_distance_
stop in front of an obstacle at a distance defined by the variable
keep_distance_
stop in front of an obstacle at a distance defined by the variable keep_distance_
void LocalPlanner::stopInFrontObstacles() {
  if (first_brake_) {
    double braking_distance =
        fabsf(distance_to_closest_point_ - keep_distance_);
    Eigen::Vector2f pose_to_goal(goal_.x - pose_.pose.position.x,
                                 goal_.y - pose_.pose.position.y);
    goal_.x = pose_.pose.position.x +
              (braking_distance * pose_to_goal(0) / pose_to_goal.norm());
    goal_.y = pose_.pose.position.y +
              (braking_distance * pose_to_goal(1) / pose_to_goal.norm());
    first_brake_ = false;
    stop_in_front_active_ = true;
  }
  ROS_INFO(
      "\033[0;35m [OA] New Stop Goal: [%.2f %.2f %.2f], obstacle distance "
      "%.2f. \033[0m",
      goal_.x, goal_.y, goal_.z, distance_to_closest_point_);
}
void LocalPlanner :: stopInFrontObstacles ( ) { if ( first_brake_ ) { double braking_distance = fabsf ( distance_to_closest_point_ - keep_distance_ ) ; Eigen :: Vector2f pose_to_goal ( goal_ . x - pose_ . pose . position . x , goal_ . y - pose_ . pose . position . y ) ; goal_ . x = pose_ . pose . position . x + ( braking_distance * pose_to_goal ( 0 ) / pose_to_goal . norm ( ) ) ; goal_ . y = pose_ . pose . position . y + ( braking_distance * pose_to_goal ( 1 ) / pose_to_goal . norm ( ) ) ; first_brake_ = false ; stop_in_front_active_ = true ; } ROS_INFO ( " \033 " " \033 " , goal_ . x , goal_ . y , goal_ . z , distance_to_closest_point_ ) ; }
stop in front of an obstacle at a distance defined by the variable
keep_distance_
stop in front of an obstacle at a distance defined by the variable keep_distance_