From 483f39f66b5b9e20a60313d23da58b007e441b42 Mon Sep 17 00:00:00 2001 From: EJ05 Date: Wed, 23 Mar 2022 19:37:22 -0400 Subject: [PATCH 1/4] lmao --- include/isc_nav/utility/bfs.hpp | 2 +- include/isc_nav/utility/map.hpp | 18 ++- include/isc_nav/utility/rrt.hpp | 266 ++++++++++++++++++++++++++++++++ src/path_planner_node.cpp | 11 ++ 4 files changed, 289 insertions(+), 8 deletions(-) create mode 100644 include/isc_nav/utility/rrt.hpp diff --git a/include/isc_nav/utility/bfs.hpp b/include/isc_nav/utility/bfs.hpp index 59cf7fb..55fdb93 100644 --- a/include/isc_nav/utility/bfs.hpp +++ b/include/isc_nav/utility/bfs.hpp @@ -34,7 +34,7 @@ class BreadthFirstSearch // TODO: Param if ((current - goal_).x < 0.1 && (current - goal_).y < 0.1) { - std::cout << "Goal found, getting path." << std::endl; + //std::cout << "Goal found, getting path." << std::endl; trace_back_path(current, path, came_from); } diff --git a/include/isc_nav/utility/map.hpp b/include/isc_nav/utility/map.hpp index 084d109..6a0f23e 100644 --- a/include/isc_nav/utility/map.hpp +++ b/include/isc_nav/utility/map.hpp @@ -132,13 +132,9 @@ class CostMap return neighbors; } -private: - uint32_t size_x_; - uint32_t size_y_; - float resolution_; - Point2D origin_; - - std::vector costmap_; + double get_size_x() { return size_x_; } + double get_size_y() { return size_y_; } + float get_resolution() { return resolution_; } // Values cooresponding to definition of Occupancy Grid in ROS static constexpr int8_t OCC_GRID_UNKNOWN = -1; @@ -149,5 +145,13 @@ class CostMap static constexpr uint8_t NO_INFORMATION = 255; static constexpr uint8_t LETHAL_OBSTACLE = 254; static constexpr uint8_t FREE_SPACE = 0; + +private: + uint32_t size_x_; + uint32_t size_y_; + float resolution_; + Point2D origin_; + + std::vector costmap_; }; } // namespace isc_nav diff --git a/include/isc_nav/utility/rrt.hpp b/include/isc_nav/utility/rrt.hpp new file mode 100644 index 0000000..d8fd302 --- /dev/null +++ b/include/isc_nav/utility/rrt.hpp @@ -0,0 +1,266 @@ +#pragma once + +#include "isc_nav/utility/point.hpp" +#include +#include +#include +#include + +#include +#include +#include + +namespace isc_nav +{ + class RRT + { + public: + explicit RRT(const nav_msgs::msg::OccupancyGrid& grid) : start_{}, goal_{}, map_(grid) {} + + const nav_msgs::msg::Path get_path(const geometry_msgs::msg::Pose& start_pose, const geometry_msgs::msg::Pose& goal_pose) + { + nav_msgs::msg::Path path; + start_ = Point2D(start_pose.position.x, start_pose.position.y); + goal_ = Point2D(goal_pose.position.x, goal_pose.position.y); + + Point2D random, Xnew, Xnearest; + double Xnearest_dist = 0; + + int lim = 1000; // number of iterations + + // Graph with vertices and edges + V.push_back(start_); + E.push_back(0); + G.push_back(E); + + for (int i = 0; i < lim; i++) + { + random = randomPosition(); + + // Find nearest vertex to the random node + Xnearest = findNearest(V, random, Xnearest_dist); + + if (getDistance(Xnearest, random) <= max_dist_) + { + Xnew = Xnearest; + } + else + { + Xnew = findNew(Xnearest, random, Xnearest_dist); + } + + if (isInObstacle(Xnew)) + { + continue; + } + + // Make sure no obstacle between Xnearest and Xnew + if (!isCollisionFree(Xnearest, Xnew)) + { + continue; + } + + // Get the index of to and from + int attach_to = getIndex(V, Xnew); + int attach_from = getIndex(V, Xnearest); + + // Add edge + if (attach_to == -1) + { + int add_new = V.size(); + V.push_back(Xnew); + E.push_back(0); + + for (int i = 0; i < attach_to; i++) + { + G.at(i).push_back(0); + } + + G.at(attach_from).at(add_new)++; + G.push_back(E); + } + else + { + V.push_back(Xnew); + E.push_back(0); + + for (int i = 0; i < attach_to; i++) + { + G.at(i).push_back(0); + } + + G.at(attach_from).at(attach_to)++; + G.push_back(E); + } + + // If vertex at goal return graph + if ((Xnew.x - goal_.x < 0.1) && (Xnew.y - goal_.y < 0.1)) + { + trace_back_path(Xnew, path); + } + } + + return path; + } + + + Point2D randomPosition() + { + srand(time(0)); + + while (true) + { + int32_t x = fRand(0, map_.get_size_x()); + int32_t y = fRand(0, map_.get_size_y()); + + if (map_.valid_cell(x, y)) + { + Point2D random_node(x, y); + return random_node; + } + } + } + + + Point2D findNearest(std::vector& V, Point2D random_node, double& Xnearest_dist) + { + double dist; + Point2D Xnearest; + + if (!V.empty()) + { + // Assign values to be compared + Xnearest_dist = getDistance(V.at(0), random_node); + Xnearest = V.at(0); + } + + for (auto vertex : V) + { + dist = getDistance(vertex, random_node); + if (dist < Xnearest_dist) + { + Xnearest_dist = dist; + Xnearest = vertex; + } + } + + return Xnearest; + } + + // p1 is Xnearest, p2 is random node + Point2D findNew(Point2D& p1, Point2D& p2, double& Xnearest_dist) + { + Point2D Xnew; + double n = max_dist_; + double m = Xnearest_dist - n; + + Xnew.x = (m * p2.x + n * p1.x)/(m + n); + Xnew.y = (m * p2.y + n * p1.y)/(m + n); + + return Xnew; + } + + + bool isInObstacle(Point2D& random_node) + { + return map_.at(random_node) == CostMap::LETHAL_OBSTACLE; + } + + bool isCollisionFree(Point2D& p1, Point2D& p2) + { + float reso = map_.get_resolution(); + double dist = getDistance(p1, p2); + int num_of_pt = int(std::ceil(dist / reso)); + Point2D pt = p1; + + for(int i = 0; i < num_of_pt; i++) + { + pt = generateNew(pt, p2, dist, reso); + if (isInObstacle(pt)) + { + return false; + } + } + + return true; + } + + Point2D generateNew(Point2D& p1, Point2D& p2, double& dist, float& reso) + { + Point2D Xnew; + double n = reso; + double m = dist - n; + + Xnew.x = (m * p2.x + n * p1.x)/(m + n); + Xnew.y = (m * p2.y + n * p1.y)/(m + n); + + dist -= reso; + return Xnew; + } + + int32_t fRand(int32_t fMin, int32_t fMax) + { + int32_t f = (int32_t)rand() / RAND_MAX; + return fMin + f * (fMax - fMin); + } + + double getDistance(Point2D p1, Point2D p2) + { + double dist = std::sqrt(pow((p1.x + p2.x), 2) + pow((p1.y + p2.y), 2)); + return dist; + } + + int getIndex(std::vector v, Point2D key) + { + auto it = find(v.begin(), v.end(), key); + + // If element was found + if (it != v.end()) + { + // Calculate the index for key + int index = it - v.begin(); + return index; + } + else { + // If the element is not present in the vector + return -1; + } + } + + void trace_back_path(Point2D& current, nav_msgs::msg::Path& path) noexcept + { + int size = V.size(); + int attach_from = getIndex(V, current); + + while (current != start_) + { + geometry_msgs::msg::PoseStamped pose{}; + pose.pose.position.x = current.x; + pose.pose.position.y = current.y; + path.poses.emplace_back(pose); + + for (int i = 0; i < size; i++) + { + if (G.at(i).at(attach_from)) + { + current = V.at(i); + break; + } + } + } + std::reverse(path.poses.begin(), path.poses.end()); + } + + private: + Point2D start_; + Point2D goal_; + CostMap map_; + + std::vector> G; + std::vector E; + std::vector V; + + double max_dist_ = 50; // shld be shorter of longer?? + }; + +} // namespace isc_nav \ No newline at end of file diff --git a/src/path_planner_node.cpp b/src/path_planner_node.cpp index 96b5c99..83291dc 100644 --- a/src/path_planner_node.cpp +++ b/src/path_planner_node.cpp @@ -22,6 +22,7 @@ #include "isc_nav/path_planner_node.hpp" #include +#include namespace isc_nav { @@ -90,12 +91,22 @@ void PathPlanner::update_plan() RCLCPP_INFO(this->get_logger(), "Updating plan."); + /* auto bfs = BreadthFirstSearch(*last_map_state_); nav_msgs::msg::Path bfs_path = bfs.get_path(*last_pos_state_, last_goal_state_->pose); bfs_path.header.frame_id = map_frame_; bfs_path.header.stamp = this->get_clock()->now(); path_publisher_->publish(bfs_path); + */ + + auto rrt = RRT(*last_map_state_); + nav_msgs::msg::Path rrt_path = rrt.get_path(*last_pos_state_, last_goal_state_->pose); + + rrt_path.header.frame_id = map_frame_; + rrt_path.header.stamp = this->get_clock()->now(); + path_publisher_->publish(rrt_path); + } void PathPlanner::map_callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) From 0c6d0abe16685e4cce59d7551206a0cd4a75f751 Mon Sep 17 00:00:00 2001 From: EJ05 Date: Wed, 30 Mar 2022 22:10:20 -0400 Subject: [PATCH 2/4] Andy added for unit testing --- CMakeLists.txt | 30 ++++++++++++++++++++++-------- include/isc_nav/utility/rrt.hpp | 1 + launch/isc_nav.launch.py | 1 + package.xml | 3 +-- src/path_planner_node.cpp | 2 +- 5 files changed, 26 insertions(+), 11 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d6c4e13..545518a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,6 +15,8 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +add_compile_options(-g) + # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) @@ -66,14 +68,26 @@ install( ) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(${PROJECT_NAME}_my_test test/test.cpp) + + # Link isc_nav files + target_include_directories(${PROJECT_NAME}_my_test PUBLIC src/) + + target_include_directories(${PROJECT_NAME}_my_test PUBLIC + $ + $) + + # We need to ament link because we use isc_nav files which need ros + ament_target_dependencies( + ${PROJECT_NAME}_my_test + geometry_msgs + rclcpp + nav_msgs + tf2 + tf2_geometry_msgs + tf2_ros +) endif() ament_package() diff --git a/include/isc_nav/utility/rrt.hpp b/include/isc_nav/utility/rrt.hpp index d8fd302..fd31664 100644 --- a/include/isc_nav/utility/rrt.hpp +++ b/include/isc_nav/utility/rrt.hpp @@ -42,6 +42,7 @@ namespace isc_nav if (getDistance(Xnearest, random) <= max_dist_) { + // If random vertex is short Xnew = Xnearest; } else diff --git a/launch/isc_nav.launch.py b/launch/isc_nav.launch.py index 3d57098..4f7c396 100644 --- a/launch/isc_nav.launch.py +++ b/launch/isc_nav.launch.py @@ -62,6 +62,7 @@ def generate_launch_description(): executable='path_planner_node', name='path_planner_node', output='screen', + prefix=['xterm -e gdb -ex run --args'], parameters=[{ 'use_sim_time': use_sim_time, 'robot_frame': robot_frame, diff --git a/package.xml b/package.xml index d842c6a..962cbee 100644 --- a/package.xml +++ b/package.xml @@ -16,8 +16,7 @@ tf2_ros tf2_geometry_msgs - ament_lint_auto - ament_lint_common + ament_cmake_gtest ament_cmake diff --git a/src/path_planner_node.cpp b/src/path_planner_node.cpp index 83291dc..3aab350 100644 --- a/src/path_planner_node.cpp +++ b/src/path_planner_node.cpp @@ -34,7 +34,7 @@ PathPlanner::PathPlanner(rclcpp::NodeOptions options) { this->declare_parameter("robot_frame", "base_footprint"); this->declare_parameter("map_frame", "map"); - this->declare_parameter("tf_timeout", 0.03f); + this->declare_parameter("tf_timeout", 0.5f); update_params(); param_update_timer_ = this->create_wall_timer( 1000ms, std::bind(&PathPlanner::update_params, this) From d353a7bd94e81fa02a77515721cc08f0bb099a0a Mon Sep 17 00:00:00 2001 From: EJ05 Date: Mon, 4 Apr 2022 20:55:47 -0400 Subject: [PATCH 3/4] Updated according to Andy's review --- CMakeLists.txt | 2 -- include/isc_nav/utility/map.hpp | 6 ++-- include/isc_nav/utility/rrt.hpp | 53 +++++++++++++++++---------------- launch/isc_nav.launch.py | 1 - src/path_planner_node.cpp | 2 +- 5 files changed, 33 insertions(+), 31 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 545518a..0d59388 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,8 +15,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -add_compile_options(-g) - # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) diff --git a/include/isc_nav/utility/map.hpp b/include/isc_nav/utility/map.hpp index 6a0f23e..4b2f0ef 100644 --- a/include/isc_nav/utility/map.hpp +++ b/include/isc_nav/utility/map.hpp @@ -1,8 +1,10 @@ #pragma once +#include #include -#include +#include "../utility/point.hpp" #include +#include "utility" namespace isc_nav { @@ -71,7 +73,7 @@ class CostMap bool valid_cell(const int32_t& x, const int32_t& y) const noexcept { - if (x > size_x_ || y > size_y_ || + if (x > (int64_t)size_x_ || y > (int64_t)size_y_ || x < 0 || y < 0) { return false; diff --git a/include/isc_nav/utility/rrt.hpp b/include/isc_nav/utility/rrt.hpp index fd31664..5460d73 100644 --- a/include/isc_nav/utility/rrt.hpp +++ b/include/isc_nav/utility/rrt.hpp @@ -1,21 +1,27 @@ #pragma once -#include "isc_nav/utility/point.hpp" +#include "../utility/point.hpp" #include -#include +#include "../utility/map.hpp" #include #include #include #include #include +#include namespace isc_nav { class RRT { public: - explicit RRT(const nav_msgs::msg::OccupancyGrid& grid) : start_{}, goal_{}, map_(grid) {} + explicit RRT(const nav_msgs::msg::OccupancyGrid& grid) : start_{}, goal_{}, map_(grid) { + std::random_device rd; + std::mt19937 rng(rd()); + + this->rng = rng; + } const nav_msgs::msg::Path get_path(const geometry_msgs::msg::Pose& start_pose, const geometry_msgs::msg::Pose& goal_pose) { @@ -26,8 +32,6 @@ namespace isc_nav Point2D random, Xnew, Xnearest; double Xnearest_dist = 0; - int lim = 1000; // number of iterations - // Graph with vertices and edges V.push_back(start_); E.push_back(0); @@ -106,13 +110,13 @@ namespace isc_nav Point2D randomPosition() - { - srand(time(0)); - + { + std::uniform_real_distribution dist_x(0.0, map_.get_size_x()); + std::uniform_real_distribution dist_y(0.0, map_.get_size_y()); while (true) { - int32_t x = fRand(0, map_.get_size_x()); - int32_t y = fRand(0, map_.get_size_y()); + double x = dist_x(rng); + double y = dist_y(rng); if (map_.valid_cell(x, y)) { @@ -122,8 +126,7 @@ namespace isc_nav } } - - Point2D findNearest(std::vector& V, Point2D random_node, double& Xnearest_dist) + Point2D findNearest(const std::vector& V, Point2D random_node, double& Xnearest_dist) { double dist; Point2D Xnearest; @@ -149,7 +152,7 @@ namespace isc_nav } // p1 is Xnearest, p2 is random node - Point2D findNew(Point2D& p1, Point2D& p2, double& Xnearest_dist) + Point2D findNew(const Point2D& p1, const Point2D& p2, const double& Xnearest_dist) { Point2D Xnew; double n = max_dist_; @@ -162,16 +165,16 @@ namespace isc_nav } - bool isInObstacle(Point2D& random_node) + bool isInObstacle(const Point2D& random_node) { return map_.at(random_node) == CostMap::LETHAL_OBSTACLE; } - bool isCollisionFree(Point2D& p1, Point2D& p2) + bool isCollisionFree(const Point2D& p1, const Point2D& p2) { float reso = map_.get_resolution(); double dist = getDistance(p1, p2); - int num_of_pt = int(std::ceil(dist / reso)); + int num_of_pt = static_cast(dist / reso); Point2D pt = p1; for(int i = 0; i < num_of_pt; i++) @@ -186,7 +189,7 @@ namespace isc_nav return true; } - Point2D generateNew(Point2D& p1, Point2D& p2, double& dist, float& reso) + Point2D generateNew(const Point2D& p1, const Point2D& p2, double& dist, const float& reso) { Point2D Xnew; double n = reso; @@ -199,19 +202,13 @@ namespace isc_nav return Xnew; } - int32_t fRand(int32_t fMin, int32_t fMax) - { - int32_t f = (int32_t)rand() / RAND_MAX; - return fMin + f * (fMax - fMin); - } - - double getDistance(Point2D p1, Point2D p2) + double getDistance(const Point2D& p1, const Point2D& p2) { double dist = std::sqrt(pow((p1.x + p2.x), 2) + pow((p1.y + p2.y), 2)); return dist; } - int getIndex(std::vector v, Point2D key) + int getIndex(const std::vector& v, const Point2D& key) { auto it = find(v.begin(), v.end(), key); @@ -262,6 +259,12 @@ namespace isc_nav std::vector V; double max_dist_ = 50; // shld be shorter of longer?? + int lim = 1000; // number of iterations + + // random number generator + std::mt19937 rng; + + }; } // namespace isc_nav \ No newline at end of file diff --git a/launch/isc_nav.launch.py b/launch/isc_nav.launch.py index 4f7c396..3d57098 100644 --- a/launch/isc_nav.launch.py +++ b/launch/isc_nav.launch.py @@ -62,7 +62,6 @@ def generate_launch_description(): executable='path_planner_node', name='path_planner_node', output='screen', - prefix=['xterm -e gdb -ex run --args'], parameters=[{ 'use_sim_time': use_sim_time, 'robot_frame': robot_frame, diff --git a/src/path_planner_node.cpp b/src/path_planner_node.cpp index 3aab350..74447af 100644 --- a/src/path_planner_node.cpp +++ b/src/path_planner_node.cpp @@ -20,7 +20,7 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE // SOFTWARE. -#include "isc_nav/path_planner_node.hpp" +#include "../path_planner_node.hpp" #include #include From 05ac8ca9ec1862dfdea6ce4f648b30a725e08ec2 Mon Sep 17 00:00:00 2001 From: EJ05 Date: Fri, 8 Apr 2022 23:03:06 -0400 Subject: [PATCH 4/4] Updated rrt.hpp and namespace --- include/isc_nav/pure_pursuit.hpp | 6 +- include/isc_nav/pure_pursuit_node.hpp | 2 +- include/isc_nav/utility/distance_helper.hpp | 4 + include/isc_nav/utility/point.hpp | 7 +- include/isc_nav/utility/rrt.hpp | 172 ++++++++++++++------ src/path_planner_node.cpp | 2 +- src/pure_pursuit.cpp | 2 +- 7 files changed, 138 insertions(+), 57 deletions(-) diff --git a/include/isc_nav/pure_pursuit.hpp b/include/isc_nav/pure_pursuit.hpp index 0ad785f..4600ceb 100644 --- a/include/isc_nav/pure_pursuit.hpp +++ b/include/isc_nav/pure_pursuit.hpp @@ -27,12 +27,14 @@ #include #include "utility/point.hpp" + + +namespace isc_nav +{ typedef std::vector Path; typedef std::pair Segment3D; typedef std::pair Segment2D; -namespace PurePursuit -{ class PurePursuit { public: diff --git a/include/isc_nav/pure_pursuit_node.hpp b/include/isc_nav/pure_pursuit_node.hpp index 55273a7..4469c3d 100644 --- a/include/isc_nav/pure_pursuit_node.hpp +++ b/include/isc_nav/pure_pursuit_node.hpp @@ -26,7 +26,7 @@ namespace isc_nav class PurePursuitNode : public rclcpp::Node { public: - PurePursuit::PurePursuit m_tracker; + PurePursuit m_tracker; explicit PurePursuitNode(rclcpp::NodeOptions options); private: diff --git a/include/isc_nav/utility/distance_helper.hpp b/include/isc_nav/utility/distance_helper.hpp index b6ab31d..40c76bd 100644 --- a/include/isc_nav/utility/distance_helper.hpp +++ b/include/isc_nav/utility/distance_helper.hpp @@ -3,6 +3,9 @@ #include "point.hpp" #include +namespace isc_nav +{ + constexpr double EPSILON = 1e-3; // distance formula function that finds distance between two points @@ -46,3 +49,4 @@ inline bool approximately_equals( const Point2D& expected, const Point2D& actual return ( std::fabs( actual.x - expected.x ) < EPSILON ) && ( std::fabs( actual.y - expected.y ) < EPSILON ); } +} \ No newline at end of file diff --git a/include/isc_nav/utility/point.hpp b/include/isc_nav/utility/point.hpp index 485ee65..a4aac13 100644 --- a/include/isc_nav/utility/point.hpp +++ b/include/isc_nav/utility/point.hpp @@ -3,6 +3,8 @@ #include #include +namespace isc_nav +{ /** * @brief data structure to represent x and y coordinates for the robot */ @@ -55,12 +57,13 @@ inline bool operator!=(const Point3D& lhs, const Point3D& rhs) { return ( lhs.x != rhs.x && lhs.y != rhs.y && lhs.z != rhs.z ); } +} namespace std { -template<> struct hash +template<> struct hash { - [[nodiscard]] std::size_t operator()(const Point2D& point) const noexcept + [[nodiscard]] std::size_t operator()(const isc_nav::Point2D& point) const noexcept { return std::hash()(static_cast(point.x) ^ (static_cast(point.y) << 16)); } diff --git a/include/isc_nav/utility/rrt.hpp b/include/isc_nav/utility/rrt.hpp index 5460d73..c5758b9 100644 --- a/include/isc_nav/utility/rrt.hpp +++ b/include/isc_nav/utility/rrt.hpp @@ -1,11 +1,14 @@ #pragma once -#include "../utility/point.hpp" +#include #include -#include "../utility/map.hpp" +#include #include #include +#include +#include +#include #include #include #include @@ -16,20 +19,31 @@ namespace isc_nav class RRT { public: - explicit RRT(const nav_msgs::msg::OccupancyGrid& grid) : start_{}, goal_{}, map_(grid) { + /** + * @brief Construct a new RRT object and seed the number generator + * @param grid occupancy grid + */ + //RRT(); + RRT(const nav_msgs::msg::OccupancyGrid& grid) : start_{}, goal_{}, map_(grid) { std::random_device rd; std::mt19937 rng(rd()); this->rng = rng; } + /** + * @brief Get path to start rrt + * @param start_pose a 2D point + * @param goal_pose a 2D point + * @return const nav_msgs::msg::Path + */ const nav_msgs::msg::Path get_path(const geometry_msgs::msg::Pose& start_pose, const geometry_msgs::msg::Pose& goal_pose) { nav_msgs::msg::Path path; start_ = Point2D(start_pose.position.x, start_pose.position.y); goal_ = Point2D(goal_pose.position.x, goal_pose.position.y); - Point2D random, Xnew, Xnearest; + Point2D random{}, Xnew{}, Xnearest{}; double Xnearest_dist = 0; // Graph with vertices and edges @@ -37,65 +51,62 @@ namespace isc_nav E.push_back(0); G.push_back(E); - for (int i = 0; i < lim; i++) + for (int i = 0; i < lim_; i++) { random = randomPosition(); - // Find nearest vertex to the random node - Xnearest = findNearest(V, random, Xnearest_dist); + Xnearest = findNearest(random, Xnearest_dist); if (getDistance(Xnearest, random) <= max_dist_) { - // If random vertex is short + // If the random node is within the max_dist_ set it as the new node Xnew = Xnearest; } else - { + { Xnew = findNew(Xnearest, random, Xnearest_dist); } if (isInObstacle(Xnew)) { - continue; + continue; // Skip this loop } - // Make sure no obstacle between Xnearest and Xnew if (!isCollisionFree(Xnearest, Xnew)) { - continue; + continue; // Skip this loop } - // Get the index of to and from - int attach_to = getIndex(V, Xnew); - int attach_from = getIndex(V, Xnearest); + // Get the index of new random node and the nearest vertex + int attach_to = getIndex(Xnew); + int attach_from = getIndex(Xnearest); + + if (attach_from == -1) { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Xnearest does not exist"); + continue; // Skip this loop + } - // Add edge if (attach_to == -1) { int add_new = V.size(); V.push_back(Xnew); E.push_back(0); - for (int i = 0; i < attach_to; i++) + for (int i = 0; i < add_new; i++) { + // Fill in the new column G.at(i).push_back(0); } - G.at(attach_from).at(add_new)++; + // Fill in new row G.push_back(E); + // Add edge + G.at(attach_from).at(add_new)++; } else { - V.push_back(Xnew); - E.push_back(0); - - for (int i = 0; i < attach_to; i++) - { - G.at(i).push_back(0); - } - + // Add edge G.at(attach_from).at(attach_to)++; - G.push_back(E); } // If vertex at goal return graph @@ -108,7 +119,10 @@ namespace isc_nav return path; } - + /** + * @brief Generate a random position on the map + * @return Point2D + */ Point2D randomPosition() { std::uniform_real_distribution dist_x(0.0, map_.get_size_x()); @@ -118,6 +132,7 @@ namespace isc_nav double x = dist_x(rng); double y = dist_y(rng); + // Check if the generated random node is on the map if (map_.valid_cell(x, y)) { Point2D random_node(x, y); @@ -126,10 +141,17 @@ namespace isc_nav } } - Point2D findNearest(const std::vector& V, Point2D random_node, double& Xnearest_dist) + /** + * @brief Get the vertex (already generated) nearest to the random node + * @param V a list of generated vertices + * @param random_node + * @param Xnearest_dist track the distance between nearest vertex and random node + * @return Point2D + */ + Point2D findNearest(const Point2D& random_node, double& Xnearest_dist) { - double dist; - Point2D Xnearest; + double dist{}; + Point2D Xnearest{}; if (!V.empty()) { @@ -137,12 +159,17 @@ namespace isc_nav Xnearest_dist = getDistance(V.at(0), random_node); Xnearest = V.at(0); } + else { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Empty list"); + return Xnearest; + } for (auto vertex : V) { dist = getDistance(vertex, random_node); if (dist < Xnearest_dist) - { + { + // Update distance between nearest vertex and random node Xnearest_dist = dist; Xnearest = vertex; } @@ -151,10 +178,17 @@ namespace isc_nav return Xnearest; } - // p1 is Xnearest, p2 is random node + /** + * @brief Get a new node that is on the line (connecting the nearest vertex to random node) + and within the max_dist_ to random node + * @param p1 nearest vertex (already in the generated list) + * @param p2 random node + * @param Xnearest_dist track the distance between nearest vertex and random node + * @return Point2D + */ Point2D findNew(const Point2D& p1, const Point2D& p2, const double& Xnearest_dist) { - Point2D Xnew; + Point2D Xnew{}; double n = max_dist_; double m = Xnearest_dist - n; @@ -164,12 +198,24 @@ namespace isc_nav return Xnew; } - + /** + * @brief Check if the new random node is at obstacle on map + * @param random_node a random generated nose + * @return true + * @return false + */ bool isInObstacle(const Point2D& random_node) { return map_.at(random_node) == CostMap::LETHAL_OBSTACLE; } + /** + * @brief Check if there is any obstacle between the nearest vertex and random node + * @param p1 nearest node + * @param p2 random node (aka new node) + * @return true + * @return false + */ bool isCollisionFree(const Point2D& p1, const Point2D& p2) { float reso = map_.get_resolution(); @@ -179,7 +225,7 @@ namespace isc_nav for(int i = 0; i < num_of_pt; i++) { - pt = generateNew(pt, p2, dist, reso); + pt = generateAlongLine(pt, p2, dist, reso); if (isInObstacle(pt)) { return false; @@ -189,9 +235,17 @@ namespace isc_nav return true; } - Point2D generateNew(const Point2D& p1, const Point2D& p2, double& dist, const float& reso) + /** + * @brief Generate a node on the line connecting nearest vertex and new random node + * @param p1 new generated node along the line + * @param p2 new random node + * @param dist distance between the new generated along the line and the random node + * @param reso resolution of the map + * @return Point2D + */ + Point2D generateAlongLine(const Point2D& p1, const Point2D& p2, double& dist, const float& reso) { - Point2D Xnew; + Point2D Xnew{}; double n = reso; double m = dist - n; @@ -202,21 +256,33 @@ namespace isc_nav return Xnew; } + /** + * @brief Get the distance between 2 points on map + * @param p1 a 2D point + * @param p2 a 2D point + * @return double + */ double getDistance(const Point2D& p1, const Point2D& p2) { double dist = std::sqrt(pow((p1.x + p2.x), 2) + pow((p1.y + p2.y), 2)); return dist; } - int getIndex(const std::vector& v, const Point2D& key) + /** + * @brief Get the index of a point in the vector list (which corresponds to graph G) + * @param v + * @param key + * @return int + */ + int getIndex(const Point2D& key) { - auto it = find(v.begin(), v.end(), key); + auto it = find(V.begin(), V.end(), key); // If element was found - if (it != v.end()) + if (it != V.end()) { // Calculate the index for key - int index = it - v.begin(); + int index = it - V.begin(); return index; } else { @@ -225,10 +291,16 @@ namespace isc_nav } } - void trace_back_path(Point2D& current, nav_msgs::msg::Path& path) noexcept + /** + * @brief Generate a path from the graph + * @param current the latest generated random node that falls in the goal + * @param path + */ + void trace_back_path(const Point2D& end, nav_msgs::msg::Path& path) noexcept { int size = V.size(); - int attach_from = getIndex(V, current); + Point2D current = end; + int curr_index = getIndex(current); while (current != start_) { @@ -239,13 +311,14 @@ namespace isc_nav for (int i = 0; i < size; i++) { - if (G.at(i).at(attach_from)) + if (G.at(i).at(curr_index)) { current = V.at(i); break; } } } + std::reverse(path.poses.begin(), path.poses.end()); } @@ -258,13 +331,12 @@ namespace isc_nav std::vector E; std::vector V; - double max_dist_ = 50; // shld be shorter of longer?? - int lim = 1000; // number of iterations + // Constants + double max_dist_ = 50; // Shld be shorter of longer?? + int lim_ = 1000; // Number of iterations - // random number generator + // Random number generator std::mt19937 rng; - - }; } // namespace isc_nav \ No newline at end of file diff --git a/src/path_planner_node.cpp b/src/path_planner_node.cpp index 74447af..d64cba2 100644 --- a/src/path_planner_node.cpp +++ b/src/path_planner_node.cpp @@ -20,7 +20,7 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE // SOFTWARE. -#include "../path_planner_node.hpp" +#include #include #include diff --git a/src/pure_pursuit.cpp b/src/pure_pursuit.cpp index 48dd576..b9d4e62 100644 --- a/src/pure_pursuit.cpp +++ b/src/pure_pursuit.cpp @@ -29,7 +29,7 @@ #include #include -namespace PurePursuit +namespace isc_nav { PurePursuit::PurePursuit(const Path& robot_path, const double& lookahead_distance): m_robot_path(robot_path)