diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 258db2d552..4c1a673c8a 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -21,7 +21,6 @@ #include #include -#include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_core/controller.hpp" #include "rclcpp/rclcpp.hpp" #include "pluginlib/class_loader.hpp" @@ -184,16 +183,14 @@ class RegulatedPurePursuitController : public nav2_core::Controller const double &, const double &); /** - * @brief checks for collision at projected pose + * @brief Whether point is in collision * @param x Pose of pose x * @param y Pose of pose y * @param theta orientation of Yaw * @return Whether in collision */ - bool inCollision( - const double & x, - const double & y, - const double & theta); + bool inCollision(const double & x, const double & y); + /** * @brief Cost at a point * @param x Pose of pose x @@ -268,8 +265,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller std::shared_ptr> carrot_pub_; std::shared_ptr> carrot_arc_pub_; - std::unique_ptr> - collision_checker_; }; } // namespace nav2_regulated_pure_pursuit_controller diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 70dd66b2aa..ada60b6b76 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -172,11 +172,6 @@ void RegulatedPurePursuitController::configure( global_path_pub_ = node->create_publisher("received_global_plan", 1); carrot_pub_ = node->create_publisher("lookahead_point", 1); carrot_arc_pub_ = node->create_publisher("lookahead_collision_arc", 1); - - // initialize collision checker and set costmap - collision_checker_ = std::make_unique>(costmap_); - collision_checker_->setCostmap(costmap_); } void RegulatedPurePursuitController::cleanup() @@ -382,10 +377,7 @@ bool RegulatedPurePursuitController::isCollisionImminent( // odom frame and the carrot_pose is in robot base frame. // check current point is OK - if (inCollision( - robot_pose.pose.position.x, robot_pose.pose.position.y, - tf2::getYaw(robot_pose.pose.orientation))) - { + if (inCollision(robot_pose.pose.position.x, robot_pose.pose.position.y)) { return true; } @@ -404,9 +396,13 @@ bool RegulatedPurePursuitController::isCollisionImminent( curr_pose.y = robot_pose.pose.position.y; curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); - // only forward simulate within time requested int i = 1; - while (i * projection_time < max_allowed_time_to_collision_) { + while (true) { + // only forward simulate within time requested + if (i * projection_time > max_allowed_time_to_collision_) { + break; + } + i++; // apply velocity at curr_pose over distance @@ -420,8 +416,8 @@ bool RegulatedPurePursuitController::isCollisionImminent( pose_msg.pose.position.z = 0.01; arc_pts_msg.poses.push_back(pose_msg); - // check for collision at the projected pose - if (inCollision(curr_pose.x, curr_pose.y, curr_pose.theta)) { + // check for collision at this point + if (inCollision(curr_pose.x, curr_pose.y)) { carrot_arc_pub_->publish(arc_pts_msg); return true; } @@ -432,10 +428,7 @@ bool RegulatedPurePursuitController::isCollisionImminent( return false; } -bool RegulatedPurePursuitController::inCollision( - const double & x, - const double & y, - const double & theta) +bool RegulatedPurePursuitController::inCollision(const double & x, const double & y) { unsigned int mx, my; @@ -448,16 +441,13 @@ bool RegulatedPurePursuitController::inCollision( return false; } - double footprint_cost = collision_checker_->footprintCostAtPose( - x, y, theta, costmap_ros_->getRobotFootprint()); - if (footprint_cost == static_cast(NO_INFORMATION) && - costmap_ros_->getLayeredCostmap()->isTrackingUnknown()) - { - return false; - } + unsigned char cost = costmap_->getCost(mx, my); - // if occupied or unknown and not to traverse unknown space - return footprint_cost >= static_cast(LETHAL_OBSTACLE); + if (costmap_ros_->getLayeredCostmap()->isTrackingUnknown()) { + return cost >= INSCRIBED_INFLATED_OBSTACLE && cost != NO_INFORMATION; + } else { + return cost >= INSCRIBED_INFLATED_OBSTACLE; + } } double RegulatedPurePursuitController::costAtPose(const double & x, const double & y)