From cfa23739faa9a0e5cb6f786a0132c37c257ccfc7 Mon Sep 17 00:00:00 2001 From: sathak93 Date: Thu, 2 Sep 2021 17:05:50 +0530 Subject: [PATCH 01/26] initial --- .../regulated_pure_pursuit_controller.hpp | 8 +++++- .../src/regulated_pure_pursuit_controller.cpp | 26 ++++++++++++++++++- 2 files changed, 32 insertions(+), 2 deletions(-) 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 d09ff2b4bfd..b3041d1473b 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 @@ -59,7 +59,8 @@ class RegulatedPurePursuitController : public nav2_core::Controller void configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name, const std::shared_ptr & tf, - const std::shared_ptr & costmap_ros) override; + const std::shared_ptr & costmap_ros, + std::shared_ptr collision_checker) override; /** * @brief Cleanup controller state machine @@ -233,6 +234,9 @@ class RegulatedPurePursuitController : public nav2_core::Controller nav2_costmap_2d::Costmap2D * costmap_; rclcpp::Logger logger_ {rclcpp::get_logger("RegulatedPurePursuitController")}; rclcpp::Clock::SharedPtr clock_; + std::shared_ptr collision_checker_; + std::unique_ptr costmap_sub_; + std::unique_ptr footprint_sub_; double desired_linear_vel_, base_desired_linear_vel_; double lookahead_dist_; @@ -260,6 +264,8 @@ class RegulatedPurePursuitController : public nav2_core::Controller double rotate_to_heading_min_angle_; double goal_dist_tol_; bool allow_reversing_; + std::string global_frame_; + std::string robot_base_frame_; nav_msgs::msg::Path global_plan_; std::shared_ptr> global_path_pub_; 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 634e697504f..0ea2dd0fbf2 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 @@ -39,7 +39,8 @@ namespace nav2_regulated_pure_pursuit_controller void RegulatedPurePursuitController::configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name, const std::shared_ptr & tf, - const std::shared_ptr & costmap_ros) + const std::shared_ptr & costmap_ros, + std::shared_ptr collision_checker) { auto node = parent.lock(); if (!node) { @@ -178,6 +179,22 @@ 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); + + std::string costmap_topic, footprint_topic; + node->get_parameter("costmap_topic", costmap_topic); + node->get_parameter("footprint_topic", footprint_topic); + node->get_parameter("transform_tolerance", transform_tolerance_); + costmap_sub_ = std::make_unique( + shared_from_this(), costmap_topic); + footprint_sub_ = std::make_unique( + shared_from_this(), footprint_topic, 1.0); + + std::string global_frame, robot_base_frame; + get_parameter("global_frame", global_frame); + get_parameter("robot_base_frame", robot_base_frame); + collision_checker_ = std::make_shared( + *costmap_sub_, *footprint_sub_, *tf_, this->get_name(), + global_frame, robot_base_frame, transform_tolerance_); } void RegulatedPurePursuitController::cleanup() @@ -190,6 +207,9 @@ void RegulatedPurePursuitController::cleanup() global_path_pub_.reset(); carrot_pub_.reset(); carrot_arc_pub_.reset(); + footprint_sub_.reset(); + costmap_sub_.reset(); + collision_checker_.reset(); } void RegulatedPurePursuitController::activate() @@ -386,6 +406,10 @@ bool RegulatedPurePursuitController::isCollisionImminent( if (inCollision(robot_pose.pose.position.x, robot_pose.pose.position.y)) { return true; } + if (collision_checker_->collision)) { + return true; + } + // visualization messages nav_msgs::msg::Path arc_pts_msg; From 898b91d58822fefc8d97111f96af7883d0b69b1d Mon Sep 17 00:00:00 2001 From: sathak93 Date: Fri, 3 Sep 2021 10:03:33 +0530 Subject: [PATCH 02/26] initial --- .../collision_checker.hpp | 216 ++++++++++++++++++ .../regulated_pure_pursuit_controller.hpp | 2 + .../src/regulated_pure_pursuit_controller.cpp | 21 ++ 3 files changed, 239 insertions(+) create mode 100644 nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp new file mode 100644 index 00000000000..94a9fbfed1e --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp @@ -0,0 +1,216 @@ +// Copyright (c) 2020, Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. +#include +#include "nav2_costmap_2d/footprint_collision_checker.hpp" + + +#ifndef NAV2_REGULATED_PURE_PURSUIT_CONTROLLER___COLLISION_CHECKER_HPP_ +#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER___COLLISION_CHECKER_HPP_ + +namespace nav2_regulated_pure_pursuit_controller +{ + + const float UNKNOWN = 255.0; + const float OCCUPIED = 254.0; + const float INSCRIBED = 253.0; + const float MAX_NON_OBSTACLE = 252.0; + const float FREE = 0; + +/** + * + * @class nav2_smac_planner::GridCollisionChecker + * @brief A costmap grid collision checker + */ +class GridCollisionChecker + : public nav2_costmap_2d::FootprintCollisionChecker +{ +public: + /** + * @brief A constructor for nav2_smac_planner::GridCollisionChecker + * @param costmap The costmap to collision check against + * @param num_quantizations The number of quantizations to precompute footprint + * orientations for to speed up collision checking + */ + GridCollisionChecker( + nav2_costmap_2d::Costmap2D * costmap, + unsigned int num_quantizations) + : FootprintCollisionChecker(costmap), + num_quantizations_(num_quantizations) + { + } + + /** + * @brief Set the footprint to use with collision checker + * @param footprint The footprint to collision check against + * @param radius Whether or not the footprint is a circle and use radius collision checking + */ + void setFootprint( + const nav2_costmap_2d::Footprint & footprint, + const bool & radius, + const double & possible_inscribed_cost) + { + possible_inscribed_cost_ = possible_inscribed_cost; + footprint_is_radius_ = radius; + + // Use radius, no caching required + if (radius) { + return; + } + + // No change, no updates required + if (footprint == unoriented_footprint_) { + return; + } + + bin_size_ = 2.0 * M_PI / static_cast(num_quantizations_); + oriented_footprints_.reserve(num_quantizations_); + double sin_th, cos_th; + geometry_msgs::msg::Point new_pt; + const unsigned int footprint_size = footprint.size(); + + // Precompute the orientation bins for checking to use + for (unsigned int i = 0; i != num_quantizations_; i++) { + sin_th = sin(i * bin_size_); + cos_th = cos(i * bin_size_); + nav2_costmap_2d::Footprint oriented_footprint; + oriented_footprint.reserve(footprint_size); + + for (unsigned int j = 0; j < footprint_size; j++) { + new_pt.x = footprint[j].x * cos_th - footprint[j].y * sin_th; + new_pt.y = footprint[j].x * sin_th + footprint[j].y * cos_th; + oriented_footprint.push_back(new_pt); + } + + oriented_footprints_.push_back(oriented_footprint); + } + + unoriented_footprint_ = footprint; + } + + /** + * @brief Check if in collision with costmap and footprint at pose + * @param x X coordinate of pose to check against + * @param y Y coordinate of pose to check against + * @param theta Angle of pose to check against + * @param traverse_unknown Whether or not to traverse in unknown space + * @return boolean if in collision or not. + */ + bool inCollision( + const double & x, + const double & y, + const double & theta, + const bool & traverse_unknown) + { + // Assumes setFootprint already set + double wx, wy; + costmap_->mapToWorld(static_cast(x), static_cast(y), wx, wy); + + if (!footprint_is_radius_) { + // if footprint, then we check for the footprint's points, but first see + // if the robot is even potentially in an inscribed collision + footprint_cost_ = costmap_->getCost( + static_cast(x), static_cast(y)); + + if (footprint_cost_ < possible_inscribed_cost_) { + return false; + } + + // If its inscribed, in collision, or unknown in the middle, + // no need to even check the footprint, its invalid + if (footprint_cost_ == UNKNOWN && !traverse_unknown) { + return true; + } + + if (footprint_cost_ == INSCRIBED || footprint_cost_ == OCCUPIED) { + return true; + } + + // if possible inscribed, need to check actual footprint pose. + // Use precomputed oriented footprints are done on initialization, + // offset by translation value to collision check + int angle_bin = theta / bin_size_; + geometry_msgs::msg::Point new_pt; + const nav2_costmap_2d::Footprint & oriented_footprint = oriented_footprints_[angle_bin]; + nav2_costmap_2d::Footprint current_footprint; + current_footprint.reserve(oriented_footprint.size()); + for (unsigned int i = 0; i < oriented_footprint.size(); ++i) { + new_pt.x = wx + oriented_footprint[i].x; + new_pt.y = wy + oriented_footprint[i].y; + current_footprint.push_back(new_pt); + } + + footprint_cost_ = footprintCost(current_footprint); + + if (footprint_cost_ == UNKNOWN && traverse_unknown) { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return footprint_cost_ >= OCCUPIED; + } else { + // if radius, then we can check the center of the cost assuming inflation is used + footprint_cost_ = costmap_->getCost( + static_cast(x), static_cast(y)); + + if (footprint_cost_ == UNKNOWN && traverse_unknown) { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return footprint_cost_ >= INSCRIBED; + } + } + + /** + * @brief Check if in collision with costmap and footprint at pose + * @param i Index to search collision status of + * @param traverse_unknown Whether or not to traverse in unknown space + * @return boolean if in collision or not. + */ + bool inCollision( + const unsigned int & i, + const bool & traverse_unknown) + { + footprint_cost_ = costmap_->getCost(i); + if (footprint_cost_ == UNKNOWN && traverse_unknown) { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return footprint_cost_ >= INSCRIBED; + } + + /** + * @brief Get cost at footprint pose in costmap + * @return the cost at the pose in costmap + */ + float getCost() + { + // Assumes inCollision called prior + return static_cast(footprint_cost_); + } + +protected: + std::vector oriented_footprints_; + nav2_costmap_2d::Footprint unoriented_footprint_; + double footprint_cost_; + bool footprint_is_radius_; + unsigned int num_quantizations_; + double bin_size_; + double possible_inscribed_cost_{-1}; +}; + +} // namespace nav2_smac_planner + +#endif // NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_ 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 d09ff2b4bfd..97e159bad6f 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 @@ -28,6 +28,7 @@ #include "nav2_util/odometry_utils.hpp" #include "nav2_util/geometry_utils.hpp" #include "geometry_msgs/msg/pose2_d.hpp" +#include "nav2_regulated_pure_pursuit_controller/collision_checker.hpp" namespace nav2_regulated_pure_pursuit_controller { @@ -266,6 +267,7 @@ 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 634e697504f..66d19169860 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 @@ -178,6 +178,13 @@ 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); + + _collision_checker = std::make_unique(costmap_, 1 /*for 2D, most be 1*/); + _collision_checker->setFootprint( + costmap_ros->getRobotFootprint(), + true /*for 2D, most use radius*/, + 0.0 /*for 2D cost at inscribed isn't relevent*/); + } void RegulatedPurePursuitController::cleanup() @@ -383,10 +390,17 @@ bool RegulatedPurePursuitController::isCollisionImminent( // odom frame and the carrot_pose is in robot base frame. // check current point is OK + if (_collision_checker->inCollision(robot_pose.pose.position.x, robot_pose.pose.position.y, + tf2::getYaw(robot_pose.pose.orientation),true)) { + //return true; + RCLCPP_INFO(logger_, "inside collision checker = true "); + } if (inCollision(robot_pose.pose.position.x, robot_pose.pose.position.y)) { + RCLCPP_INFO(logger_, "inside incollision = true"); return true; } + // visualization messages nav_msgs::msg::Path arc_pts_msg; arc_pts_msg.header.frame_id = costmap_ros_->getGlobalFrameID(); @@ -422,11 +436,18 @@ bool RegulatedPurePursuitController::isCollisionImminent( pose_msg.pose.position.z = 0.01; arc_pts_msg.poses.push_back(pose_msg); + if (_collision_checker->inCollision(curr_pose.x, curr_pose.y,curr_pose.theta,true) ){ + carrot_arc_pub_->publish(arc_pts_msg); + RCLCPP_INFO(logger_, "inside collision checker = true "); + //return true; + } // check for collision at this point if (inCollision(curr_pose.x, curr_pose.y)) { carrot_arc_pub_->publish(arc_pts_msg); + RCLCPP_INFO(logger_, "inside incollision = true "); return true; } + } carrot_arc_pub_->publish(arc_pts_msg); From d3d21a7ca4f126c8afa0d4a9958f32d9566090d8 Mon Sep 17 00:00:00 2001 From: sathak93 Date: Fri, 3 Sep 2021 21:28:51 +0530 Subject: [PATCH 03/26] initialize --- .../regulated_pure_pursuit_controller.hpp | 6 ++-- .../src/regulated_pure_pursuit_controller.cpp | 35 ++++++++++--------- 2 files changed, 21 insertions(+), 20 deletions(-) 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 97e159bad6f..b160d345609 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 @@ -43,12 +43,12 @@ class RegulatedPurePursuitController : public nav2_core::Controller /** * @brief Constructor for nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController */ - RegulatedPurePursuitController() = default; + RegulatedPurePursuitController(); /** * @brief Destrructor for nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController */ - ~RegulatedPurePursuitController() override = default; + ~RegulatedPurePursuitController(); /** * @brief Configure controller state machine @@ -267,7 +267,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller std::shared_ptr> carrot_pub_; std::shared_ptr> carrot_arc_pub_; - std::unique_ptr _collision_checker; + GridCollisionChecker _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 ecf01b54503..d12651edd65 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 @@ -36,6 +36,18 @@ using namespace nav2_costmap_2d; // NOLINT namespace nav2_regulated_pure_pursuit_controller { +RegulatedPurePursuitController::RegulatedPurePursuitController() + : + costmap_ros_(nullptr), + global_path_pub_(nullptr), + carrot_pub_(nullptr), + carrot_arc_pub_(nullptr), + _collision_checker(nullptr, 1) +{ +} + +RegulatedPurePursuitController::~RegulatedPurePursuitController(){} + void RegulatedPurePursuitController::configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name, const std::shared_ptr & tf, @@ -179,10 +191,10 @@ void RegulatedPurePursuitController::configure( carrot_pub_ = node->create_publisher("lookahead_point", 1); carrot_arc_pub_ = node->create_publisher("lookahead_collision_arc", 1); - _collision_checker = std::make_unique(costmap_, 1 /*for 2D, most be 1*/); - _collision_checker->setFootprint( + _collision_checker = GridCollisionChecker(costmap_, 1 /*for 2D, most be 1*/); + _collision_checker.setFootprint( costmap_ros->getRobotFootprint(), - true /*for 2D, most use radius*/, + false /*for 2D, most use radius*/, 0.0 /*for 2D cost at inscribed isn't relevent*/); } @@ -390,13 +402,8 @@ bool RegulatedPurePursuitController::isCollisionImminent( // odom frame and the carrot_pose is in robot base frame. // check current point is OK - if (_collision_checker->inCollision(robot_pose.pose.position.x, robot_pose.pose.position.y, + if (_collision_checker.inCollision(robot_pose.pose.position.x, robot_pose.pose.position.y, tf2::getYaw(robot_pose.pose.orientation),true)) { - //return true; - RCLCPP_INFO(logger_, "inside collision checker = true "); - } - if (inCollision(robot_pose.pose.position.x, robot_pose.pose.position.y)) { - RCLCPP_INFO(logger_, "inside incollision = true"); return true; } // visualization messages @@ -434,18 +441,12 @@ bool RegulatedPurePursuitController::isCollisionImminent( pose_msg.pose.position.z = 0.01; arc_pts_msg.poses.push_back(pose_msg); - if (_collision_checker->inCollision(curr_pose.x, curr_pose.y,curr_pose.theta,true) ){ + if (_collision_checker.inCollision(curr_pose.x, curr_pose.y,curr_pose.theta,true) ){ carrot_arc_pub_->publish(arc_pts_msg); RCLCPP_INFO(logger_, "inside collision checker = true "); - //return true; - } - // check for collision at this point - if (inCollision(curr_pose.x, curr_pose.y)) { - carrot_arc_pub_->publish(arc_pts_msg); - RCLCPP_INFO(logger_, "inside incollision = true "); return true; } - + // check for collision at this point } carrot_arc_pub_->publish(arc_pts_msg); From 044955be0618b06e16eb8b4568844027ec2ccefb Mon Sep 17 00:00:00 2001 From: sathak93 Date: Sat, 4 Sep 2021 14:49:42 +0530 Subject: [PATCH 04/26] un --- nav2_bringup/launch/rviz_launch.py | 3 +- .../collision_checker.hpp | 36 +++++++++++------- .../src/regulated_pure_pursuit_controller.cpp | 38 ++++++++++++------- 3 files changed, 48 insertions(+), 29 deletions(-) diff --git a/nav2_bringup/launch/rviz_launch.py b/nav2_bringup/launch/rviz_launch.py index 9f123b81eb6..9c7a2eda509 100644 --- a/nav2_bringup/launch/rviz_launch.py +++ b/nav2_bringup/launch/rviz_launch.py @@ -58,7 +58,8 @@ def generate_launch_description(): package='rviz2', executable='rviz2', arguments=['-d', rviz_config_file], - output='screen') + #output='screen' + ) namespaced_rviz_config_file = ReplaceString( source_file=rviz_config_file, diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp index 94a9fbfed1e..7a7b610e5b9 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. #include +#include #include "nav2_costmap_2d/footprint_collision_checker.hpp" @@ -21,11 +22,11 @@ namespace nav2_regulated_pure_pursuit_controller { - const float UNKNOWN = 255.0; - const float OCCUPIED = 254.0; - const float INSCRIBED = 253.0; - const float MAX_NON_OBSTACLE = 252.0; - const float FREE = 0; + static constexpr unsigned char UNKNOWN = 255; + static constexpr unsigned char OCCUPIED = 254; + static constexpr unsigned char INSCRIBED = 253; + static constexpr unsigned char MAX_NON_OBSTACLE = 252; + static constexpr unsigned char FREE = 0; /** * @@ -50,6 +51,9 @@ class GridCollisionChecker { } + + + /** * @brief Set the footprint to use with collision checker * @param footprint The footprint to collision check against @@ -116,6 +120,7 @@ class GridCollisionChecker double wx, wy; costmap_->mapToWorld(static_cast(x), static_cast(y), wx, wy); + RCLCPP_INFO(logger_,"footprint_is_radius_ %i", footprint_is_radius_); if (!footprint_is_radius_) { // if footprint, then we check for the footprint's points, but first see // if the robot is even potentially in an inscribed collision @@ -125,13 +130,13 @@ class GridCollisionChecker if (footprint_cost_ < possible_inscribed_cost_) { return false; } - + RCLCPP_INFO(logger_,"saka 30"); // If its inscribed, in collision, or unknown in the middle, // no need to even check the footprint, its invalid if (footprint_cost_ == UNKNOWN && !traverse_unknown) { return true; } - + RCLCPP_INFO(logger_,"saka 32"); if (footprint_cost_ == INSCRIBED || footprint_cost_ == OCCUPIED) { return true; } @@ -151,7 +156,7 @@ class GridCollisionChecker } footprint_cost_ = footprintCost(current_footprint); - + RCLCPP_INFO(logger_,"saka 33"); if (footprint_cost_ == UNKNOWN && traverse_unknown) { return false; } @@ -160,16 +165,18 @@ class GridCollisionChecker return footprint_cost_ >= OCCUPIED; } else { // if radius, then we can check the center of the cost assuming inflation is used + RCLCPP_INFO(logger_,"saka 35"); footprint_cost_ = costmap_->getCost( - static_cast(x), static_cast(y)); - + static_cast(wx), static_cast(wy)); + RCLCPP_INFO(logger_,"footprint_cost_ %c", footprint_cost_); if (footprint_cost_ == UNKNOWN && traverse_unknown) { return false; } - + RCLCPP_INFO(logger_,"saka 35"); // if occupied or unknown and not to traverse unknown space return footprint_cost_ >= INSCRIBED; } + RCLCPP_INFO(logger_,"saka 3000"); } /** @@ -195,20 +202,21 @@ class GridCollisionChecker * @brief Get cost at footprint pose in costmap * @return the cost at the pose in costmap */ - float getCost() + double getCost() { // Assumes inCollision called prior - return static_cast(footprint_cost_); + return static_cast(footprint_cost_); } protected: std::vector oriented_footprints_; nav2_costmap_2d::Footprint unoriented_footprint_; - double footprint_cost_; + unsigned char footprint_cost_; bool footprint_is_radius_; unsigned int num_quantizations_; double bin_size_; double possible_inscribed_cost_{-1}; + rclcpp::Logger logger_ {rclcpp::get_logger("RegulatedPurePursuitController")}; }; } // namespace nav2_smac_planner 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 d12651edd65..a8bd86b7472 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 @@ -38,11 +38,13 @@ namespace nav2_regulated_pure_pursuit_controller RegulatedPurePursuitController::RegulatedPurePursuitController() : + tf_(nullptr), costmap_ros_(nullptr), + costmap_(nullptr), global_path_pub_(nullptr), carrot_pub_(nullptr), carrot_arc_pub_(nullptr), - _collision_checker(nullptr, 1) + _collision_checker(costmap_, 1) { } @@ -57,7 +59,7 @@ void RegulatedPurePursuitController::configure( if (!node) { throw nav2_core::PlannerException("Unable to lock node!"); } - + RCLCPP_INFO(logger_,"saka 1"); costmap_ros_ = costmap_ros; costmap_ = costmap_ros_->getCostmap(); tf_ = tf; @@ -190,13 +192,7 @@ 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); - - _collision_checker = GridCollisionChecker(costmap_, 1 /*for 2D, most be 1*/); - _collision_checker.setFootprint( - costmap_ros->getRobotFootprint(), - false /*for 2D, most use radius*/, - 0.0 /*for 2D cost at inscribed isn't relevent*/); - + RCLCPP_INFO(logger_,"saka 2"); } void RegulatedPurePursuitController::cleanup() @@ -272,7 +268,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } else { goal_dist_tol_ = pose_tolerance.position.x; } - + RCLCPP_INFO(logger_,"saka 3"); // Transform path to robot base frame auto transformed_plan = transformGlobalPlan(pose); @@ -400,12 +396,25 @@ bool RegulatedPurePursuitController::isCollisionImminent( { // Note(stevemacenski): This may be a bit unusual, but the robot_pose is in // odom frame and the carrot_pose is in robot base frame. + RCLCPP_INFO(logger_,"saka 4"); + //_collision_checker = GridCollisionChecker(costmap_, 1 /*for 2D, most be 1*/); + _collision_checker.setCostmap(costmap_); + _collision_checker.setFootprint( + costmap_ros_->getRobotFootprint(), + true /*for 2D, most use radius*/, + 0.0 /*for 2D cost at inscribed isn't relevent*/); + + + RCLCPP_INFO(logger_,"saka 5"); + RCLCPP_INFO(logger_,"saka 5 %lf , %lf ,%lf ", robot_pose.pose.position.x, robot_pose.pose.position.y, tf2::getYaw(robot_pose.pose.orientation)); // check current point is OK if (_collision_checker.inCollision(robot_pose.pose.position.x, robot_pose.pose.position.y, - tf2::getYaw(robot_pose.pose.orientation),true)) { + tf2::getYaw(robot_pose.pose.orientation),false)) { + RCLCPP_INFO(logger_,"saka 6"); return true; } + RCLCPP_INFO(logger_,"saka 6-1"); // visualization messages nav_msgs::msg::Path arc_pts_msg; arc_pts_msg.header.frame_id = costmap_ros_->getGlobalFrameID(); @@ -429,7 +438,7 @@ bool RegulatedPurePursuitController::isCollisionImminent( } i++; - + RCLCPP_INFO(logger_,"saka 6"); // apply velocity at curr_pose over distance curr_pose.x += projection_time * (linear_vel * cos(curr_pose.theta)); curr_pose.y += projection_time * (linear_vel * sin(curr_pose.theta)); @@ -441,7 +450,7 @@ bool RegulatedPurePursuitController::isCollisionImminent( pose_msg.pose.position.z = 0.01; arc_pts_msg.poses.push_back(pose_msg); - if (_collision_checker.inCollision(curr_pose.x, curr_pose.y,curr_pose.theta,true) ){ + if (_collision_checker.inCollision(curr_pose.x, curr_pose.y,curr_pose.theta,false) ){ carrot_arc_pub_->publish(arc_pts_msg); RCLCPP_INFO(logger_, "inside collision checker = true "); return true; @@ -643,8 +652,9 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( double RegulatedPurePursuitController::findDirectionChange( const geometry_msgs::msg::PoseStamped & pose) { + RCLCPP_INFO(logger_,"saka 3"); // Iterating through the global path to determine the position of the cusp - for (unsigned int pose_id = 1; pose_id < global_plan_.poses.size(); ++pose_id) { + for (unsigned int pose_id = 1; pose_id < global_plan_.poses.size() - 1; ++pose_id) { // We have two vectors for the dot product OA and AB. Determining the vectors. double oa_x = global_plan_.poses[pose_id].pose.position.x - global_plan_.poses[pose_id - 1].pose.position.x; From 456e8529d72ab5d5cf8d725d860bf555b0666030 Mon Sep 17 00:00:00 2001 From: sathak93 Date: Sat, 4 Sep 2021 16:50:01 +0530 Subject: [PATCH 05/26] working --- .../collision_checker.hpp | 224 ------------------ .../regulated_pure_pursuit_controller.hpp | 34 ++- .../src/regulated_pure_pursuit_controller.cpp | 126 ++++++++-- 3 files changed, 140 insertions(+), 244 deletions(-) delete mode 100644 nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp deleted file mode 100644 index 7a7b610e5b9..00000000000 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp +++ /dev/null @@ -1,224 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. Reserved. -#include -#include -#include "nav2_costmap_2d/footprint_collision_checker.hpp" - - -#ifndef NAV2_REGULATED_PURE_PURSUIT_CONTROLLER___COLLISION_CHECKER_HPP_ -#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER___COLLISION_CHECKER_HPP_ - -namespace nav2_regulated_pure_pursuit_controller -{ - - static constexpr unsigned char UNKNOWN = 255; - static constexpr unsigned char OCCUPIED = 254; - static constexpr unsigned char INSCRIBED = 253; - static constexpr unsigned char MAX_NON_OBSTACLE = 252; - static constexpr unsigned char FREE = 0; - -/** - * - * @class nav2_smac_planner::GridCollisionChecker - * @brief A costmap grid collision checker - */ -class GridCollisionChecker - : public nav2_costmap_2d::FootprintCollisionChecker -{ -public: - /** - * @brief A constructor for nav2_smac_planner::GridCollisionChecker - * @param costmap The costmap to collision check against - * @param num_quantizations The number of quantizations to precompute footprint - * orientations for to speed up collision checking - */ - GridCollisionChecker( - nav2_costmap_2d::Costmap2D * costmap, - unsigned int num_quantizations) - : FootprintCollisionChecker(costmap), - num_quantizations_(num_quantizations) - { - } - - - - - /** - * @brief Set the footprint to use with collision checker - * @param footprint The footprint to collision check against - * @param radius Whether or not the footprint is a circle and use radius collision checking - */ - void setFootprint( - const nav2_costmap_2d::Footprint & footprint, - const bool & radius, - const double & possible_inscribed_cost) - { - possible_inscribed_cost_ = possible_inscribed_cost; - footprint_is_radius_ = radius; - - // Use radius, no caching required - if (radius) { - return; - } - - // No change, no updates required - if (footprint == unoriented_footprint_) { - return; - } - - bin_size_ = 2.0 * M_PI / static_cast(num_quantizations_); - oriented_footprints_.reserve(num_quantizations_); - double sin_th, cos_th; - geometry_msgs::msg::Point new_pt; - const unsigned int footprint_size = footprint.size(); - - // Precompute the orientation bins for checking to use - for (unsigned int i = 0; i != num_quantizations_; i++) { - sin_th = sin(i * bin_size_); - cos_th = cos(i * bin_size_); - nav2_costmap_2d::Footprint oriented_footprint; - oriented_footprint.reserve(footprint_size); - - for (unsigned int j = 0; j < footprint_size; j++) { - new_pt.x = footprint[j].x * cos_th - footprint[j].y * sin_th; - new_pt.y = footprint[j].x * sin_th + footprint[j].y * cos_th; - oriented_footprint.push_back(new_pt); - } - - oriented_footprints_.push_back(oriented_footprint); - } - - unoriented_footprint_ = footprint; - } - - /** - * @brief Check if in collision with costmap and footprint at pose - * @param x X coordinate of pose to check against - * @param y Y coordinate of pose to check against - * @param theta Angle of pose to check against - * @param traverse_unknown Whether or not to traverse in unknown space - * @return boolean if in collision or not. - */ - bool inCollision( - const double & x, - const double & y, - const double & theta, - const bool & traverse_unknown) - { - // Assumes setFootprint already set - double wx, wy; - costmap_->mapToWorld(static_cast(x), static_cast(y), wx, wy); - - RCLCPP_INFO(logger_,"footprint_is_radius_ %i", footprint_is_radius_); - if (!footprint_is_radius_) { - // if footprint, then we check for the footprint's points, but first see - // if the robot is even potentially in an inscribed collision - footprint_cost_ = costmap_->getCost( - static_cast(x), static_cast(y)); - - if (footprint_cost_ < possible_inscribed_cost_) { - return false; - } - RCLCPP_INFO(logger_,"saka 30"); - // If its inscribed, in collision, or unknown in the middle, - // no need to even check the footprint, its invalid - if (footprint_cost_ == UNKNOWN && !traverse_unknown) { - return true; - } - RCLCPP_INFO(logger_,"saka 32"); - if (footprint_cost_ == INSCRIBED || footprint_cost_ == OCCUPIED) { - return true; - } - - // if possible inscribed, need to check actual footprint pose. - // Use precomputed oriented footprints are done on initialization, - // offset by translation value to collision check - int angle_bin = theta / bin_size_; - geometry_msgs::msg::Point new_pt; - const nav2_costmap_2d::Footprint & oriented_footprint = oriented_footprints_[angle_bin]; - nav2_costmap_2d::Footprint current_footprint; - current_footprint.reserve(oriented_footprint.size()); - for (unsigned int i = 0; i < oriented_footprint.size(); ++i) { - new_pt.x = wx + oriented_footprint[i].x; - new_pt.y = wy + oriented_footprint[i].y; - current_footprint.push_back(new_pt); - } - - footprint_cost_ = footprintCost(current_footprint); - RCLCPP_INFO(logger_,"saka 33"); - if (footprint_cost_ == UNKNOWN && traverse_unknown) { - return false; - } - - // if occupied or unknown and not to traverse unknown space - return footprint_cost_ >= OCCUPIED; - } else { - // if radius, then we can check the center of the cost assuming inflation is used - RCLCPP_INFO(logger_,"saka 35"); - footprint_cost_ = costmap_->getCost( - static_cast(wx), static_cast(wy)); - RCLCPP_INFO(logger_,"footprint_cost_ %c", footprint_cost_); - if (footprint_cost_ == UNKNOWN && traverse_unknown) { - return false; - } - RCLCPP_INFO(logger_,"saka 35"); - // if occupied or unknown and not to traverse unknown space - return footprint_cost_ >= INSCRIBED; - } - RCLCPP_INFO(logger_,"saka 3000"); - } - - /** - * @brief Check if in collision with costmap and footprint at pose - * @param i Index to search collision status of - * @param traverse_unknown Whether or not to traverse in unknown space - * @return boolean if in collision or not. - */ - bool inCollision( - const unsigned int & i, - const bool & traverse_unknown) - { - footprint_cost_ = costmap_->getCost(i); - if (footprint_cost_ == UNKNOWN && traverse_unknown) { - return false; - } - - // if occupied or unknown and not to traverse unknown space - return footprint_cost_ >= INSCRIBED; - } - - /** - * @brief Get cost at footprint pose in costmap - * @return the cost at the pose in costmap - */ - double getCost() - { - // Assumes inCollision called prior - return static_cast(footprint_cost_); - } - -protected: - std::vector oriented_footprints_; - nav2_costmap_2d::Footprint unoriented_footprint_; - unsigned char footprint_cost_; - bool footprint_is_radius_; - unsigned int num_quantizations_; - double bin_size_; - double possible_inscribed_cost_{-1}; - rclcpp::Logger logger_ {rclcpp::get_logger("RegulatedPurePursuitController")}; -}; - -} // namespace nav2_smac_planner - -#endif // NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_ 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 b160d345609..9c63ce75156 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,6 +21,7 @@ #include #include +#include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_core/controller.hpp" #include "rclcpp/rclcpp.hpp" #include "pluginlib/class_loader.hpp" @@ -28,10 +29,15 @@ #include "nav2_util/odometry_utils.hpp" #include "nav2_util/geometry_utils.hpp" #include "geometry_msgs/msg/pose2_d.hpp" -#include "nav2_regulated_pure_pursuit_controller/collision_checker.hpp" + namespace nav2_regulated_pure_pursuit_controller { + static constexpr unsigned char UNKNOWN = 255; + static constexpr unsigned char OCCUPIED = 254; + static constexpr unsigned char INSCRIBED = 253; + static constexpr unsigned char MAX_NON_OBSTACLE = 252; + static constexpr unsigned char FREE = 0; /** * @class nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController @@ -189,7 +195,11 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @param y Pose of pose y * @return Whether in collision */ - bool inCollision(const double & x, const double & y); + bool inCollision( + const double & x, + const double & y, + const double & theta, + const bool & traverse_unknown); /** * @brief Cost at a point @@ -228,6 +238,16 @@ class RegulatedPurePursuitController : public nav2_core::Controller */ double findDirectionChange(const geometry_msgs::msg::PoseStamped & pose); + /** + * @brief Set the footprint to use with collision checker + * @param footprint The footprint to collision check against + * @param radius Whether or not the footprint is a circle and use radius collision checking + */ + void setFootprint( + const nav2_costmap_2d::Footprint & footprint, + const bool & radius, + const double & possible_inscribed_cost); + std::shared_ptr tf_; std::string plugin_name_; std::shared_ptr costmap_ros_; @@ -262,12 +282,20 @@ class RegulatedPurePursuitController : public nav2_core::Controller double goal_dist_tol_; bool allow_reversing_; + std::vector oriented_footprints_; + nav2_costmap_2d::Footprint unoriented_footprint_; + unsigned char footprint_cost_; + bool footprint_is_radius_; + unsigned int num_quantizations_; + double bin_size_; + double possible_inscribed_cost_{-1}; + nav_msgs::msg::Path global_plan_; std::shared_ptr> global_path_pub_; std::shared_ptr> carrot_pub_; std::shared_ptr> carrot_arc_pub_; - GridCollisionChecker _collision_checker; + nav2_costmap_2d::FootprintCollisionChecker _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 a8bd86b7472..edcd4ed17ef 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 @@ -19,6 +19,7 @@ #include #include + #include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp" #include "nav2_core/exceptions.hpp" #include "nav2_util/node_utils.hpp" @@ -44,7 +45,7 @@ RegulatedPurePursuitController::RegulatedPurePursuitController() global_path_pub_(nullptr), carrot_pub_(nullptr), carrot_arc_pub_(nullptr), - _collision_checker(costmap_, 1) + _collision_checker(costmap_) { } @@ -399,17 +400,15 @@ bool RegulatedPurePursuitController::isCollisionImminent( RCLCPP_INFO(logger_,"saka 4"); //_collision_checker = GridCollisionChecker(costmap_, 1 /*for 2D, most be 1*/); _collision_checker.setCostmap(costmap_); - _collision_checker.setFootprint( - costmap_ros_->getRobotFootprint(), - true /*for 2D, most use radius*/, - 0.0 /*for 2D cost at inscribed isn't relevent*/); + setFootprint(costmap_ros_->getRobotFootprint(),true /*for 2D, most use radius*/, 0.0 /*for 2 + D cost at inscribed isn't relevent*/); RCLCPP_INFO(logger_,"saka 5"); RCLCPP_INFO(logger_,"saka 5 %lf , %lf ,%lf ", robot_pose.pose.position.x, robot_pose.pose.position.y, tf2::getYaw(robot_pose.pose.orientation)); // check current point is OK - if (_collision_checker.inCollision(robot_pose.pose.position.x, robot_pose.pose.position.y, + if (inCollision(robot_pose.pose.position.x, robot_pose.pose.position.y, tf2::getYaw(robot_pose.pose.orientation),false)) { RCLCPP_INFO(logger_,"saka 6"); return true; @@ -450,7 +449,7 @@ bool RegulatedPurePursuitController::isCollisionImminent( pose_msg.pose.position.z = 0.01; arc_pts_msg.poses.push_back(pose_msg); - if (_collision_checker.inCollision(curr_pose.x, curr_pose.y,curr_pose.theta,false) ){ + if (inCollision(curr_pose.x, curr_pose.y,curr_pose.theta,false) ){ carrot_arc_pub_->publish(arc_pts_msg); RCLCPP_INFO(logger_, "inside collision checker = true "); return true; @@ -463,11 +462,13 @@ bool RegulatedPurePursuitController::isCollisionImminent( return false; } -bool RegulatedPurePursuitController::inCollision(const double & x, const double & y) +bool RegulatedPurePursuitController::inCollision(const double & x, const double & y , const double & theta, + const bool & traverse_unknown) { - unsigned int mx, my; + // Assumes setFootprint already set + unsigned int wx, wy; - if (!costmap_->worldToMap(x, y, mx, my)) { + if (!costmap_->worldToMap(x, y, wx, wy)) { RCLCPP_WARN_THROTTLE( logger_, *(clock_), 30000, "The dimensions of the costmap is too small to successfully check for " @@ -476,14 +477,62 @@ bool RegulatedPurePursuitController::inCollision(const double & x, const double return false; } - unsigned char cost = costmap_->getCost(mx, my); - - if (costmap_ros_->getLayeredCostmap()->isTrackingUnknown()) { - return cost >= INSCRIBED_INFLATED_OBSTACLE && cost != NO_INFORMATION; - } else { - return cost >= INSCRIBED_INFLATED_OBSTACLE; + RCLCPP_INFO(logger_,"footprint_is_radius_ %i", footprint_is_radius_); + if (!footprint_is_radius_) { + // if footprint, then we check for the footprint's points, but first see + // if the robot is even potentially in an inscribed collision + footprint_cost_ = costmap_->getCost(wx,wy); + + if (footprint_cost_ < possible_inscribed_cost_) { + return false; + } + RCLCPP_INFO(logger_,"saka 30"); + // If its inscribed, in collision, or unknown in the middle, + // no need to even check the footprint, its invalid + if (footprint_cost_ == UNKNOWN && !traverse_unknown) { + return true; + } + RCLCPP_INFO(logger_,"saka 32"); + if (footprint_cost_ == INSCRIBED || footprint_cost_ == OCCUPIED) { + return true; + } + + // if possible inscribed, need to check actual footprint pose. + // Use precomputed oriented footprints are done on initialization, + // offset by translation value to collision check + int angle_bin = theta / bin_size_; + geometry_msgs::msg::Point new_pt; + const nav2_costmap_2d::Footprint & oriented_footprint = oriented_footprints_[angle_bin]; + nav2_costmap_2d::Footprint current_footprint; + current_footprint.reserve(oriented_footprint.size()); + for (unsigned int i = 0; i < oriented_footprint.size(); ++i) { + new_pt.x = wx + oriented_footprint[i].x; + new_pt.y = wy + oriented_footprint[i].y; + current_footprint.push_back(new_pt); + } + + footprint_cost_ = _collision_checker.footprintCost(current_footprint); + RCLCPP_INFO(logger_,"saka 33"); + if (footprint_cost_ == UNKNOWN && traverse_unknown) { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return footprint_cost_ >= OCCUPIED; + } else { + // if radius, then we can check the center of the cost assuming inflation is used + RCLCPP_INFO(logger_,"saka 35"); + footprint_cost_ = costmap_->getCost(wx,wy); + RCLCPP_INFO(logger_,"footprint_cost_ %c", footprint_cost_); + if (footprint_cost_ == UNKNOWN && traverse_unknown) { + return false; + } + RCLCPP_INFO(logger_,"saka 35"); + // if occupied or unknown and not to traverse unknown space + return footprint_cost_ >= INSCRIBED; + } + RCLCPP_INFO(logger_,"saka 3000"); } -} double RegulatedPurePursuitController::costAtPose(const double & x, const double & y) { @@ -698,6 +747,49 @@ bool RegulatedPurePursuitController::transformPose( return false; } +void RegulatedPurePursuitController::setFootprint( + const nav2_costmap_2d::Footprint & footprint, + const bool & radius, + const double & possible_inscribed_cost) + { + possible_inscribed_cost_ = possible_inscribed_cost; + footprint_is_radius_ = radius; + + // Use radius, no caching required + if (radius) { + return; + } + + // No change, no updates required + if (footprint == unoriented_footprint_) { + return; + } + + bin_size_ = 2.0 * M_PI / static_cast(num_quantizations_); + oriented_footprints_.reserve(num_quantizations_); + double sin_th, cos_th; + geometry_msgs::msg::Point new_pt; + const unsigned int footprint_size = footprint.size(); + + // Precompute the orientation bins for checking to use + for (unsigned int i = 0; i != num_quantizations_; i++) { + sin_th = sin(i * bin_size_); + cos_th = cos(i * bin_size_); + nav2_costmap_2d::Footprint oriented_footprint; + oriented_footprint.reserve(footprint_size); + + for (unsigned int j = 0; j < footprint_size; j++) { + new_pt.x = footprint[j].x * cos_th - footprint[j].y * sin_th; + new_pt.y = footprint[j].x * sin_th + footprint[j].y * cos_th; + oriented_footprint.push_back(new_pt); + } + + oriented_footprints_.push_back(oriented_footprint); + } + + unoriented_footprint_ = footprint; + } + } // namespace nav2_regulated_pure_pursuit_controller // Register this controller as a nav2_core plugin From 536e2e7f9607057af4725aa1b82e8eeda3749b71 Mon Sep 17 00:00:00 2001 From: sathak93 Date: Sat, 4 Sep 2021 21:54:23 +0530 Subject: [PATCH 06/26] lint fix --- .../regulated_pure_pursuit_controller.hpp | 16 +- .../src/regulated_pure_pursuit_controller.cpp | 222 ++++++++---------- 2 files changed, 111 insertions(+), 127 deletions(-) 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 9c63ce75156..99f80b1432b 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 @@ -33,11 +33,11 @@ namespace nav2_regulated_pure_pursuit_controller { - static constexpr unsigned char UNKNOWN = 255; - static constexpr unsigned char OCCUPIED = 254; - static constexpr unsigned char INSCRIBED = 253; - static constexpr unsigned char MAX_NON_OBSTACLE = 252; - static constexpr unsigned char FREE = 0; +const double UNKNOWN = 255; +const double OCCUPIED = 254; +const double INSCRIBED = 253; +const double MAX_NON_OBSTACLE = 252; +const double FREE = 0; /** * @class nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController @@ -195,7 +195,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @param y Pose of pose y * @return Whether in collision */ - bool inCollision( + bool inCollision( const double & x, const double & y, const double & theta, @@ -238,7 +238,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller */ double findDirectionChange(const geometry_msgs::msg::PoseStamped & pose); - /** + /** * @brief Set the footprint to use with collision checker * @param footprint The footprint to collision check against * @param radius Whether or not the footprint is a circle and use radius collision checking @@ -284,7 +284,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller std::vector oriented_footprints_; nav2_costmap_2d::Footprint unoriented_footprint_; - unsigned char footprint_cost_; + double footprint_cost_; bool footprint_is_radius_; unsigned int num_quantizations_; double bin_size_; 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 edcd4ed17ef..41579fc449a 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 @@ -38,8 +38,7 @@ namespace nav2_regulated_pure_pursuit_controller { RegulatedPurePursuitController::RegulatedPurePursuitController() - : - tf_(nullptr), +: tf_(nullptr), costmap_ros_(nullptr), costmap_(nullptr), global_path_pub_(nullptr), @@ -49,7 +48,7 @@ RegulatedPurePursuitController::RegulatedPurePursuitController() { } -RegulatedPurePursuitController::~RegulatedPurePursuitController(){} +RegulatedPurePursuitController::~RegulatedPurePursuitController() {} void RegulatedPurePursuitController::configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, @@ -60,7 +59,6 @@ void RegulatedPurePursuitController::configure( if (!node) { throw nav2_core::PlannerException("Unable to lock node!"); } - RCLCPP_INFO(logger_,"saka 1"); costmap_ros_ = costmap_ros; costmap_ = costmap_ros_->getCostmap(); tf_ = tf; @@ -193,7 +191,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); - RCLCPP_INFO(logger_,"saka 2"); } void RegulatedPurePursuitController::cleanup() @@ -206,7 +203,7 @@ void RegulatedPurePursuitController::cleanup() global_path_pub_.reset(); carrot_pub_.reset(); carrot_arc_pub_.reset(); - } +} void RegulatedPurePursuitController::activate() { @@ -269,7 +266,6 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } else { goal_dist_tol_ = pose_tolerance.position.x; } - RCLCPP_INFO(logger_,"saka 3"); // Transform path to robot base frame auto transformed_plan = transformGlobalPlan(pose); @@ -395,25 +391,20 @@ bool RegulatedPurePursuitController::isCollisionImminent( const geometry_msgs::msg::PoseStamped & robot_pose, const double & linear_vel, const double & angular_vel) { - // Note(stevemacenski): This may be a bit unusual, but the robot_pose is in - // odom frame and the carrot_pose is in robot base frame. - RCLCPP_INFO(logger_,"saka 4"); - //_collision_checker = GridCollisionChecker(costmap_, 1 /*for 2D, most be 1*/); _collision_checker.setCostmap(costmap_); - setFootprint(costmap_ros_->getRobotFootprint(),true /*for 2D, most use radius*/, 0.0 /*for 2 - D cost at inscribed isn't relevent*/); - - - RCLCPP_INFO(logger_,"saka 5"); - RCLCPP_INFO(logger_,"saka 5 %lf , %lf ,%lf ", robot_pose.pose.position.x, robot_pose.pose.position.y, tf2::getYaw(robot_pose.pose.orientation)); + setFootprint( + costmap_ros_->getRobotFootprint(), false /*for 2D, most use radius*/, + 0.0 /*for 2D cost at inscribed isn't relevent*/); + // Note(stevemacenski): This may be a bit unusual, but the robot_pose is in + // 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),false)) { - RCLCPP_INFO(logger_,"saka 6"); + if (inCollision( + robot_pose.pose.position.x, robot_pose.pose.position.y, + tf2::getYaw(robot_pose.pose.orientation), false)) + { return true; } - RCLCPP_INFO(logger_,"saka 6-1"); // visualization messages nav_msgs::msg::Path arc_pts_msg; arc_pts_msg.header.frame_id = costmap_ros_->getGlobalFrameID(); @@ -437,7 +428,6 @@ bool RegulatedPurePursuitController::isCollisionImminent( } i++; - RCLCPP_INFO(logger_,"saka 6"); // apply velocity at curr_pose over distance curr_pose.x += projection_time * (linear_vel * cos(curr_pose.theta)); curr_pose.y += projection_time * (linear_vel * sin(curr_pose.theta)); @@ -449,12 +439,11 @@ bool RegulatedPurePursuitController::isCollisionImminent( pose_msg.pose.position.z = 0.01; arc_pts_msg.poses.push_back(pose_msg); - if (inCollision(curr_pose.x, curr_pose.y,curr_pose.theta,false) ){ + // check for collision at this point + if (inCollision(curr_pose.x, curr_pose.y, curr_pose.theta, false)) { carrot_arc_pub_->publish(arc_pts_msg); - RCLCPP_INFO(logger_, "inside collision checker = true "); return true; } - // check for collision at this point } carrot_arc_pub_->publish(arc_pts_msg); @@ -462,11 +451,12 @@ bool RegulatedPurePursuitController::isCollisionImminent( return false; } -bool RegulatedPurePursuitController::inCollision(const double & x, const double & y , const double & theta, - const bool & traverse_unknown) +bool RegulatedPurePursuitController::inCollision( + const double & x, const double & y, const double & theta, + const bool & traverse_unknown) { - // Assumes setFootprint already set - unsigned int wx, wy; +// Assumes setFootprint already set + unsigned int wx, wy; if (!costmap_->worldToMap(x, y, wx, wy)) { RCLCPP_WARN_THROTTLE( @@ -477,62 +467,57 @@ bool RegulatedPurePursuitController::inCollision(const double & x, const double return false; } - RCLCPP_INFO(logger_,"footprint_is_radius_ %i", footprint_is_radius_); - if (!footprint_is_radius_) { - // if footprint, then we check for the footprint's points, but first see - // if the robot is even potentially in an inscribed collision - footprint_cost_ = costmap_->getCost(wx,wy); - - if (footprint_cost_ < possible_inscribed_cost_) { - return false; - } - RCLCPP_INFO(logger_,"saka 30"); - // If its inscribed, in collision, or unknown in the middle, - // no need to even check the footprint, its invalid - if (footprint_cost_ == UNKNOWN && !traverse_unknown) { - return true; - } - RCLCPP_INFO(logger_,"saka 32"); - if (footprint_cost_ == INSCRIBED || footprint_cost_ == OCCUPIED) { - return true; - } - - // if possible inscribed, need to check actual footprint pose. - // Use precomputed oriented footprints are done on initialization, - // offset by translation value to collision check - int angle_bin = theta / bin_size_; - geometry_msgs::msg::Point new_pt; - const nav2_costmap_2d::Footprint & oriented_footprint = oriented_footprints_[angle_bin]; - nav2_costmap_2d::Footprint current_footprint; - current_footprint.reserve(oriented_footprint.size()); - for (unsigned int i = 0; i < oriented_footprint.size(); ++i) { - new_pt.x = wx + oriented_footprint[i].x; - new_pt.y = wy + oriented_footprint[i].y; - current_footprint.push_back(new_pt); - } - - footprint_cost_ = _collision_checker.footprintCost(current_footprint); - RCLCPP_INFO(logger_,"saka 33"); - if (footprint_cost_ == UNKNOWN && traverse_unknown) { - return false; - } - - // if occupied or unknown and not to traverse unknown space - return footprint_cost_ >= OCCUPIED; - } else { - // if radius, then we can check the center of the cost assuming inflation is used - RCLCPP_INFO(logger_,"saka 35"); - footprint_cost_ = costmap_->getCost(wx,wy); - RCLCPP_INFO(logger_,"footprint_cost_ %c", footprint_cost_); - if (footprint_cost_ == UNKNOWN && traverse_unknown) { - return false; - } - RCLCPP_INFO(logger_,"saka 35"); - // if occupied or unknown and not to traverse unknown space - return footprint_cost_ >= INSCRIBED; + if (!footprint_is_radius_) { + // if footprint, then we check for the footprint's points, but first see + // if the robot is even potentially in an inscribed collision + footprint_cost_ = _collision_checker.pointCost(wx, wy); + + if (footprint_cost_ < possible_inscribed_cost_) { + return false; + } + // If its inscribed, in collision, or unknown in the middle, + // no need to even check the footprint, its invalid + if (footprint_cost_ == UNKNOWN && !traverse_unknown) { + return true; + } + if (footprint_cost_ == INSCRIBED || footprint_cost_ == OCCUPIED) { + return true; + } + + // if possible inscribed, need to check actual footprint pose. + // Use precomputed oriented footprints are done on initialization, + // offset by translation value to collision check + int angle_bin = theta / bin_size_; + geometry_msgs::msg::Point new_pt; + const nav2_costmap_2d::Footprint & oriented_footprint = oriented_footprints_[angle_bin]; + nav2_costmap_2d::Footprint current_footprint; + current_footprint.reserve(oriented_footprint.size()); + for (unsigned int i = 0; i < oriented_footprint.size(); ++i) { + new_pt.x = wx + oriented_footprint[i].x; + new_pt.y = wy + oriented_footprint[i].y; + current_footprint.push_back(new_pt); + } + + footprint_cost_ = _collision_checker.footprintCost(current_footprint); + if (footprint_cost_ == UNKNOWN && traverse_unknown) { + return false; } - RCLCPP_INFO(logger_,"saka 3000"); + + // if occupied or unknown and not to traverse unknown space + return footprint_cost_ >= OCCUPIED; + } else { + // if radius, then we can check the center of the cost assuming inflation is used + footprint_cost_ = _collision_checker.pointCost(wx, wy); + RCLCPP_INFO(logger_, "footprint_cost_ %lf", footprint_cost_); + + if (footprint_cost_ == UNKNOWN && traverse_unknown) { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return footprint_cost_ >= INSCRIBED; } +} double RegulatedPurePursuitController::costAtPose(const double & x, const double & y) { @@ -701,7 +686,6 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( double RegulatedPurePursuitController::findDirectionChange( const geometry_msgs::msg::PoseStamped & pose) { - RCLCPP_INFO(logger_,"saka 3"); // Iterating through the global path to determine the position of the cusp for (unsigned int pose_id = 1; pose_id < global_plan_.poses.size() - 1; ++pose_id) { // We have two vectors for the dot product OA and AB. Determining the vectors. @@ -748,48 +732,48 @@ bool RegulatedPurePursuitController::transformPose( } void RegulatedPurePursuitController::setFootprint( - const nav2_costmap_2d::Footprint & footprint, - const bool & radius, - const double & possible_inscribed_cost) - { - possible_inscribed_cost_ = possible_inscribed_cost; - footprint_is_radius_ = radius; + const nav2_costmap_2d::Footprint & footprint, + const bool & radius, + const double & possible_inscribed_cost) +{ + possible_inscribed_cost_ = possible_inscribed_cost; + footprint_is_radius_ = radius; - // Use radius, no caching required - if (radius) { - return; - } + // Use radius, no caching required + if (radius) { + return; + } - // No change, no updates required - if (footprint == unoriented_footprint_) { - return; - } + // No change, no updates required + if (footprint == unoriented_footprint_) { + return; + } - bin_size_ = 2.0 * M_PI / static_cast(num_quantizations_); - oriented_footprints_.reserve(num_quantizations_); - double sin_th, cos_th; - geometry_msgs::msg::Point new_pt; - const unsigned int footprint_size = footprint.size(); - - // Precompute the orientation bins for checking to use - for (unsigned int i = 0; i != num_quantizations_; i++) { - sin_th = sin(i * bin_size_); - cos_th = cos(i * bin_size_); - nav2_costmap_2d::Footprint oriented_footprint; - oriented_footprint.reserve(footprint_size); - - for (unsigned int j = 0; j < footprint_size; j++) { - new_pt.x = footprint[j].x * cos_th - footprint[j].y * sin_th; - new_pt.y = footprint[j].x * sin_th + footprint[j].y * cos_th; - oriented_footprint.push_back(new_pt); - } - - oriented_footprints_.push_back(oriented_footprint); + bin_size_ = 2.0 * M_PI / static_cast(num_quantizations_); + oriented_footprints_.reserve(num_quantizations_); + double sin_th, cos_th; + geometry_msgs::msg::Point new_pt; + const unsigned int footprint_size = footprint.size(); + + // Precompute the orientation bins for checking to use + for (unsigned int i = 0; i != num_quantizations_; i++) { + sin_th = sin(i * bin_size_); + cos_th = cos(i * bin_size_); + nav2_costmap_2d::Footprint oriented_footprint; + oriented_footprint.reserve(footprint_size); + + for (unsigned int j = 0; j < footprint_size; j++) { + new_pt.x = footprint[j].x * cos_th - footprint[j].y * sin_th; + new_pt.y = footprint[j].x * sin_th + footprint[j].y * cos_th; + oriented_footprint.push_back(new_pt); } - unoriented_footprint_ = footprint; + oriented_footprints_.push_back(oriented_footprint); } + unoriented_footprint_ = footprint; +} + } // namespace nav2_regulated_pure_pursuit_controller // Register this controller as a nav2_core plugin From d769f53b1d813ed533d87e30d81fdb618a36920a Mon Sep 17 00:00:00 2001 From: sathak93 Date: Tue, 7 Sep 2021 18:54:09 +0530 Subject: [PATCH 07/26] wrk --- .../regulated_pure_pursuit_controller.hpp | 19 ++- .../src/regulated_pure_pursuit_controller.cpp | 157 +++++++++++++----- 2 files changed, 131 insertions(+), 45 deletions(-) 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 99f80b1432b..12517f82e92 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 @@ -200,7 +200,19 @@ class RegulatedPurePursuitController : public nav2_core::Controller const double & y, const double & theta, const bool & traverse_unknown); - + + /** + * @brief Whether point is in collision + * @param x Pose of pose x + * @param y Pose of pose y + * @return Whether in collision + */ + bool inCollision( + const double & x, + const double & y, + const double & theta, + const nav2_costmap_2d::Footprint & footprint_spec, + const bool & traverse_unknown); /** * @brief Cost at a point * @param x Pose of pose x @@ -286,16 +298,17 @@ class RegulatedPurePursuitController : public nav2_core::Controller nav2_costmap_2d::Footprint unoriented_footprint_; double footprint_cost_; bool footprint_is_radius_; - unsigned int num_quantizations_; + unsigned int num_quantizations_{1}; double bin_size_; double possible_inscribed_cost_{-1}; + nav2_costmap_2d::Footprint oriented_footprint; nav_msgs::msg::Path global_plan_; std::shared_ptr> global_path_pub_; std::shared_ptr> carrot_pub_; std::shared_ptr> carrot_arc_pub_; - nav2_costmap_2d::FootprintCollisionChecker _collision_checker; + 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 41579fc449a..3ed58b588c1 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 @@ -43,8 +43,8 @@ RegulatedPurePursuitController::RegulatedPurePursuitController() costmap_(nullptr), global_path_pub_(nullptr), carrot_pub_(nullptr), - carrot_arc_pub_(nullptr), - _collision_checker(costmap_) + carrot_arc_pub_(nullptr)//, + // _collision_checker(costmap_) { } @@ -191,6 +191,12 @@ 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); + + _collision_checker = std::make_unique>(costmap_); + _collision_checker->setCostmap(costmap_); + setFootprint( + costmap_ros_->getRobotFootprint(), false /*for 2D, most use radius*/, + 0.0 /*for 2D cost at inscribed isn't relevent*/); } void RegulatedPurePursuitController::cleanup() @@ -391,10 +397,7 @@ bool RegulatedPurePursuitController::isCollisionImminent( const geometry_msgs::msg::PoseStamped & robot_pose, const double & linear_vel, const double & angular_vel) { - _collision_checker.setCostmap(costmap_); - setFootprint( - costmap_ros_->getRobotFootprint(), false /*for 2D, most use radius*/, - 0.0 /*for 2D cost at inscribed isn't relevent*/); + // Note(stevemacenski): This may be a bit unusual, but the robot_pose is in // odom frame and the carrot_pose is in robot base frame. @@ -403,6 +406,7 @@ bool RegulatedPurePursuitController::isCollisionImminent( robot_pose.pose.position.x, robot_pose.pose.position.y, tf2::getYaw(robot_pose.pose.orientation), false)) { + RCLCPP_INFO(logger_, "current saka"); return true; } // visualization messages @@ -421,7 +425,7 @@ bool RegulatedPurePursuitController::isCollisionImminent( curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); int i = 1; - while (true) { + while (true && i < 4) { // only forward simulate within time requested if (i * projection_time > max_allowed_time_to_collision_) { break; @@ -438,10 +442,12 @@ bool RegulatedPurePursuitController::isCollisionImminent( pose_msg.pose.position.y = curr_pose.y; pose_msg.pose.position.z = 0.01; arc_pts_msg.poses.push_back(pose_msg); - + + //costmap_ros_->getOrientedFootprint(oriented_footprint); // check for collision at this point - if (inCollision(curr_pose.x, curr_pose.y, curr_pose.theta, false)) { + if (inCollision(curr_pose.x, curr_pose.y, curr_pose.theta, unoriented_footprint_, false)) { carrot_arc_pub_->publish(arc_pts_msg); + RCLCPP_INFO(logger_, "predicted "); return true; } } @@ -470,7 +476,7 @@ bool RegulatedPurePursuitController::inCollision( if (!footprint_is_radius_) { // if footprint, then we check for the footprint's points, but first see // if the robot is even potentially in an inscribed collision - footprint_cost_ = _collision_checker.pointCost(wx, wy); + footprint_cost_ = _collision_checker->pointCost(wx, wy); if (footprint_cost_ < possible_inscribed_cost_) { return false; @@ -483,22 +489,85 @@ bool RegulatedPurePursuitController::inCollision( if (footprint_cost_ == INSCRIBED || footprint_cost_ == OCCUPIED) { return true; } - // if possible inscribed, need to check actual footprint pose. // Use precomputed oriented footprints are done on initialization, // offset by translation value to collision check - int angle_bin = theta / bin_size_; - geometry_msgs::msg::Point new_pt; - const nav2_costmap_2d::Footprint & oriented_footprint = oriented_footprints_[angle_bin]; - nav2_costmap_2d::Footprint current_footprint; - current_footprint.reserve(oriented_footprint.size()); - for (unsigned int i = 0; i < oriented_footprint.size(); ++i) { - new_pt.x = wx + oriented_footprint[i].x; - new_pt.y = wy + oriented_footprint[i].y; - current_footprint.push_back(new_pt); + //int angle_bin = theta / bin_size_; + costmap_ros_->getOrientedFootprint(oriented_footprint); + footprint_cost_ = _collision_checker->footprintCost(oriented_footprint); + RCLCPP_INFO(logger_, "current foot_cost_ %lf %lf %lf %lf", footprint_cost_, theta, x , y); + if (footprint_cost_ == UNKNOWN && traverse_unknown) { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return footprint_cost_ >= OCCUPIED; + } else { + // if radius, then we can check the center of the cost assuming inflation is used + footprint_cost_ = _collision_checker->pointCost(wx, wy); + RCLCPP_INFO(logger_, "current oriented footprint_cost_ %lf", footprint_cost_); + + if (footprint_cost_ == UNKNOWN && traverse_unknown) { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return footprint_cost_ >= INSCRIBED; + } +} + +bool RegulatedPurePursuitController::inCollision( + const double & x, + const double & y, + const double & theta, + const nav2_costmap_2d::Footprint & footprint_spec, + const bool & traverse_unknown) +{ +// Assumes setFootprint already set + unsigned int wx, wy; + + if (!costmap_->worldToMap(x, y, wx, wy)) { + RCLCPP_WARN_THROTTLE( + logger_, *(clock_), 30000, + "The dimensions of the costmap is too small to successfully check for " + "collisions as far ahead as requested. Proceed at your own risk, slow the robot, or " + "increase your costmap size."); + return false; + } + + if (!footprint_is_radius_) { + // if footprint, then we check for the footprint's points, but first see + // if the robot is even potentially in an inscribed collision + footprint_cost_ = _collision_checker->pointCost(wx, wy); + RCLCPP_INFO(logger_, "first point cost %lf ", footprint_cost_); + if (footprint_cost_ < possible_inscribed_cost_) { + return false; + } + // If its inscribed, in collision, or unknown in the middle, + // no need to even check the footprint, its invalid + if (footprint_cost_ == UNKNOWN && !traverse_unknown) { + return true; + } + if (footprint_cost_ == INSCRIBED || footprint_cost_ == OCCUPIED) { + return true; } - footprint_cost_ = _collision_checker.footprintCost(current_footprint); + // if possible inscribed, need to check actual footprint pose. + // Use precomputed oriented footprints are done on initialization, + // offset by translation value to collision check + //int angle_bin = theta / bin_size_; + // nav2_costmap_2d::Footprint transformed_footprint; + // transformed_footprint.clear(); + // double cos_th = cos(theta/(2 * M_PI)); + // double sin_th = sin(theta/ (2* M_PI)); + // for (unsigned int i = 0; i < footprint_spec.size(); ++i) { + // geometry_msgs::msg::Point new_pt; + // new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th); + // new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th); + // transformed_footprint.push_back(new_pt); + // } + footprint_cost_ = _collision_checker->footprintCostAtPose(x, y, theta, footprint_spec); + RCLCPP_INFO(logger_, "predicted foot_cost_ %lf %lf %lf %lf ", footprint_cost_, theta, x , y); if (footprint_cost_ == UNKNOWN && traverse_unknown) { return false; } @@ -507,7 +576,7 @@ bool RegulatedPurePursuitController::inCollision( return footprint_cost_ >= OCCUPIED; } else { // if radius, then we can check the center of the cost assuming inflation is used - footprint_cost_ = _collision_checker.pointCost(wx, wy); + footprint_cost_ = _collision_checker->pointCost(wx, wy); RCLCPP_INFO(logger_, "footprint_cost_ %lf", footprint_cost_); if (footprint_cost_ == UNKNOWN && traverse_unknown) { @@ -743,6 +812,7 @@ void RegulatedPurePursuitController::setFootprint( if (radius) { return; } + RCLCPP_INFO(logger_,"footprint %li",footprint.size()); // No change, no updates required if (footprint == unoriented_footprint_) { @@ -750,26 +820,29 @@ void RegulatedPurePursuitController::setFootprint( } bin_size_ = 2.0 * M_PI / static_cast(num_quantizations_); - oriented_footprints_.reserve(num_quantizations_); - double sin_th, cos_th; - geometry_msgs::msg::Point new_pt; - const unsigned int footprint_size = footprint.size(); - - // Precompute the orientation bins for checking to use - for (unsigned int i = 0; i != num_quantizations_; i++) { - sin_th = sin(i * bin_size_); - cos_th = cos(i * bin_size_); - nav2_costmap_2d::Footprint oriented_footprint; - oriented_footprint.reserve(footprint_size); - - for (unsigned int j = 0; j < footprint_size; j++) { - new_pt.x = footprint[j].x * cos_th - footprint[j].y * sin_th; - new_pt.y = footprint[j].x * sin_th + footprint[j].y * cos_th; - oriented_footprint.push_back(new_pt); - } - - oriented_footprints_.push_back(oriented_footprint); - } + + costmap_ros_->getOrientedFootprint(oriented_footprint); + + // oriented_footprints_.reserve(num_quantizations_); + // double sin_th, cos_th; + // geometry_msgs::msg::Point new_pt; + // const unsigned int footprint_size = footprint.size(); + + // // Precompute the orientation bins for checking to use + // for (unsigned int i = 0; i != num_quantizations_; i++) { + // sin_th = sin(i * bin_size_); + // cos_th = cos(i * bin_size_); + // nav2_costmap_2d::Footprint oriented_footprint; + // oriented_footprint.reserve(footprint_size); + + // for (unsigned int j = 0; j < footprint_size; j++) { + // new_pt.x = footprint[j].x * cos_th - footprint[j].y * sin_th; + // new_pt.y = footprint[j].x * sin_th + footprint[j].y * cos_th; + // oriented_footprint.push_back(new_pt); + // } + + // oriented_footprints_.push_back(oriented_footprint); + // } unoriented_footprint_ = footprint; } From e4cf2db22732b6f40c247186738b32e6645510cb Mon Sep 17 00:00:00 2001 From: sathak93 Date: Tue, 7 Sep 2021 20:31:23 +0530 Subject: [PATCH 08/26] clean_up --- .../regulated_pure_pursuit_controller.hpp | 29 +++-- .../src/regulated_pure_pursuit_controller.cpp | 108 +++++------------- 2 files changed, 42 insertions(+), 95 deletions(-) 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 12517f82e92..f55db1a9215 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 @@ -190,7 +190,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller const double &, const double &); /** - * @brief Whether point is in collision + * @brief Whether point or footprint is in collision * @param x Pose of pose x * @param y Pose of pose y * @return Whether in collision @@ -198,16 +198,18 @@ class RegulatedPurePursuitController : public nav2_core::Controller bool inCollision( const double & x, const double & y, - const double & theta, const bool & traverse_unknown); - + /** - * @brief Whether point is in collision + * @brief checks for collision at projected pose * @param x Pose of pose x * @param y Pose of pose y + * @param theta orientation of Yaw + * @param footprint_spec footprint of the robot unoriented + * @param traverse_unknown ignore UNKNOWN cost (255) as obstuctle * @return Whether in collision */ - bool inCollision( + bool checkCollision( const double & x, const double & y, const double & theta, @@ -253,11 +255,11 @@ class RegulatedPurePursuitController : public nav2_core::Controller /** * @brief Set the footprint to use with collision checker * @param footprint The footprint to collision check against - * @param radius Whether or not the footprint is a circle and use radius collision checking + * @param isCircular Whether or not the footprint is a circle and use radius collision checking */ void setFootprint( const nav2_costmap_2d::Footprint & footprint, - const bool & radius, + const bool & isCircular, const double & possible_inscribed_cost); std::shared_ptr tf_; @@ -293,22 +295,19 @@ class RegulatedPurePursuitController : public nav2_core::Controller double rotate_to_heading_min_angle_; double goal_dist_tol_; bool allow_reversing_; - - std::vector oriented_footprints_; - nav2_costmap_2d::Footprint unoriented_footprint_; double footprint_cost_; - bool footprint_is_radius_; - unsigned int num_quantizations_{1}; - double bin_size_; + bool footprint_is_circular_; double possible_inscribed_cost_{-1}; - nav2_costmap_2d::Footprint oriented_footprint; + nav2_costmap_2d::Footprint unoriented_footprint_; + nav2_costmap_2d::Footprint oriented_footprint_; nav_msgs::msg::Path global_plan_; std::shared_ptr> global_path_pub_; std::shared_ptr> carrot_pub_; std::shared_ptr> carrot_arc_pub_; - std::unique_ptr> _collision_checker; + 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 3ed58b588c1..adf78ca1adf 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 @@ -43,8 +43,7 @@ RegulatedPurePursuitController::RegulatedPurePursuitController() costmap_(nullptr), global_path_pub_(nullptr), carrot_pub_(nullptr), - carrot_arc_pub_(nullptr)//, - // _collision_checker(costmap_) + carrot_arc_pub_(nullptr) { } @@ -192,11 +191,13 @@ void RegulatedPurePursuitController::configure( carrot_pub_ = node->create_publisher("lookahead_point", 1); carrot_arc_pub_ = node->create_publisher("lookahead_collision_arc", 1); - _collision_checker = std::make_unique>(costmap_); + // initialize collision checker and set costmap + _collision_checker = std::make_unique>(costmap_); _collision_checker->setCostmap(costmap_); - setFootprint( - costmap_ros_->getRobotFootprint(), false /*for 2D, most use radius*/, - 0.0 /*for 2D cost at inscribed isn't relevent*/); + + // setup robot footprint + setFootprint(costmap_ros_->getRobotFootprint(), false, 0.0); } void RegulatedPurePursuitController::cleanup() @@ -397,16 +398,12 @@ bool RegulatedPurePursuitController::isCollisionImminent( const geometry_msgs::msg::PoseStamped & robot_pose, const double & linear_vel, const double & angular_vel) { - - // Note(stevemacenski): This may be a bit unusual, but the robot_pose is in // 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), false)) + robot_pose.pose.position.x, robot_pose.pose.position.y, false)) { - RCLCPP_INFO(logger_, "current saka"); return true; } // visualization messages @@ -425,7 +422,7 @@ bool RegulatedPurePursuitController::isCollisionImminent( curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); int i = 1; - while (true && i < 4) { + while (true) { // only forward simulate within time requested if (i * projection_time > max_allowed_time_to_collision_) { break; @@ -442,12 +439,10 @@ bool RegulatedPurePursuitController::isCollisionImminent( pose_msg.pose.position.y = curr_pose.y; pose_msg.pose.position.z = 0.01; arc_pts_msg.poses.push_back(pose_msg); - - //costmap_ros_->getOrientedFootprint(oriented_footprint); - // check for collision at this point - if (inCollision(curr_pose.x, curr_pose.y, curr_pose.theta, unoriented_footprint_, false)) { + + // check for collision at the projected pose + if (checkCollision(curr_pose.x, curr_pose.y, curr_pose.theta, unoriented_footprint_, false)) { carrot_arc_pub_->publish(arc_pts_msg); - RCLCPP_INFO(logger_, "predicted "); return true; } } @@ -458,8 +453,7 @@ bool RegulatedPurePursuitController::isCollisionImminent( } bool RegulatedPurePursuitController::inCollision( - const double & x, const double & y, const double & theta, - const bool & traverse_unknown) + const double & x, const double & y, const bool & traverse_unknown) { // Assumes setFootprint already set unsigned int wx, wy; @@ -473,7 +467,7 @@ bool RegulatedPurePursuitController::inCollision( return false; } - if (!footprint_is_radius_) { + if (!footprint_is_circular_) { // if footprint, then we check for the footprint's points, but first see // if the robot is even potentially in an inscribed collision footprint_cost_ = _collision_checker->pointCost(wx, wy); @@ -489,13 +483,9 @@ bool RegulatedPurePursuitController::inCollision( if (footprint_cost_ == INSCRIBED || footprint_cost_ == OCCUPIED) { return true; } - // if possible inscribed, need to check actual footprint pose. - // Use precomputed oriented footprints are done on initialization, - // offset by translation value to collision check - //int angle_bin = theta / bin_size_; - costmap_ros_->getOrientedFootprint(oriented_footprint); - footprint_cost_ = _collision_checker->footprintCost(oriented_footprint); - RCLCPP_INFO(logger_, "current foot_cost_ %lf %lf %lf %lf", footprint_cost_, theta, x , y); + + costmap_ros_->getOrientedFootprint(oriented_footprint_); + footprint_cost_ = _collision_checker->footprintCost(oriented_footprint_); if (footprint_cost_ == UNKNOWN && traverse_unknown) { return false; } @@ -505,7 +495,6 @@ bool RegulatedPurePursuitController::inCollision( } else { // if radius, then we can check the center of the cost assuming inflation is used footprint_cost_ = _collision_checker->pointCost(wx, wy); - RCLCPP_INFO(logger_, "current oriented footprint_cost_ %lf", footprint_cost_); if (footprint_cost_ == UNKNOWN && traverse_unknown) { return false; @@ -516,12 +505,12 @@ bool RegulatedPurePursuitController::inCollision( } } -bool RegulatedPurePursuitController::inCollision( - const double & x, - const double & y, - const double & theta, - const nav2_costmap_2d::Footprint & footprint_spec, - const bool & traverse_unknown) +bool RegulatedPurePursuitController::checkCollision( + const double & x, + const double & y, + const double & theta, + const nav2_costmap_2d::Footprint & footprint_spec, + const bool & traverse_unknown) { // Assumes setFootprint already set unsigned int wx, wy; @@ -535,11 +524,10 @@ bool RegulatedPurePursuitController::inCollision( return false; } - if (!footprint_is_radius_) { + if (!footprint_is_circular_) { // if footprint, then we check for the footprint's points, but first see // if the robot is even potentially in an inscribed collision footprint_cost_ = _collision_checker->pointCost(wx, wy); - RCLCPP_INFO(logger_, "first point cost %lf ", footprint_cost_); if (footprint_cost_ < possible_inscribed_cost_) { return false; } @@ -552,22 +540,7 @@ bool RegulatedPurePursuitController::inCollision( return true; } - // if possible inscribed, need to check actual footprint pose. - // Use precomputed oriented footprints are done on initialization, - // offset by translation value to collision check - //int angle_bin = theta / bin_size_; - // nav2_costmap_2d::Footprint transformed_footprint; - // transformed_footprint.clear(); - // double cos_th = cos(theta/(2 * M_PI)); - // double sin_th = sin(theta/ (2* M_PI)); - // for (unsigned int i = 0; i < footprint_spec.size(); ++i) { - // geometry_msgs::msg::Point new_pt; - // new_pt.x = x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th); - // new_pt.y = y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th); - // transformed_footprint.push_back(new_pt); - // } footprint_cost_ = _collision_checker->footprintCostAtPose(x, y, theta, footprint_spec); - RCLCPP_INFO(logger_, "predicted foot_cost_ %lf %lf %lf %lf ", footprint_cost_, theta, x , y); if (footprint_cost_ == UNKNOWN && traverse_unknown) { return false; } @@ -577,7 +550,6 @@ bool RegulatedPurePursuitController::inCollision( } else { // if radius, then we can check the center of the cost assuming inflation is used footprint_cost_ = _collision_checker->pointCost(wx, wy); - RCLCPP_INFO(logger_, "footprint_cost_ %lf", footprint_cost_); if (footprint_cost_ == UNKNOWN && traverse_unknown) { return false; @@ -802,47 +774,23 @@ bool RegulatedPurePursuitController::transformPose( void RegulatedPurePursuitController::setFootprint( const nav2_costmap_2d::Footprint & footprint, - const bool & radius, + const bool & isCircular, const double & possible_inscribed_cost) { possible_inscribed_cost_ = possible_inscribed_cost; - footprint_is_radius_ = radius; + footprint_is_circular_ = isCircular; // Use radius, no caching required - if (radius) { + if (isCircular) { return; } - RCLCPP_INFO(logger_,"footprint %li",footprint.size()); // No change, no updates required if (footprint == unoriented_footprint_) { return; } - bin_size_ = 2.0 * M_PI / static_cast(num_quantizations_); - - costmap_ros_->getOrientedFootprint(oriented_footprint); - - // oriented_footprints_.reserve(num_quantizations_); - // double sin_th, cos_th; - // geometry_msgs::msg::Point new_pt; - // const unsigned int footprint_size = footprint.size(); - - // // Precompute the orientation bins for checking to use - // for (unsigned int i = 0; i != num_quantizations_; i++) { - // sin_th = sin(i * bin_size_); - // cos_th = cos(i * bin_size_); - // nav2_costmap_2d::Footprint oriented_footprint; - // oriented_footprint.reserve(footprint_size); - - // for (unsigned int j = 0; j < footprint_size; j++) { - // new_pt.x = footprint[j].x * cos_th - footprint[j].y * sin_th; - // new_pt.y = footprint[j].x * sin_th + footprint[j].y * cos_th; - // oriented_footprint.push_back(new_pt); - // } - - // oriented_footprints_.push_back(oriented_footprint); - // } + costmap_ros_->getOrientedFootprint(oriented_footprint_); unoriented_footprint_ = footprint; } From 6cef4004acf25a6403701d73b4b369c39e96a2c8 Mon Sep 17 00:00:00 2001 From: sathak93 Date: Wed, 8 Sep 2021 12:29:02 +0530 Subject: [PATCH 09/26] remove code repettion --- .../regulated_pure_pursuit_controller.hpp | 13 +---- .../src/regulated_pure_pursuit_controller.cpp | 58 +------------------ 2 files changed, 4 insertions(+), 67 deletions(-) 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 f55db1a9215..365fd20a4f5 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 @@ -189,17 +189,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller const geometry_msgs::msg::PoseStamped &, const double &, const double &); - /** - * @brief Whether point or footprint is in collision - * @param x Pose of pose x - * @param y Pose of pose y - * @return Whether in collision - */ - bool inCollision( - const double & x, - const double & y, - const bool & traverse_unknown); - /** * @brief checks for collision at projected pose * @param x Pose of pose x @@ -209,7 +198,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @param traverse_unknown ignore UNKNOWN cost (255) as obstuctle * @return Whether in collision */ - bool checkCollision( + bool inCollision( const double & x, const double & y, const double & theta, 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 adf78ca1adf..a949eee7155 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 @@ -402,7 +402,8 @@ 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, false)) + robot_pose.pose.position.x, robot_pose.pose.position.y, + tf2::getYaw(robot_pose.pose.orientation), unoriented_footprint_, false)) { return true; } @@ -441,7 +442,7 @@ bool RegulatedPurePursuitController::isCollisionImminent( arc_pts_msg.poses.push_back(pose_msg); // check for collision at the projected pose - if (checkCollision(curr_pose.x, curr_pose.y, curr_pose.theta, unoriented_footprint_, false)) { + if (inCollision(curr_pose.x, curr_pose.y, curr_pose.theta, unoriented_footprint_, false)) { carrot_arc_pub_->publish(arc_pts_msg); return true; } @@ -453,59 +454,6 @@ bool RegulatedPurePursuitController::isCollisionImminent( } bool RegulatedPurePursuitController::inCollision( - const double & x, const double & y, const bool & traverse_unknown) -{ -// Assumes setFootprint already set - unsigned int wx, wy; - - if (!costmap_->worldToMap(x, y, wx, wy)) { - RCLCPP_WARN_THROTTLE( - logger_, *(clock_), 30000, - "The dimensions of the costmap is too small to successfully check for " - "collisions as far ahead as requested. Proceed at your own risk, slow the robot, or " - "increase your costmap size."); - return false; - } - - if (!footprint_is_circular_) { - // if footprint, then we check for the footprint's points, but first see - // if the robot is even potentially in an inscribed collision - footprint_cost_ = _collision_checker->pointCost(wx, wy); - - if (footprint_cost_ < possible_inscribed_cost_) { - return false; - } - // If its inscribed, in collision, or unknown in the middle, - // no need to even check the footprint, its invalid - if (footprint_cost_ == UNKNOWN && !traverse_unknown) { - return true; - } - if (footprint_cost_ == INSCRIBED || footprint_cost_ == OCCUPIED) { - return true; - } - - costmap_ros_->getOrientedFootprint(oriented_footprint_); - footprint_cost_ = _collision_checker->footprintCost(oriented_footprint_); - if (footprint_cost_ == UNKNOWN && traverse_unknown) { - return false; - } - - // if occupied or unknown and not to traverse unknown space - return footprint_cost_ >= OCCUPIED; - } else { - // if radius, then we can check the center of the cost assuming inflation is used - footprint_cost_ = _collision_checker->pointCost(wx, wy); - - if (footprint_cost_ == UNKNOWN && traverse_unknown) { - return false; - } - - // if occupied or unknown and not to traverse unknown space - return footprint_cost_ >= INSCRIBED; - } -} - -bool RegulatedPurePursuitController::checkCollision( const double & x, const double & y, const double & theta, From 7413f17865578b2f3b4cca4b62d9ceb8f1aab1cd Mon Sep 17 00:00:00 2001 From: sathak93 Date: Wed, 8 Sep 2021 19:20:53 +0530 Subject: [PATCH 10/26] rm unused variable --- .../regulated_pure_pursuit_controller.hpp | 1 - .../src/regulated_pure_pursuit_controller.cpp | 2 -- 2 files changed, 3 deletions(-) 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 365fd20a4f5..5937566376e 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 @@ -289,7 +289,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller double possible_inscribed_cost_{-1}; nav2_costmap_2d::Footprint unoriented_footprint_; - nav2_costmap_2d::Footprint oriented_footprint_; nav_msgs::msg::Path global_plan_; std::shared_ptr> global_path_pub_; std::shared_ptr> 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 a949eee7155..e0e17119d91 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 @@ -738,8 +738,6 @@ void RegulatedPurePursuitController::setFootprint( return; } - costmap_ros_->getOrientedFootprint(oriented_footprint_); - unoriented_footprint_ = footprint; } From a465f9d7a872bcc480f9fef5094f63c21ac00aa8 Mon Sep 17 00:00:00 2001 From: sathak93 Date: Wed, 8 Sep 2021 22:25:40 +0530 Subject: [PATCH 11/26] initialize footprint on configure itself --- .../regulated_pure_pursuit_controller.hpp | 11 --- .../src/regulated_pure_pursuit_controller.cpp | 75 ++++++------------- 2 files changed, 21 insertions(+), 65 deletions(-) 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 5937566376e..862e7afef3f 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 @@ -241,16 +241,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller */ double findDirectionChange(const geometry_msgs::msg::PoseStamped & pose); - /** - * @brief Set the footprint to use with collision checker - * @param footprint The footprint to collision check against - * @param isCircular Whether or not the footprint is a circle and use radius collision checking - */ - void setFootprint( - const nav2_costmap_2d::Footprint & footprint, - const bool & isCircular, - const double & possible_inscribed_cost); - std::shared_ptr tf_; std::string plugin_name_; std::shared_ptr costmap_ros_; @@ -285,7 +275,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller double goal_dist_tol_; bool allow_reversing_; double footprint_cost_; - bool footprint_is_circular_; double possible_inscribed_cost_{-1}; nav2_costmap_2d::Footprint unoriented_footprint_; 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 e0e17119d91..6633cec9a90 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 @@ -197,7 +197,8 @@ void RegulatedPurePursuitController::configure( _collision_checker->setCostmap(costmap_); // setup robot footprint - setFootprint(costmap_ros_->getRobotFootprint(), false, 0.0); + unoriented_footprint_ = costmap_ros_->getRobotFootprint(); + possible_inscribed_cost_ = 0.0; } void RegulatedPurePursuitController::cleanup() @@ -472,40 +473,27 @@ bool RegulatedPurePursuitController::inCollision( return false; } - if (!footprint_is_circular_) { - // if footprint, then we check for the footprint's points, but first see - // if the robot is even potentially in an inscribed collision - footprint_cost_ = _collision_checker->pointCost(wx, wy); - if (footprint_cost_ < possible_inscribed_cost_) { - return false; - } - // If its inscribed, in collision, or unknown in the middle, - // no need to even check the footprint, its invalid - if (footprint_cost_ == UNKNOWN && !traverse_unknown) { - return true; - } - if (footprint_cost_ == INSCRIBED || footprint_cost_ == OCCUPIED) { - return true; - } - - footprint_cost_ = _collision_checker->footprintCostAtPose(x, y, theta, footprint_spec); - if (footprint_cost_ == UNKNOWN && traverse_unknown) { - return false; - } - - // if occupied or unknown and not to traverse unknown space - return footprint_cost_ >= OCCUPIED; - } else { - // if radius, then we can check the center of the cost assuming inflation is used - footprint_cost_ = _collision_checker->pointCost(wx, wy); - - if (footprint_cost_ == UNKNOWN && traverse_unknown) { - return false; - } + // if the robot is even potentially in an inscribed collision + footprint_cost_ = _collision_checker->pointCost(wx, wy); + if (footprint_cost_ < possible_inscribed_cost_) { + return false; + } + // If its inscribed, in collision, or unknown in the middle, + // no need to even check the footprint, its invalid + if (footprint_cost_ == UNKNOWN && !traverse_unknown) { + return true; + } + if (footprint_cost_ == INSCRIBED || footprint_cost_ == OCCUPIED) { + return true; + } - // if occupied or unknown and not to traverse unknown space - return footprint_cost_ >= INSCRIBED; + footprint_cost_ = _collision_checker->footprintCostAtPose(x, y, theta, footprint_spec); + if (footprint_cost_ == UNKNOWN && traverse_unknown) { + return false; } + + // if occupied or unknown and not to traverse unknown space + return footprint_cost_ >= OCCUPIED; } double RegulatedPurePursuitController::costAtPose(const double & x, const double & y) @@ -720,27 +708,6 @@ bool RegulatedPurePursuitController::transformPose( return false; } -void RegulatedPurePursuitController::setFootprint( - const nav2_costmap_2d::Footprint & footprint, - const bool & isCircular, - const double & possible_inscribed_cost) -{ - possible_inscribed_cost_ = possible_inscribed_cost; - footprint_is_circular_ = isCircular; - - // Use radius, no caching required - if (isCircular) { - return; - } - - // No change, no updates required - if (footprint == unoriented_footprint_) { - return; - } - - unoriented_footprint_ = footprint; -} - } // namespace nav2_regulated_pure_pursuit_controller // Register this controller as a nav2_core plugin From f58ba26d722911a9364c0ed4953959445bb5ad45 Mon Sep 17 00:00:00 2001 From: sathak93 Date: Wed, 8 Sep 2021 22:40:03 +0530 Subject: [PATCH 12/26] revert to defaulted constructer --- .../regulated_pure_pursuit_controller.hpp | 4 ++-- .../src/regulated_pure_pursuit_controller.cpp | 12 ------------ 2 files changed, 2 insertions(+), 14 deletions(-) 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 862e7afef3f..139a8e89c9f 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 @@ -49,12 +49,12 @@ class RegulatedPurePursuitController : public nav2_core::Controller /** * @brief Constructor for nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController */ - RegulatedPurePursuitController(); + RegulatedPurePursuitController() = default; /** * @brief Destrructor for nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController */ - ~RegulatedPurePursuitController(); + ~RegulatedPurePursuitController() override = default; /** * @brief Configure controller state machine 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 6633cec9a90..13b9fd6f6bb 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 @@ -37,18 +37,6 @@ using namespace nav2_costmap_2d; // NOLINT namespace nav2_regulated_pure_pursuit_controller { -RegulatedPurePursuitController::RegulatedPurePursuitController() -: tf_(nullptr), - costmap_ros_(nullptr), - costmap_(nullptr), - global_path_pub_(nullptr), - carrot_pub_(nullptr), - carrot_arc_pub_(nullptr) -{ -} - -RegulatedPurePursuitController::~RegulatedPurePursuitController() {} - void RegulatedPurePursuitController::configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name, const std::shared_ptr & tf, From 0b572e817e46ee65f101ca96948c8a98daf63802 Mon Sep 17 00:00:00 2001 From: sathak93 Date: Wed, 8 Sep 2021 22:42:36 +0530 Subject: [PATCH 13/26] remove rviz change --- nav2_bringup/launch/rviz_launch.py | 3 +-- .../src/regulated_pure_pursuit_controller.cpp | 1 - 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/nav2_bringup/launch/rviz_launch.py b/nav2_bringup/launch/rviz_launch.py index 9c7a2eda509..9f123b81eb6 100644 --- a/nav2_bringup/launch/rviz_launch.py +++ b/nav2_bringup/launch/rviz_launch.py @@ -58,8 +58,7 @@ def generate_launch_description(): package='rviz2', executable='rviz2', arguments=['-d', rviz_config_file], - #output='screen' - ) + output='screen') namespaced_rviz_config_file = ReplaceString( source_file=rviz_config_file, 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 13b9fd6f6bb..70878a3e0d2 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 @@ -19,7 +19,6 @@ #include #include - #include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp" #include "nav2_core/exceptions.hpp" #include "nav2_util/node_utils.hpp" From b797c55d40aebe21bd81597f1b9258695f57689f Mon Sep 17 00:00:00 2001 From: sathak93 Date: Wed, 8 Sep 2021 22:55:03 +0530 Subject: [PATCH 14/26] remove unneccessary changes --- .../regulated_pure_pursuit_controller.hpp | 1 - .../src/regulated_pure_pursuit_controller.cpp | 4 ++++ 2 files changed, 4 insertions(+), 1 deletion(-) 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 139a8e89c9f..4c0e8141ba7 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 @@ -30,7 +30,6 @@ #include "nav2_util/geometry_utils.hpp" #include "geometry_msgs/msg/pose2_d.hpp" - namespace nav2_regulated_pure_pursuit_controller { const double UNKNOWN = 255; 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 70878a3e0d2..95d7248f331 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 @@ -45,6 +45,7 @@ void RegulatedPurePursuitController::configure( if (!node) { throw nav2_core::PlannerException("Unable to lock node!"); } + costmap_ros_ = costmap_ros; costmap_ = costmap_ros_->getCostmap(); tf_ = tf; @@ -261,6 +262,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } else { goal_dist_tol_ = pose_tolerance.position.x; } + // Transform path to robot base frame auto transformed_plan = transformGlobalPlan(pose); @@ -388,6 +390,7 @@ bool RegulatedPurePursuitController::isCollisionImminent( { // Note(stevemacenski): This may be a bit unusual, but the robot_pose is in // 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, @@ -411,6 +414,7 @@ bool RegulatedPurePursuitController::isCollisionImminent( curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); int i = 1; + while (true) { // only forward simulate within time requested if (i * projection_time > max_allowed_time_to_collision_) { From defbb234a521a8a699c2f617d33527c22e4865b2 Mon Sep 17 00:00:00 2001 From: sathak93 Date: Thu, 9 Sep 2021 08:23:10 +0530 Subject: [PATCH 15/26] review changes --- .../regulated_pure_pursuit_controller.hpp | 14 +------ .../src/regulated_pure_pursuit_controller.cpp | 37 +++++-------------- 2 files changed, 11 insertions(+), 40 deletions(-) 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 4c0e8141ba7..a6f70447d44 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 @@ -32,11 +32,6 @@ namespace nav2_regulated_pure_pursuit_controller { -const double UNKNOWN = 255; -const double OCCUPIED = 254; -const double INSCRIBED = 253; -const double MAX_NON_OBSTACLE = 252; -const double FREE = 0; /** * @class nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController @@ -200,9 +195,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller bool inCollision( const double & x, const double & y, - const double & theta, - const nav2_costmap_2d::Footprint & footprint_spec, - const bool & traverse_unknown); + const double & theta); /** * @brief Cost at a point * @param x Pose of pose x @@ -273,17 +266,14 @@ class RegulatedPurePursuitController : public nav2_core::Controller double rotate_to_heading_min_angle_; double goal_dist_tol_; bool allow_reversing_; - double footprint_cost_; - double possible_inscribed_cost_{-1}; - nav2_costmap_2d::Footprint unoriented_footprint_; nav_msgs::msg::Path global_plan_; std::shared_ptr> global_path_pub_; std::shared_ptr> carrot_pub_; std::shared_ptr> carrot_arc_pub_; std::unique_ptr> - _collision_checker; + 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 95d7248f331..dfb92cb06dd 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 @@ -180,13 +180,9 @@ void RegulatedPurePursuitController::configure( 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_); - - // setup robot footprint - unoriented_footprint_ = costmap_ros_->getRobotFootprint(); - possible_inscribed_cost_ = 0.0; + collision_checker_->setCostmap(costmap_); } void RegulatedPurePursuitController::cleanup() @@ -394,7 +390,7 @@ bool RegulatedPurePursuitController::isCollisionImminent( // check current point is OK if (inCollision( robot_pose.pose.position.x, robot_pose.pose.position.y, - tf2::getYaw(robot_pose.pose.orientation), unoriented_footprint_, false)) + tf2::getYaw(robot_pose.pose.orientation))) { return true; } @@ -434,7 +430,7 @@ bool RegulatedPurePursuitController::isCollisionImminent( 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, unoriented_footprint_, false)) { + if (inCollision(curr_pose.x, curr_pose.y, curr_pose.theta)) { carrot_arc_pub_->publish(arc_pts_msg); return true; } @@ -448,9 +444,7 @@ bool RegulatedPurePursuitController::isCollisionImminent( bool RegulatedPurePursuitController::inCollision( const double & x, const double & y, - const double & theta, - const nav2_costmap_2d::Footprint & footprint_spec, - const bool & traverse_unknown) + const double & theta) { // Assumes setFootprint already set unsigned int wx, wy; @@ -464,27 +458,14 @@ bool RegulatedPurePursuitController::inCollision( return false; } - // if the robot is even potentially in an inscribed collision - footprint_cost_ = _collision_checker->pointCost(wx, wy); - if (footprint_cost_ < possible_inscribed_cost_) { - return false; - } - // If its inscribed, in collision, or unknown in the middle, - // no need to even check the footprint, its invalid - if (footprint_cost_ == UNKNOWN && !traverse_unknown) { - return true; - } - if (footprint_cost_ == INSCRIBED || footprint_cost_ == OCCUPIED) { - return true; - } - - footprint_cost_ = _collision_checker->footprintCostAtPose(x, y, theta, footprint_spec); - if (footprint_cost_ == UNKNOWN && traverse_unknown) { + double footprint_cost_ = collision_checker_->footprintCostAtPose( + x, y, theta, costmap_ros_->getRobotFootprint()); + if (footprint_cost_ == static_cast(NO_INFORMATION)) { return false; } // if occupied or unknown and not to traverse unknown space - return footprint_cost_ >= OCCUPIED; + return footprint_cost_ >= static_cast(LETHAL_OBSTACLE); } double RegulatedPurePursuitController::costAtPose(const double & x, const double & y) From 0743ab26e2a74e9ba5f402a13ab017610c092415 Mon Sep 17 00:00:00 2001 From: sathak93 Date: Thu, 9 Sep 2021 08:36:48 +0530 Subject: [PATCH 16/26] define traverse unkown --- .../regulated_pure_pursuit_controller.hpp | 3 +-- .../src/regulated_pure_pursuit_controller.cpp | 3 ++- 2 files changed, 3 insertions(+), 3 deletions(-) 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 a6f70447d44..42aa2c1fd25 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 @@ -188,8 +188,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @param x Pose of pose x * @param y Pose of pose y * @param theta orientation of Yaw - * @param footprint_spec footprint of the robot unoriented - * @param traverse_unknown ignore UNKNOWN cost (255) as obstuctle * @return Whether in collision */ bool inCollision( @@ -266,6 +264,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller double rotate_to_heading_min_angle_; double goal_dist_tol_; bool allow_reversing_; + bool traverse_unknown; nav_msgs::msg::Path global_plan_; std::shared_ptr> global_path_pub_; 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 dfb92cb06dd..edf454f7436 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 @@ -183,6 +183,7 @@ void RegulatedPurePursuitController::configure( collision_checker_ = std::make_unique>(costmap_); collision_checker_->setCostmap(costmap_); + traverse_unknown = false; } void RegulatedPurePursuitController::cleanup() @@ -460,7 +461,7 @@ bool RegulatedPurePursuitController::inCollision( double footprint_cost_ = collision_checker_->footprintCostAtPose( x, y, theta, costmap_ros_->getRobotFootprint()); - if (footprint_cost_ == static_cast(NO_INFORMATION)) { + if (footprint_cost_ == static_cast(NO_INFORMATION) && traverse_unknown) { return false; } From 1889ad33c5f1a6ce08c159d9dadd3b45236d9395 Mon Sep 17 00:00:00 2001 From: sathak93 Date: Thu, 9 Sep 2021 08:50:11 +0530 Subject: [PATCH 17/26] set true --- .../src/regulated_pure_pursuit_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 edf454f7436..72ca85e10c2 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 @@ -183,7 +183,7 @@ void RegulatedPurePursuitController::configure( collision_checker_ = std::make_unique>(costmap_); collision_checker_->setCostmap(costmap_); - traverse_unknown = false; + traverse_unknown = true; } void RegulatedPurePursuitController::cleanup() From 16e8823b8f75a09df78611ed7f471ed86189d7da Mon Sep 17 00:00:00 2001 From: sathak93 Date: Fri, 10 Sep 2021 06:24:44 +0530 Subject: [PATCH 18/26] reviw changes --- .../regulated_pure_pursuit_controller.hpp | 1 - .../src/regulated_pure_pursuit_controller.cpp | 15 ++++++++------- 2 files changed, 8 insertions(+), 8 deletions(-) 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 42aa2c1fd25..3d32ac6ed16 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 @@ -264,7 +264,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller double rotate_to_heading_min_angle_; double goal_dist_tol_; bool allow_reversing_; - bool traverse_unknown; nav_msgs::msg::Path global_plan_; std::shared_ptr> global_path_pub_; 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 72ca85e10c2..2e91cd2841a 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 @@ -183,7 +183,6 @@ void RegulatedPurePursuitController::configure( collision_checker_ = std::make_unique>(costmap_); collision_checker_->setCostmap(costmap_); - traverse_unknown = true; } void RegulatedPurePursuitController::cleanup() @@ -411,7 +410,6 @@ bool RegulatedPurePursuitController::isCollisionImminent( curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); int i = 1; - while (true) { // only forward simulate within time requested if (i * projection_time > max_allowed_time_to_collision_) { @@ -419,6 +417,7 @@ bool RegulatedPurePursuitController::isCollisionImminent( } i++; + // apply velocity at curr_pose over distance curr_pose.x += projection_time * (linear_vel * cos(curr_pose.theta)); curr_pose.y += projection_time * (linear_vel * sin(curr_pose.theta)); @@ -448,9 +447,9 @@ bool RegulatedPurePursuitController::inCollision( const double & theta) { // Assumes setFootprint already set - unsigned int wx, wy; + unsigned int mx, my; - if (!costmap_->worldToMap(x, y, wx, wy)) { + if (!costmap_->worldToMap(x, y, mx, my)) { RCLCPP_WARN_THROTTLE( logger_, *(clock_), 30000, "The dimensions of the costmap is too small to successfully check for " @@ -459,14 +458,16 @@ bool RegulatedPurePursuitController::inCollision( return false; } - double footprint_cost_ = collision_checker_->footprintCostAtPose( + double footprint_cost = collision_checker_->footprintCostAtPose( x, y, theta, costmap_ros_->getRobotFootprint()); - if (footprint_cost_ == static_cast(NO_INFORMATION) && traverse_unknown) { + if (footprint_cost == static_cast(NO_INFORMATION) && + costmap_ros_->getLayeredCostmap()->isTrackingUnknown()) + { return false; } // if occupied or unknown and not to traverse unknown space - return footprint_cost_ >= static_cast(LETHAL_OBSTACLE); + return footprint_cost >= static_cast(LETHAL_OBSTACLE); } double RegulatedPurePursuitController::costAtPose(const double & x, const double & y) From 631984b7984144ccea47ab0bdc17a33eec75b77f Mon Sep 17 00:00:00 2001 From: sathak93 Date: Wed, 15 Sep 2021 06:09:44 +0530 Subject: [PATCH 19/26] styling changes --- .../src/regulated_pure_pursuit_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 2e91cd2841a..b085b1c9349 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 @@ -394,6 +394,7 @@ bool RegulatedPurePursuitController::isCollisionImminent( { return true; } + // visualization messages nav_msgs::msg::Path arc_pts_msg; arc_pts_msg.header.frame_id = costmap_ros_->getGlobalFrameID(); @@ -446,7 +447,6 @@ bool RegulatedPurePursuitController::inCollision( const double & y, const double & theta) { -// Assumes setFootprint already set unsigned int mx, my; if (!costmap_->worldToMap(x, y, mx, my)) { From 61ccdffc730d445e7cab9daad2c29cd1e7d272d5 Mon Sep 17 00:00:00 2001 From: sathak93 Date: Fri, 8 Oct 2021 22:25:36 +0530 Subject: [PATCH 20/26] collision checking condition --- .../src/regulated_pure_pursuit_controller.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) 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 b085b1c9349..76c3773b995 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 @@ -410,8 +410,14 @@ bool RegulatedPurePursuitController::isCollisionImminent( curr_pose.y = robot_pose.pose.position.y; curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); + // compute distace to goal to stop collision checking when closer to goal + auto last_id = global_plan_.poses.size() - 1; + double dx = global_plan_.poses[last_id].pose.position.x - robot_pose.pose.position.x; + double dy = global_plan_.poses[last_id].pose.position.y - robot_pose.pose.position.y; + double dist_to_goal = std::hypot(dx, dy); + int i = 1; - while (true) { + while (dist_to_goal > linear_vel * max_allowed_time_to_collision_) { // only forward simulate within time requested if (i * projection_time > max_allowed_time_to_collision_) { break; From 21daab57bf2d5feaafb73edd40e29c3d812c2981 Mon Sep 17 00:00:00 2001 From: sathak93 Date: Sat, 9 Oct 2021 19:24:14 +0530 Subject: [PATCH 21/26] used back() --- .../src/regulated_pure_pursuit_controller.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) 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 76c3773b995..be483e838b6 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 @@ -411,9 +411,8 @@ bool RegulatedPurePursuitController::isCollisionImminent( curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); // compute distace to goal to stop collision checking when closer to goal - auto last_id = global_plan_.poses.size() - 1; - double dx = global_plan_.poses[last_id].pose.position.x - robot_pose.pose.position.x; - double dy = global_plan_.poses[last_id].pose.position.y - robot_pose.pose.position.y; + double dx = global_plan_.poses.back().pose.position.x - robot_pose.pose.position.x; + double dy = global_plan_.poses.back().pose.position.y - robot_pose.pose.position.y; double dist_to_goal = std::hypot(dx, dy); int i = 1; From 02554623f9d7dbcf8f1a41d7688c21a40566a19c Mon Sep 17 00:00:00 2001 From: sathak93 Date: Tue, 12 Oct 2021 20:00:21 +0530 Subject: [PATCH 22/26] make loop end condition fixed --- .../src/regulated_pure_pursuit_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 be483e838b6..6af3ca19ae4 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 @@ -416,7 +416,7 @@ bool RegulatedPurePursuitController::isCollisionImminent( double dist_to_goal = std::hypot(dx, dy); int i = 1; - while (dist_to_goal > linear_vel * max_allowed_time_to_collision_) { + while (dist_to_goal > desired_linear_vel_ * max_allowed_time_to_collision_) { // only forward simulate within time requested if (i * projection_time > max_allowed_time_to_collision_) { break; From 97bc1e4fdcae819b03c34e849c5f49afd2d0ca02 Mon Sep 17 00:00:00 2001 From: sathak93 Date: Wed, 27 Oct 2021 19:14:35 +0530 Subject: [PATCH 23/26] review changes --- .../src/regulated_pure_pursuit_controller.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) 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 c979bf7b0d8..9f7780c344c 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 @@ -427,13 +427,10 @@ bool RegulatedPurePursuitController::isCollisionImminent( double dy = global_plan_.poses.back().pose.position.y - robot_pose.pose.position.y; double dist_to_goal = std::hypot(dx, dy); - int i = 1; - while (dist_to_goal > desired_linear_vel_ * max_allowed_time_to_collision_) { - // only forward simulate within time requested - if (i * projection_time > max_allowed_time_to_collision_) { - break; - } + // only forward simulate within time requested + int i = 1; + while (i * projection_time < max_allowed_time_to_collision_) { i++; // apply velocity at curr_pose over distance @@ -441,6 +438,14 @@ bool RegulatedPurePursuitController::isCollisionImminent( curr_pose.y += projection_time * (linear_vel * sin(curr_pose.theta)); curr_pose.theta += projection_time * angular_vel; + dx = global_plan_.poses.back().pose.position.x - curr_pose.x; + dy = global_plan_.poses.back().pose.position.y - curr_pose.y; + dist_to_goal = std::hypot(dx, dy); + + if (dist_to_goal < 2 * costmap_->getResolution()) { + break; + } + // store it for visualization pose_msg.pose.position.x = curr_pose.x; pose_msg.pose.position.y = curr_pose.y; From cb05bb632e655304e379e8b9f579e9bd8e1a3d0f Mon Sep 17 00:00:00 2001 From: sathak93 Date: Mon, 1 Nov 2021 18:40:30 +0530 Subject: [PATCH 24/26] fix --- .../src/regulated_pure_pursuit_controller.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) 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 9f7780c344c..6e7db7e5039 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 @@ -299,10 +299,18 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity (carrot_pose.pose.position.x * carrot_pose.pose.position.x) + (carrot_pose.pose.position.y * carrot_pose.pose.position.y); + // similarly find dist^2 to half of look_ahead_point to smooth the curvature + auto mid_pose = getLookAheadPoint(lookahead_dist / 2.0, transformed_plan); + const double mid_dist2 = + (mid_pose.pose.position.x * mid_pose.pose.position.x) + + (mid_pose.pose.position.y * mid_pose.pose.position.y); + // Find curvature of circle (k = 1 / R) double curvature = 0.0; if (carrot_dist2 > 0.001) { curvature = 2.0 * carrot_pose.pose.position.y / carrot_dist2; + auto curvature2 = 2.0 * mid_pose.pose.position.y / mid_dist2; + curvature = (curvature + curvature2) / 2.0; } // Setting the velocity direction @@ -442,7 +450,7 @@ bool RegulatedPurePursuitController::isCollisionImminent( dy = global_plan_.poses.back().pose.position.y - curr_pose.y; dist_to_goal = std::hypot(dx, dy); - if (dist_to_goal < 2 * costmap_->getResolution()) { + if (dist_to_goal < costmap_->getResolution()) { break; } From edde2afa24428aa9126c417bbdfd9208490bc870 Mon Sep 17 00:00:00 2001 From: sathak93 Date: Thu, 11 Nov 2021 20:27:38 +0530 Subject: [PATCH 25/26] removed midpose curvature --- .../src/regulated_pure_pursuit_controller.cpp | 8 -------- 1 file changed, 8 deletions(-) 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 ee51a5bb8c2..87103e0c844 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 @@ -293,18 +293,10 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity (carrot_pose.pose.position.x * carrot_pose.pose.position.x) + (carrot_pose.pose.position.y * carrot_pose.pose.position.y); - // similarly find dist^2 to half of look_ahead_point to smooth the curvature - auto mid_pose = getLookAheadPoint(lookahead_dist / 2.0, transformed_plan); - const double mid_dist2 = - (mid_pose.pose.position.x * mid_pose.pose.position.x) + - (mid_pose.pose.position.y * mid_pose.pose.position.y); - // Find curvature of circle (k = 1 / R) double curvature = 0.0; if (carrot_dist2 > 0.001) { curvature = 2.0 * carrot_pose.pose.position.y / carrot_dist2; - auto curvature2 = 2.0 * mid_pose.pose.position.y / mid_dist2; - curvature = (curvature + curvature2) / 2.0; } // Setting the velocity direction From a5c59c81fe8d03d3ceaed7b099c5178d9f3af79d Mon Sep 17 00:00:00 2001 From: sathak93 Date: Thu, 11 Nov 2021 20:35:46 +0530 Subject: [PATCH 26/26] removed stop collision check at goal pose --- .../src/regulated_pure_pursuit_controller.cpp | 14 -------------- 1 file changed, 14 deletions(-) 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 87103e0c844..bbd2a44632b 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 @@ -416,12 +416,6 @@ bool RegulatedPurePursuitController::isCollisionImminent( curr_pose.y = robot_pose.pose.position.y; curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); - // compute distace to goal to stop collision checking when closer to goal - double dx = global_plan_.poses.back().pose.position.x - robot_pose.pose.position.x; - double dy = global_plan_.poses.back().pose.position.y - robot_pose.pose.position.y; - double dist_to_goal = std::hypot(dx, dy); - - // only forward simulate within time requested int i = 1; while (i * projection_time < max_allowed_time_to_collision_) { @@ -432,14 +426,6 @@ bool RegulatedPurePursuitController::isCollisionImminent( curr_pose.y += projection_time * (linear_vel * sin(curr_pose.theta)); curr_pose.theta += projection_time * angular_vel; - dx = global_plan_.poses.back().pose.position.x - curr_pose.x; - dy = global_plan_.poses.back().pose.position.y - curr_pose.y; - dist_to_goal = std::hypot(dx, dy); - - if (dist_to_goal < costmap_->getResolution()) { - break; - } - // store it for visualization pose_msg.pose.position.x = curr_pose.x; pose_msg.pose.position.y = curr_pose.y;