From 7b0ad95c1e9c5771e9a09db832a06ecbc2ff99fe Mon Sep 17 00:00:00 2001 From: LucasNolasco Date: Wed, 17 Mar 2021 13:04:43 -0300 Subject: [PATCH 1/4] Fixed goal reached checking --- .../teb_local_planner/teb_local_planner_ros.h | 4 +- src/teb_local_planner_ros.cpp | 58 +++++++++++-------- 2 files changed, 38 insertions(+), 24 deletions(-) diff --git a/include/teb_local_planner/teb_local_planner_ros.h b/include/teb_local_planner/teb_local_planner_ros.h index 8f60f7ad..1bfa88a8 100644 --- a/include/teb_local_planner/teb_local_planner_ros.h +++ b/include/teb_local_planner/teb_local_planner_ros.h @@ -426,7 +426,7 @@ class TebLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap PoseSE2 robot_pose_; //!< Store current robot pose PoseSE2 robot_goal_; //!< Store current robot goal geometry_msgs::Twist robot_vel_; //!< Store current robot translational and angular velocity (vx, vy, omega) - bool goal_reached_; //!< store whether the goal is reached or not + // bool goal_reached_; //!< store whether the goal is reached or not ros::Time time_last_infeasible_plan_; //!< Store at which time stamp the last infeasible plan was detected int no_infeasible_plans_; //!< Store how many times in a row the planner failed to find a feasible plan. ros::Time time_last_oscillation_; //!< Store at which time stamp the last oscillation was detected @@ -444,6 +444,8 @@ class TebLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap // flags bool initialized_; //!< Keeps track about the correct initialization of this class + geometry_msgs::PoseStamped global_goal; + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/src/teb_local_planner_ros.cpp b/src/teb_local_planner_ros.cpp index 7b8f3d7e..d05a612d 100644 --- a/src/teb_local_planner_ros.cpp +++ b/src/teb_local_planner_ros.cpp @@ -68,7 +68,7 @@ namespace teb_local_planner TebLocalPlannerROS::TebLocalPlannerROS() : costmap_ros_(NULL), tf_(NULL), costmap_model_(NULL), costmap_converter_loader_("costmap_converter", "costmap_converter::BaseCostmapToPolygons"), - dynamic_recfg_(NULL), custom_via_points_active_(false), goal_reached_(false), no_infeasible_plans_(0), + dynamic_recfg_(NULL), custom_via_points_active_(false), no_infeasible_plans_(0), last_preferred_rotdir_(RotType::none), initialized_(false) { } @@ -212,9 +212,6 @@ bool TebLocalPlannerROS::setPlan(const std::vector& // we do not clear the local planner here, since setPlan is called frequently whenever the global planner updates the plan. // the local planner checks whether it is required to reinitialize the trajectory or not within each velocity computation step. - - // reset goal_reached_ flag - goal_reached_ = false; return true; } @@ -248,7 +245,7 @@ uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt cmd_vel.header.stamp = ros::Time::now(); cmd_vel.header.frame_id = robot_base_frame_; cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0; - goal_reached_ = false; + // goal_reached_ = false; // Get robot pose geometry_msgs::PoseStamped robot_pose; @@ -281,22 +278,8 @@ uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt if (!custom_via_points_active_) updateViaPointsContainer(transformed_plan, cfg_.trajectory.global_plan_viapoint_sep); - nav_msgs::Odometry base_odom; - odom_helper_.getOdom(base_odom); - // check if global goal is reached - geometry_msgs::PoseStamped global_goal; - tf2::doTransform(global_plan_.back(), global_goal, tf_plan_to_global); - double dx = global_goal.pose.position.x - robot_pose_.x(); - double dy = global_goal.pose.position.y - robot_pose_.y(); - double delta_orient = g2o::normalize_theta( tf2::getYaw(global_goal.pose.orientation) - robot_pose_.theta() ); - if(fabs(std::sqrt(dx*dx+dy*dy)) < cfg_.goal_tolerance.xy_goal_tolerance - && fabs(delta_orient) < cfg_.goal_tolerance.yaw_goal_tolerance - && (!cfg_.goal_tolerance.complete_global_plan || via_points_.size() == 0) - && (base_local_planner::stopped(base_odom, cfg_.goal_tolerance.theta_stopped_vel, cfg_.goal_tolerance.trans_stopped_vel) - || cfg_.goal_tolerance.free_goal_vel)) - { - goal_reached_ = true; + if(this->isGoalReached()) { return mbf_msgs::ExePathResult::SUCCESS; } @@ -460,12 +443,41 @@ uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt bool TebLocalPlannerROS::isGoalReached() { - if (goal_reached_) + if(!initialized_) { + return false; + } + + geometry_msgs::PoseStamped robot_pose; + costmap_ros_->getRobotPose(robot_pose); + robot_pose_ = PoseSE2(robot_pose.pose); + + nav_msgs::Odometry base_odom; + odom_helper_.getOdom(base_odom); + + std::vector transformed_plan; + int goal_idx; + geometry_msgs::TransformStamped tf_plan_to_global; + if (!transformGlobalPlan(*tf_, global_plan_, robot_pose, *costmap_, global_frame_, cfg_.trajectory.max_global_plan_lookahead_dist, + transformed_plan, &goal_idx, &tf_plan_to_global)) { - ROS_INFO("GOAL Reached!"); - planner_->clearPlanner(); + ROS_WARN("Could not transform the global plan to the frame of the controller"); + return false; + } + + tf2::doTransform(global_plan_.back(), global_goal, tf_plan_to_global); + double dx = global_goal.pose.position.x - robot_pose_.x(); + double dy = global_goal.pose.position.y - robot_pose_.y(); + double delta_orient = g2o::normalize_theta( tf2::getYaw(global_goal.pose.orientation) - robot_pose_.theta() ); + if(fabs(std::sqrt(dx*dx+dy*dy)) < cfg_.goal_tolerance.xy_goal_tolerance + && fabs(delta_orient) < cfg_.goal_tolerance.yaw_goal_tolerance + && (!cfg_.goal_tolerance.complete_global_plan || via_points_.size() == 0) + && (base_local_planner::stopped(base_odom, cfg_.goal_tolerance.theta_stopped_vel, cfg_.goal_tolerance.trans_stopped_vel) + || cfg_.goal_tolerance.free_goal_vel)) + { + ROS_INFO("GOT A SUCCES WHILE CHECKING"); return true; } + return false; } From bb6a1553ba728157adc1d8c1599715ca8eb5cb4e Mon Sep 17 00:00:00 2001 From: LucasNolasco Date: Fri, 19 Mar 2021 10:18:53 -0300 Subject: [PATCH 2/4] isGoalReached method reorganized --- .../teb_local_planner/teb_local_planner_ros.h | 3 - src/teb_local_planner_ros.cpp | 60 ++++++++++--------- 2 files changed, 32 insertions(+), 31 deletions(-) diff --git a/include/teb_local_planner/teb_local_planner_ros.h b/include/teb_local_planner/teb_local_planner_ros.h index 1bfa88a8..dfdd0d9e 100644 --- a/include/teb_local_planner/teb_local_planner_ros.h +++ b/include/teb_local_planner/teb_local_planner_ros.h @@ -426,7 +426,6 @@ class TebLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap PoseSE2 robot_pose_; //!< Store current robot pose PoseSE2 robot_goal_; //!< Store current robot goal geometry_msgs::Twist robot_vel_; //!< Store current robot translational and angular velocity (vx, vy, omega) - // bool goal_reached_; //!< store whether the goal is reached or not ros::Time time_last_infeasible_plan_; //!< Store at which time stamp the last infeasible plan was detected int no_infeasible_plans_; //!< Store how many times in a row the planner failed to find a feasible plan. ros::Time time_last_oscillation_; //!< Store at which time stamp the last oscillation was detected @@ -444,8 +443,6 @@ class TebLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap // flags bool initialized_; //!< Keeps track about the correct initialization of this class - geometry_msgs::PoseStamped global_goal; - public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/src/teb_local_planner_ros.cpp b/src/teb_local_planner_ros.cpp index d05a612d..28aad5df 100644 --- a/src/teb_local_planner_ros.cpp +++ b/src/teb_local_planner_ros.cpp @@ -279,7 +279,7 @@ uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt updateViaPointsContainer(transformed_plan, cfg_.trajectory.global_plan_viapoint_sep); // check if global goal is reached - if(this->isGoalReached()) { + if(isGoalReached()) { return mbf_msgs::ExePathResult::SUCCESS; } @@ -443,39 +443,43 @@ uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt bool TebLocalPlannerROS::isGoalReached() { - if(!initialized_) { + if(!initialized_ || global_plan_.empty()) /* check if the planner was initialized and it there is a global plan */ + { return false; } - geometry_msgs::PoseStamped robot_pose; - costmap_ros_->getRobotPose(robot_pose); - robot_pose_ = PoseSE2(robot_pose.pose); - - nav_msgs::Odometry base_odom; - odom_helper_.getOdom(base_odom); - - std::vector transformed_plan; - int goal_idx; - geometry_msgs::TransformStamped tf_plan_to_global; - if (!transformGlobalPlan(*tf_, global_plan_, robot_pose, *costmap_, global_frame_, cfg_.trajectory.max_global_plan_lookahead_dist, - transformed_plan, &goal_idx, &tf_plan_to_global)) + // check if global goal is reached + try { - ROS_WARN("Could not transform the global plan to the frame of the controller"); - return false; + geometry_msgs::PoseStamped robot_pose; + costmap_ros_->getRobotPose(robot_pose); + + nav_msgs::Odometry base_odom; + odom_helper_.getOdom(base_odom); + + /* assume that the global goal is the last point of the global plan and transform it to the planning frame */ + geometry_msgs::PoseStamped global_goal; + const geometry_msgs::PoseStamped& plan_pose = global_plan_.back(); + geometry_msgs::TransformStamped tf_plan_to_global = tf_->lookupTransform(global_frame_, ros::Time(), plan_pose.header.frame_id, plan_pose.header.stamp, + plan_pose.header.frame_id, ros::Duration(cfg_.robot.transform_tolerance)); + tf2::doTransform(global_plan_.back(), global_goal, tf_plan_to_global); + + /* Check if the goal is reached */ + double dx = global_goal.pose.position.x - robot_pose.pose.position.x; + double dy = global_goal.pose.position.y - robot_pose.pose.position.y; + double delta_orient = g2o::normalize_theta( tf2::getYaw(global_goal.pose.orientation) - robot_pose_.theta() ); + if(fabs(std::sqrt(dx*dx+dy*dy)) < cfg_.goal_tolerance.xy_goal_tolerance + && fabs(delta_orient) < cfg_.goal_tolerance.yaw_goal_tolerance + && (!cfg_.goal_tolerance.complete_global_plan || via_points_.size() == 0) + && (base_local_planner::stopped(base_odom, cfg_.goal_tolerance.theta_stopped_vel, cfg_.goal_tolerance.trans_stopped_vel) + || cfg_.goal_tolerance.free_goal_vel)) + { + return true; + } } - - tf2::doTransform(global_plan_.back(), global_goal, tf_plan_to_global); - double dx = global_goal.pose.position.x - robot_pose_.x(); - double dy = global_goal.pose.position.y - robot_pose_.y(); - double delta_orient = g2o::normalize_theta( tf2::getYaw(global_goal.pose.orientation) - robot_pose_.theta() ); - if(fabs(std::sqrt(dx*dx+dy*dy)) < cfg_.goal_tolerance.xy_goal_tolerance - && fabs(delta_orient) < cfg_.goal_tolerance.yaw_goal_tolerance - && (!cfg_.goal_tolerance.complete_global_plan || via_points_.size() == 0) - && (base_local_planner::stopped(base_odom, cfg_.goal_tolerance.theta_stopped_vel, cfg_.goal_tolerance.trans_stopped_vel) - || cfg_.goal_tolerance.free_goal_vel)) + catch(...) /* Failed to transform the goal to the planning frame */ { - ROS_INFO("GOT A SUCCES WHILE CHECKING"); - return true; + return false; } return false; From 251d6db2b432683824db920b87a9e8d8ea133320 Mon Sep 17 00:00:00 2001 From: LucasNolasco Date: Fri, 9 Apr 2021 12:42:11 -0300 Subject: [PATCH 3/4] Fixed early goal reaching --- src/teb_local_planner_ros.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/teb_local_planner_ros.cpp b/src/teb_local_planner_ros.cpp index 28aad5df..8d06affc 100644 --- a/src/teb_local_planner_ros.cpp +++ b/src/teb_local_planner_ros.cpp @@ -278,11 +278,6 @@ uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt if (!custom_via_points_active_) updateViaPointsContainer(transformed_plan, cfg_.trajectory.global_plan_viapoint_sep); - // check if global goal is reached - if(isGoalReached()) { - return mbf_msgs::ExePathResult::SUCCESS; - } - // check if we should enter any backup mode and apply settings configureBackupModes(transformed_plan, goal_idx); @@ -443,7 +438,7 @@ uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt bool TebLocalPlannerROS::isGoalReached() { - if(!initialized_ || global_plan_.empty()) /* check if the planner was initialized and it there is a global plan */ + if(!initialized_ || ! global_plan_.size() > 0) /* check if the planner was initialized and it there is a global plan */ { return false; } @@ -474,6 +469,7 @@ bool TebLocalPlannerROS::isGoalReached() && (base_local_planner::stopped(base_odom, cfg_.goal_tolerance.theta_stopped_vel, cfg_.goal_tolerance.trans_stopped_vel) || cfg_.goal_tolerance.free_goal_vel)) { + planner_->clearPlanner(); return true; } } From b133ea9c5b2b42b69d0ca08d35b99bd5cf8f058f Mon Sep 17 00:00:00 2001 From: Lucas Nolasco Date: Mon, 21 Jun 2021 10:13:18 -0300 Subject: [PATCH 4/4] Discarding outdated plans to avoid early goal reaching --- src/teb_local_planner_ros.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/teb_local_planner_ros.cpp b/src/teb_local_planner_ros.cpp index 8d06affc..f22e4ec8 100644 --- a/src/teb_local_planner_ros.cpp +++ b/src/teb_local_planner_ros.cpp @@ -207,8 +207,16 @@ bool TebLocalPlannerROS::setPlan(const std::vector& } // store the global plan - global_plan_.clear(); - global_plan_ = orig_global_plan; + if (global_plan_.empty()) { /* If there is no plan, check if it's a valid plan */ + global_plan_ = orig_global_plan; + if (isGoalReached()) { + global_plan_.clear(); + } + } + else { /* If there is a plan, update it */ + global_plan_.clear(); + global_plan_ = orig_global_plan; + } // we do not clear the local planner here, since setPlan is called frequently whenever the global planner updates the plan. // the local planner checks whether it is required to reinitialize the trajectory or not within each velocity computation step. @@ -457,7 +465,7 @@ bool TebLocalPlannerROS::isGoalReached() const geometry_msgs::PoseStamped& plan_pose = global_plan_.back(); geometry_msgs::TransformStamped tf_plan_to_global = tf_->lookupTransform(global_frame_, ros::Time(), plan_pose.header.frame_id, plan_pose.header.stamp, plan_pose.header.frame_id, ros::Duration(cfg_.robot.transform_tolerance)); - tf2::doTransform(global_plan_.back(), global_goal, tf_plan_to_global); + tf2::doTransform(plan_pose, global_goal, tf_plan_to_global); /* Check if the goal is reached */ double dx = global_goal.pose.position.x - robot_pose.pose.position.x; @@ -469,6 +477,7 @@ bool TebLocalPlannerROS::isGoalReached() && (base_local_planner::stopped(base_odom, cfg_.goal_tolerance.theta_stopped_vel, cfg_.goal_tolerance.trans_stopped_vel) || cfg_.goal_tolerance.free_goal_vel)) { + global_plan_.clear(); planner_->clearPlanner(); return true; }