From 14c33e92bd6de9d9abf623efe7cec875b4833e0b Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Thu, 2 Jul 2020 01:36:41 +0530 Subject: [PATCH 01/81] Add lazy_theta_star --- lazy_theta_star_b/CMakeLists.txt | 89 ++++ lazy_theta_star_b/global_planner_plugin.xml | 5 + .../lazy_theta_star_b/lazy_theta_star_b.h | 141 +++++ lazy_theta_star_b/package.xml | 31 ++ lazy_theta_star_b/src/lazy_theta_star_b.cpp | 483 ++++++++++++++++++ 5 files changed, 749 insertions(+) create mode 100644 lazy_theta_star_b/CMakeLists.txt create mode 100644 lazy_theta_star_b/global_planner_plugin.xml create mode 100644 lazy_theta_star_b/include/lazy_theta_star_b/lazy_theta_star_b.h create mode 100644 lazy_theta_star_b/package.xml create mode 100644 lazy_theta_star_b/src/lazy_theta_star_b.cpp diff --git a/lazy_theta_star_b/CMakeLists.txt b/lazy_theta_star_b/CMakeLists.txt new file mode 100644 index 0000000000..98875908c0 --- /dev/null +++ b/lazy_theta_star_b/CMakeLists.txt @@ -0,0 +1,89 @@ +cmake_minimum_required(VERSION 3.5) +project(lazy_theta_star_b) + +set(CMAKE_C_STANDARD 99) +set(CMAKE_CXX_STANDARD 14) + + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -O3) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(nav2_util REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(pluginlib REQUIRED) +find_package(nav2_core REQUIRED) + +include_directories( + include +) + +set(library_name ${PROJECT_NAME}_plugin) + +set(dependencies + rclcpp + rclcpp_action + rclcpp_lifecycle + std_msgs + visualization_msgs + nav2_util + nav2_msgs + builtin_interfaces + tf2_ros + nav2_costmap_2d + nav2_core + pluginlib + ) + +add_library(${library_name} SHARED + src/lazy_theta_star_b.cpp + ) + +set_target_properties(lazy_theta_star_b_plugin PROPERTIES COMPILE_FLAGS "-O3") +ament_target_dependencies(${library_name} + ${dependencies} + ) + +target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file(nav2_core global_planner_plugin.xml) + +install(TARGETS ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} + ) + +install(DIRECTORY include/ + DESTINATION include/ + ) + +install(FILES global_planner_plugin.xml + DESTINATION share/${PROJECT_NAME} + ) + +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() +endif() + + +ament_export_include_directories(include) +ament_export_libraries(${library_name}) +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/lazy_theta_star_b/global_planner_plugin.xml b/lazy_theta_star_b/global_planner_plugin.xml new file mode 100644 index 0000000000..39eae7f80c --- /dev/null +++ b/lazy_theta_star_b/global_planner_plugin.xml @@ -0,0 +1,5 @@ + + + The implementation of lazy theta_star + + \ No newline at end of file diff --git a/lazy_theta_star_b/include/lazy_theta_star_b/lazy_theta_star_b.h b/lazy_theta_star_b/include/lazy_theta_star_b/lazy_theta_star_b.h new file mode 100644 index 0000000000..39a649b87f --- /dev/null +++ b/lazy_theta_star_b/include/lazy_theta_star_b/lazy_theta_star_b.h @@ -0,0 +1,141 @@ + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "nav2_core/global_planner.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/cost_values.hpp" + +namespace lazy_theta_star_b { + typedef int id; + using namespace std; + +#define SMALL 0.001 +#define INF_COST 10000000.0 +#define LETHAL_COST 100 + + template + struct pts { + ptsT x, y; + }; + + struct pos { + id pos_id; + double *g; + double *f; + }; + + struct tree_node { + int x, y; + double g = INF_COST; + double h = INF_COST; + id parentId; + int closed = 0; //0 - unexplored, 1 - closed, -1 - open + double f = INF_COST; + int counter = 0; + int got_here = 0; + }; + + + class LazyThetaStarB : public nav2_core::GlobalPlanner { + + public: + + int lethal_cost = LETHAL_COST; + + std::shared_ptr tf_; + nav2_util::LifecycleNode::SharedPtr node_; + nav2_costmap_2d::Costmap2D *costmap_; + std::string global_frame_, name_; + + double interpolation_dist; + int how_many_corners_; + pts src_{}, dst_{}; + + //stores the cell data + vector data; + + //stores the raw path given by the planner on the basis of their index in the vector data + vector path; + nav_msgs::msg::Path global_path; + + // is the priority queue + vector pq; + int sizeX = 0; + int sizeY = 0; + int how_many_corners; + pts src{}, dst{}; + + + void configure( + rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) override; + + void cleanup() override; + + void activate() override; + + void deactivate() override; + + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &start, + const geometry_msgs::msg::PoseStamped &goal) override; + + + //The Line of Sight checking algorithm, takes in the points, and follows the line joining points + bool los_check(int x0, int y0, int x1, int y1); + + //Gives out the Euclidean Distance + double dist(double ax, double ay, double bx, double by) { + return sqrt(pow(ax - bx, 2) + pow(ay - by, 2)); + } + + //to pop the minimum value of the queue, it is ever so slightly faster than the stl one, + //based on the implementation by Peter Sanders (2000) + void binary_heap_del_min(); + + //to compare between values in thet priority queue + static bool comp(pos p1, pos p2) { + return (*(p1.f) != *(p2.f)) ? (*(p1.f) > *(p2.f)) : (*(p1.g) > *(p2.g)); + } + + nav_msgs::msg::Path linearInterpolation(vector lp, double dist_bw_points); + + pts tf_map_to_world(int x, int y) ; + + bool withinLimits(int x, int y); + + bool isSafe(int cx, int cy); + + void push_to_pq(id id_this); + + void clearRobotCell(int mx, int my); + + void backtrace(vector *waypt, id curr_id); + + //stores the index at which the node data is stored for a particular co-ordinate + vector posn; + + //it sets/resets all the values within posn to 0 + void initializePosn(); + + //functions used to maintain indices + void addIndex(int cx, int cy, id index); + id getIndex(int cx, int cy); + + //nav_msgs::msg::Path AutoLinearInterpolation(vector lp); + }; + +} + diff --git a/lazy_theta_star_b/package.xml b/lazy_theta_star_b/package.xml new file mode 100644 index 0000000000..7e06686661 --- /dev/null +++ b/lazy_theta_star_b/package.xml @@ -0,0 +1,31 @@ + + + + lazy_theta_star_b + 0.0.1 + TODO: Package description + anshu-man + Apache-2.0 + + ament_cmake + rclcpp + nav2_core + rclcpp_action + rclcpp_lifecycle + std_msgs + visualization_msgs + nav2_util + nav2_msgs + builtin_interfaces + tf2_ros + nav2_costmap_2d + pluginlib + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + diff --git a/lazy_theta_star_b/src/lazy_theta_star_b.cpp b/lazy_theta_star_b/src/lazy_theta_star_b.cpp new file mode 100644 index 0000000000..0f58aa2c81 --- /dev/null +++ b/lazy_theta_star_b/src/lazy_theta_star_b.cpp @@ -0,0 +1,483 @@ +#include "lazy_theta_star_b/lazy_theta_star_b.h" +#include "nav2_util/node_utils.hpp" +#include "chrono" + +namespace lazy_theta_star_b { + void LazyThetaStarB::configure(rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) { + node_ = parent; + name_ = name; + tf_ = tf; + costmap_ = costmap_ros->getCostmap(); + global_frame_ = costmap_ros->getGlobalFrameID(); + + nav2_util::declare_parameter_if_not_declared( + node_, name_ + ".how_many_corners", rclcpp::ParameterValue( + 8)); + + node_->get_parameter(name_ + ".how_many_corners", how_many_corners_); + + if (how_many_corners_ != 8) + how_many_corners_ = 4; + + nav2_util::declare_parameter_if_not_declared( + node_, name_ + ".interpolation_dist", rclcpp::ParameterValue( + 0.1)); + + node_->get_parameter(name_ + ".interpolation_dist", interpolation_dist); + + } + + void LazyThetaStarB::cleanup() { + RCLCPP_INFO( + node_->get_logger(), "CleaningUp plugin %s of type LazyThetaStarB", + name_.c_str()); + } + + void LazyThetaStarB::activate() { + RCLCPP_INFO( + node_->get_logger(), "Activating plugin %s of type LazyThetaStarB", + name_.c_str()); + } + + void LazyThetaStarB::deactivate() { + RCLCPP_INFO( + node_->get_logger(), "Deactivating plugin %s of type LazyThetaStarB", + name_.c_str()); + } + + nav_msgs::msg::Path LazyThetaStarB::createPlan( + const geometry_msgs::msg::PoseStamped &start, + const geometry_msgs::msg::PoseStamped &goal) { + + + RCLCPP_INFO( + node_->get_logger(), "Path Search Begins"); + + costmap_->worldToMap(start.pose.position.x, start.pose.position.y, reinterpret_cast(src.x), + reinterpret_cast(src.y)); + + + costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, reinterpret_cast(dst.x), + reinterpret_cast(dst.y)); + src.x = int(src.x); + src.y = int(src.y); + + dst.x = int(dst.x); + dst.y = int(dst.y); + + data.clear(); + pq.clear(); + path.clear(); + posn.clear(); + lethal_cost = LETHAL_COST; + + sizeX = costmap_->getSizeInCellsX(); + sizeY = costmap_->getSizeInCellsY(); + + //has been added so that if the robot is stuck at a place with costmap cost higher than 127, then only for that run, the planner be able to give a path + if (costmap_->getCost(src.x, src.y) >= lethal_cost && costmap_->getCost(src.x, src.y) <= 253) { + lethal_cost = costmap_->getCost(src.x, src.y); + } + + initializePosn(); + data.reserve(int((sizeX) * (sizeY) * 0.01)); + + clearRobotCell(src.x, src.y); + + RCLCPP_INFO( + node_->get_logger(), "GOT THE SOURCE AND DESTINATION ------ %i, %i && %i, %i", + src.x, src.y, dst.x, dst.y); + + + if (!isSafe(src.x, src.y) || !isSafe(dst.x, dst.y) || !withinLimits(src.x, src.y) || + !withinLimits(dst.x, dst.y)) { + RCLCPP_INFO(node_->get_logger(), "NO PATH POSSIBLE!!!!!!"); + global_path.poses.clear(); + return global_path; + + } + + data.push_back({(src.x), (src.y), 0, dist(src.x, src.y, dst.x, dst.y), 0, -1, + dist(src.x, src.y, dst.x, dst.y)}); + + addIndex(src.x, src.y, 0); + + pq.push_back({0, &(data[0].g), &(data[0].f)}); + //added as a buffer, since my binary_heap_del_min starts from the index 1 instead of 0 + pq.push_back({0, &(data[0].g), &(data[0].f)}); + + + //The algorithm begins here + id curr_id = 0; + id id_gen = 1; + id last_id = 0; + int cy; + int cx; + cx = (data[curr_id].x); + cy = (data[curr_id].y); + + if (!los_check(src.x, src.y, dst.x, dst.y)) { + + while (pq.size() > 1) { + + data[curr_id].counter++; + + // The condition if current point is the destination + if (cx == dst.x && cy == dst.y) { + if (los_check(cx, cy, data[data[last_id].parentId].x, data[data[last_id].parentId].y)) { + data[curr_id].parentId = data[last_id].parentId; + data[curr_id].g = data[data[last_id].parentId].g; + data[curr_id].f = data[data[last_id].parentId].f; + break; + } else { + data[curr_id].parentId = last_id; + data[curr_id].g = data[last_id].g; + data[curr_id].f = data[last_id].f; + break; + } + } + + //generates the child nodes location + int moves[8][2] = {{cx, cy + 1}, + {cx + 1, cy}, + {cx, cy - 1}, + {cx - 1, cy}, + {cx + 1, cy + 1}, + {cx + 1, cy - 1}, + {cx - 1, cy - 1}, + {cx - 1, cy + 1}}; + + if (how_many_corners != 8) { + how_many_corners = 4; + } + + int mx, my; + id m_id; + id curr_par = (data[curr_id].parentId); + + //lazy checking for the parent + if (!(los_check(cx, cy, data[curr_par].x, data[curr_par].y)) && isSafe(cx, cy) && + isSafe(data[curr_par].x, data[curr_par].y)) { + + double min_dist = INF_COST; + int min_dist_id = curr_par; + + for (int i = 0; i < how_many_corners; i++) { + mx = moves[i][0]; + my = moves[i][1]; + if (withinLimits(mx, my)) { + m_id = getIndex(mx, my); + if (m_id != 0) { + if (data[m_id].f < min_dist) { + min_dist = data[m_id].f; + min_dist_id = m_id; + } + } + } + data[curr_id].parentId = min_dist_id; + data[curr_id].g = data[min_dist_id].g + dist(cx, cy, data[min_dist_id].x, data[min_dist_id].y); + data[curr_id].f = data[curr_id].g + data[curr_id].h; + } + } + curr_par = data[curr_id].parentId; + + for (int i = 0; i < how_many_corners; i++) { + mx = moves[i][0]; + my = moves[i][1]; + + if (mx == data[curr_par].x && my == data[curr_par].y) + continue; + + if (withinLimits(mx, my)) { + double g_cost = data[curr_par].g + dist(mx, my, data[curr_par].x, data[curr_par].y); + double h_cost, cal_cost; + m_id = getIndex(mx, my); + if (m_id == 0 && isSafe(mx, my)) { + h_cost = dist(mx, my, dst.x, dst.y); + cal_cost = g_cost + h_cost; + data.push_back({int(mx), int(my), g_cost, h_cost, curr_par, -1, cal_cost}); + addIndex(mx, my, id_gen); + push_to_pq(id_gen); + id_gen++; + continue; + } else if (m_id != 0) { + h_cost = data[m_id].h; + cal_cost = g_cost + h_cost; + if (data[m_id].f > cal_cost) { + data[m_id].g = g_cost; + data[m_id].f = cal_cost; + data[m_id].parentId = curr_par; + if (data[m_id].closed == 1) { + data[m_id].closed = -1; + push_to_pq(m_id); + } + } + continue; + } + } + } + + last_id = curr_id; + do { + curr_id = pq[1].pos_id; + cx = data[curr_id].x; + cy = data[curr_id].y; + } while (!isSafe(cx, cy)); + binary_heap_del_min(); + data[curr_id].closed = -1; + + } + + } else { + RCLCPP_INFO(node_->get_logger(), "Straight Path"); + data.push_back({(dst.x), (dst.y), dist(cx, cy, src.x, src.y), 0, 0}); + curr_id = 1; + } + + RCLCPP_INFO(node_->get_logger(), "REACHED DEST %i, %i", dst.x, dst.y); + + backtrace(&path, curr_id); + + cout << data.size() << '\n'; + + cout << "Publishing the Path" << '\n'; + + //reset all the values + global_path = linearInterpolation(path, interpolation_dist); + + geometry_msgs::msg::PoseStamped p_end; + p_end.pose.position.x = goal.pose.position.x; + p_end.pose.position.y = goal.pose.position.y; + p_end.header.stamp = node_->now(); + p_end.header.frame_id = global_frame_; + global_path.poses.push_back(p_end); + + return global_path; + } + + void LazyThetaStarB::initializePosn() { + if (pq.size() != abs(sizeX * sizeY)) { + for (int i = 0; i < sizeY * sizeX; i++) { + posn.push_back(0); + } + } else { + for (int i = 0; i < sizeY * sizeX; i++) { + posn[i] = 0; + } + } + } + + void LazyThetaStarB::addIndex(int cx, int cy, id index) { + posn[sizeX * cy + cx] = index; + } + + id LazyThetaStarB::getIndex(int cx, int cy) { + return posn[sizeX * cy + cx]; + } + + void LazyThetaStarB::backtrace(vector *waypt, id curr_id) { + vector waypt_rev; + do { + waypt_rev.push_back(curr_id); + curr_id = data[curr_id].parentId; + } while (curr_id != 0); + waypt_rev.push_back(0); + + for (int i = waypt_rev.size() - 1; i >= 0; i--) { + waypt->push_back(waypt_rev[i]); + } + + } + + void LazyThetaStarB::binary_heap_del_min() { + int hole = 1; + int succ = 2, size = pq.size() - 1, sz = size; + //the main bottom up part where it compares and assigns the smallest value to the hole + //remember that for this part the heap starts at index 1 + while (succ < sz) { + double k1 = *(pq[succ].f); + double k2 = *(pq[succ + 1].f); + if (k1 > k2) { + succ++; + pq[hole] = pq[succ]; + } else { + pq[hole] = pq[succ]; + } + hole = succ; + succ <<= 1; + } + //this part checks if the value to be used to fill the last row's hole is small or not + //if not small then it slides the small value up till it reaches the apt position + double bubble = *(pq[sz].f); + int pred = hole >> 1; + while (*(pq[pred].f) > bubble) { + pq[hole] = pq[pred]; + hole = pred; + pred >>= 1; + } + //this part simply assigns the last value in the heap to the hole after checking it + //for maintaining the heap data structure + pq[hole] = pq[sz]; + pq.pop_back(); + } + + bool LazyThetaStarB::los_check(int x0, int y0, int x1, int y1) { + + int dy = y1 - y0, dx = x1 - x0, f = 0; + int sx, sy; + + if (dy < 0) { + dy = -dy; + sy = -1; + } else { + sy = 1; + } + + if (dx < 0) { + dx = -dx; + sx = -1; + } else { + sx = 1; + } + + if (dx >= dy) { + while (x0 != x1) { + f += dy; + if (f >= dx) { + if (!isSafe(x0 + (sx - 1) / 2, y0 + (sy - 1) / 2)) + return false; + y0 += sy; + f -= dx; + } + if (f != 0 && !isSafe(x0 + (sx - 1) / 2, y0 + (sy - 1) / 2)) + return false; + if (dy == 0 && !isSafe(x0 + (sx - 1) / 2, y0) && !isSafe(x0 + (sx - 1) / 2, y0 - 1)) + return false; + x0 += sx; + } + } else { + while (y0 != y1) { + f = f + dx; + if (f >= dy) { + if (!isSafe(x0 + (sx - 1) / 2, y0 + (sy - 1) / 2)) + return false; + x0 += sx; + f -= dy; + } + if (f != 0 && !isSafe(x0 + (sx - 1) / 2, y0 + (sy - 1) / 2)) + return false; + if (dx == 0 && !isSafe(x0, y0 + (sy - 1) / 2) && !isSafe(x0 - 1, y0 + (sy - 1) / 2)) + return false; + y0 += sy; + } + } + + return true; + + } + + void LazyThetaStarB::push_to_pq(id id_this) { + pq.push_back({id_this, &(data[id_this].g), &(data[id_this].f)}); + push_heap(pq.begin() + 1, pq.end(), comp); + } + + bool LazyThetaStarB::isSafe(int cx, int cy) { + return (costmap_->getCost(cx, cy) < lethal_cost); + } + + bool LazyThetaStarB::withinLimits(int x, int y) { + return (x > 0 && x <= sizeX && y > 0 && y <= sizeY); + } + + pts LazyThetaStarB::tf_map_to_world(int x, int y) { + pts pt{}; + pt.x = double( + costmap_->getOriginX() + (x + 0.5) * costmap_->getResolution()); + pt.y = double( + costmap_->getOriginY() + (y + 0.5) * costmap_->getResolution()); + // 0.5 added to display the points at the centre of the grid cell + return pt; + } + + nav_msgs::msg::Path LazyThetaStarB::linearInterpolation(vector lp, double dist_bw_points) { + + nav_msgs::msg::Path pa; + pa.header.frame_id = "map"; + for (int i = 0; i < int(lp.size() - 1); i++) { + geometry_msgs::msg::PoseStamped p; + pts pt_copy = tf_map_to_world(data[lp[i]].x, data[lp[i]].y); + + + p.pose.position.x = pt_copy.x; + p.pose.position.y = pt_copy.y; + p.header.stamp = node_->now(); + p.header.frame_id = global_frame_; + pa.poses.push_back(p); + + pts pt2_copy = tf_map_to_world(data[lp[i + 1]].x, data[lp[i + 1]].y); + + double distance = double(dist(pt2_copy.x, pt2_copy.y, pt_copy.x, pt_copy.y)); + int loops = int(distance / dist_bw_points); + double sin_alpha = (pt2_copy.y - pt_copy.y) / distance; + double cos_alpha = (pt2_copy.x - pt_copy.x) / distance; + for (int j = 1; j < loops; j++) { + geometry_msgs::msg::PoseStamped p1; + p1.pose.position.x = pt_copy.x + j * dist_bw_points * cos_alpha; + p1.pose.position.y = pt_copy.y + j * dist_bw_points * sin_alpha; + p1.header.stamp = node_->now(); + p1.header.frame_id = global_frame_; + pa.poses.push_back(p1); + } + geometry_msgs::msg::PoseStamped p2; + p2.pose.position.x = pt2_copy.x; + p2.pose.position.y = pt2_copy.y; + p2.header.stamp = node_->now(); + p2.header.frame_id = global_frame_; + pa.poses.push_back(p2); + + } + + + return pa; + + } + + + void LazyThetaStarB::clearRobotCell(int mx, int my) { + costmap_->setCost(mx, my, nav2_costmap_2d::FREE_SPACE); + } + + + //Code to preferably not to look at + + //this finds the shortest distance between the points of the raw path given by the planner +// nav_msgs::msg::Path LazyThetaStarB::AutoLinearInterpolation(vector lp) { +// +// nav_msgs::msg::Path the_path; +// if (lp.size() > 2) { +// double min_dist = INF_COST; +// +// for (int i = 0; i < int(lp.size() - 1); i++) { +// pts pt_copy = tf_map_to_world(data[lp[i]].x, data[lp[i]].y); +// pts pt2_copy = tf_map_to_world(data[lp[i + 1]].x, data[lp[i + 1]].y) +// double dist_curr = dist(pt_copy.x, pt_copy.y, pt2_copy.x, pt2_copy.y); +// +// if (dist_curr < min_dist) +// min_dist = dist_curr; +// } +// +// the_path = linearInterpolation(lp, min_dist); +// +// } else { +// the_path = linearInterpolation(lp, 0.01); +// } +// RCLCPP_INFO(node_->get_logger(), "SENT BACK THE PATH!!!!!!"); +// return the_path; +// } +} + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(lazy_theta_star_b::LazyThetaStarB, nav2_core::GlobalPlanner) From cc9505832461b9d308cece3c94863506c7a62909 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Thu, 2 Jul 2020 02:44:18 +0530 Subject: [PATCH 02/81] Added license --- .../lazy_theta_star_b/lazy_theta_star_b.h | 37 ++++++++++++++++++- 1 file changed, 36 insertions(+), 1 deletion(-) diff --git a/lazy_theta_star_b/include/lazy_theta_star_b/lazy_theta_star_b.h b/lazy_theta_star_b/include/lazy_theta_star_b/lazy_theta_star_b.h index 39a649b87f..6fda92ebb8 100644 --- a/lazy_theta_star_b/include/lazy_theta_star_b/lazy_theta_star_b.h +++ b/lazy_theta_star_b/include/lazy_theta_star_b/lazy_theta_star_b.h @@ -1,4 +1,39 @@ - +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2020 Anshumaan Singh + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Anshumaan Singh + *********************************************************************/ #include #include #include From 74946f297581f5a8dbe17e91d56325846420406c Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Thu, 2 Jul 2020 02:45:23 +0530 Subject: [PATCH 03/81] Added license --- lazy_theta_star_b/src/lazy_theta_star_b.cpp | 36 +++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/lazy_theta_star_b/src/lazy_theta_star_b.cpp b/lazy_theta_star_b/src/lazy_theta_star_b.cpp index 0f58aa2c81..1816333d5a 100644 --- a/lazy_theta_star_b/src/lazy_theta_star_b.cpp +++ b/lazy_theta_star_b/src/lazy_theta_star_b.cpp @@ -1,3 +1,39 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2020 Anshumaan Singh + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Anshumaan Singh + *********************************************************************/ #include "lazy_theta_star_b/lazy_theta_star_b.h" #include "nav2_util/node_utils.hpp" #include "chrono" From ae6ecc44968398eafd06b1cfa50d8985b3729bdf Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Thu, 2 Jul 2020 23:32:22 +0530 Subject: [PATCH 04/81] Update lazy_theta_star_b.cpp --- lazy_theta_star_b/src/lazy_theta_star_b.cpp | 64 ++++++++++++--------- 1 file changed, 37 insertions(+), 27 deletions(-) diff --git a/lazy_theta_star_b/src/lazy_theta_star_b.cpp b/lazy_theta_star_b/src/lazy_theta_star_b.cpp index 1816333d5a..8c5985399f 100644 --- a/lazy_theta_star_b/src/lazy_theta_star_b.cpp +++ b/lazy_theta_star_b/src/lazy_theta_star_b.cpp @@ -88,8 +88,9 @@ namespace lazy_theta_star_b { const geometry_msgs::msg::PoseStamped &goal) { - RCLCPP_INFO( - node_->get_logger(), "Path Search Begins"); + RCLCPP_INFO( + node_->get_logger(), "GOT THE SOURCE AND DESTINATION ------ (%f, %f) && (%f, %f)", + start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y); costmap_->worldToMap(start.pose.position.x, start.pose.position.y, reinterpret_cast(src.x), reinterpret_cast(src.y)); @@ -122,14 +123,9 @@ namespace lazy_theta_star_b { clearRobotCell(src.x, src.y); - RCLCPP_INFO( - node_->get_logger(), "GOT THE SOURCE AND DESTINATION ------ %i, %i && %i, %i", - src.x, src.y, dst.x, dst.y); - - - if (!isSafe(src.x, src.y) || !isSafe(dst.x, dst.y) || !withinLimits(src.x, src.y) || + if (!isSafe(src.x, src.y) || !isSafe(dst.x, dst.y) || !withinLimits(src.x, src.y) || !withinLimits(dst.x, dst.y)) { - RCLCPP_INFO(node_->get_logger(), "NO PATH POSSIBLE!!!!!!"); + RCLCPP_INFO(node_->get_logger(), "THE SOURCE OR DESTINATION IS EITHER NOT WITHIN THE SIZE MAP OR IS OBSTRUCTED"); global_path.poses.clear(); return global_path; @@ -160,23 +156,7 @@ namespace lazy_theta_star_b { data[curr_id].counter++; - // The condition if current point is the destination - if (cx == dst.x && cy == dst.y) { - if (los_check(cx, cy, data[data[last_id].parentId].x, data[data[last_id].parentId].y)) { - data[curr_id].parentId = data[last_id].parentId; - data[curr_id].g = data[data[last_id].parentId].g; - data[curr_id].f = data[data[last_id].parentId].f; - break; - } else { - data[curr_id].parentId = last_id; - data[curr_id].g = data[last_id].g; - data[curr_id].f = data[last_id].f; - break; - } - } - - //generates the child nodes location - int moves[8][2] = {{cx, cy + 1}, + int moves[8][2] = {{cx, cy + 1}, {cx + 1, cy}, {cx, cy - 1}, {cx - 1, cy}, @@ -192,6 +172,32 @@ namespace lazy_theta_star_b { int mx, my; id m_id; id curr_par = (data[curr_id].parentId); + + // The condition if current point is the destination + if (cx == dst.x && cy == dst.y) { + if (!(los_check(cx, cy, data[curr_par].x, data[curr_par].y))) { + float min_dist = INF_COST; + int min_dist_id = curr_par; + + for (int i = 0; i < how_many_corners; i++) { + mx = moves[i][0]; + my = moves[i][1]; + if (withinLimits(mx, my)) { + m_id = getIndex(mx, my); + if (m_id != 0) { + if (data[m_id].f < min_dist) { + min_dist = data[m_id].f; + min_dist_id = m_id; + } + } + } + data[curr_id].parentId = min_dist_id; + data[curr_id].g = data[min_dist_id].g + dist(cx, cy, data[min_dist_id].x, data[min_dist_id].y); + data[curr_id].f = data[curr_id].g + data[curr_id].h; + } + } + break; + } //lazy checking for the parent if (!(los_check(cx, cy, data[curr_par].x, data[curr_par].y)) && isSafe(cx, cy) && @@ -265,9 +271,13 @@ namespace lazy_theta_star_b { data[curr_id].closed = -1; } + + if(pq.size() == 1){ + RCLCPP_INFO(node_->get_logger(), "NO PATH POSSIBLE"); + } } else { - RCLCPP_INFO(node_->get_logger(), "Straight Path"); + RCLCPP_INFO(node_->get_logger(), "STRAIGHT LINE PATH"); data.push_back({(dst.x), (dst.y), dist(cx, cy, src.x, src.y), 0, 0}); curr_id = 1; } From 24ab8232e63516e0fa516e95da141245324e412d Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Sat, 4 Jul 2020 09:29:56 +0530 Subject: [PATCH 05/81] restructured files - separated planner part and the algorithm - implemented all the changes as suggested --- lazy_theta_star_b/CMakeLists.txt | 7 +- lazy_theta_star_b/global_planner_plugin.xml | 2 +- .../lazy_theta_star_b/lazy_theta_star_b.h | 286 +++--- .../include/lazy_theta_star_b/planner.h | 75 ++ lazy_theta_star_b/package.xml | 3 +- lazy_theta_star_b/src/lazy_theta_star_b.cpp | 843 ++++++++---------- lazy_theta_star_b/src/planner.cpp | 145 +++ 7 files changed, 694 insertions(+), 667 deletions(-) create mode 100644 lazy_theta_star_b/include/lazy_theta_star_b/planner.h create mode 100644 lazy_theta_star_b/src/planner.cpp diff --git a/lazy_theta_star_b/CMakeLists.txt b/lazy_theta_star_b/CMakeLists.txt index 98875908c0..dc04e6d324 100644 --- a/lazy_theta_star_b/CMakeLists.txt +++ b/lazy_theta_star_b/CMakeLists.txt @@ -45,10 +45,9 @@ set(dependencies ) add_library(${library_name} SHARED + src/planner.cpp src/lazy_theta_star_b.cpp ) - -set_target_properties(lazy_theta_star_b_plugin PROPERTIES COMPILE_FLAGS "-O3") ament_target_dependencies(${library_name} ${dependencies} ) @@ -75,10 +74,10 @@ 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) + # 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) + # set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() diff --git a/lazy_theta_star_b/global_planner_plugin.xml b/lazy_theta_star_b/global_planner_plugin.xml index 39eae7f80c..5eb448e56b 100644 --- a/lazy_theta_star_b/global_planner_plugin.xml +++ b/lazy_theta_star_b/global_planner_plugin.xml @@ -1,5 +1,5 @@ - + The implementation of lazy theta_star \ No newline at end of file diff --git a/lazy_theta_star_b/include/lazy_theta_star_b/lazy_theta_star_b.h b/lazy_theta_star_b/include/lazy_theta_star_b/lazy_theta_star_b.h index 6fda92ebb8..9a2641f4d1 100644 --- a/lazy_theta_star_b/include/lazy_theta_star_b/lazy_theta_star_b.h +++ b/lazy_theta_star_b/include/lazy_theta_star_b/lazy_theta_star_b.h @@ -1,176 +1,122 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2020 Anshumaan Singh - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Anshumaan Singh - *********************************************************************/ -#include -#include -#include -#include -#include -#include -#include - +// Copyright 2020 Anshumaan Singh +// +// 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. + +#ifndef LAZY_THETA_STAR_B__LAZY_THETA_STAR_B_H_ +#define LAZY_THETA_STAR_B__LAZY_THETA_STAR_B_H_ + +#include +#include +#include +#include +#include +#include #include "rclcpp/rclcpp.hpp" - -#include "nav2_core/global_planner.hpp" -#include "nav_msgs/msg/path.hpp" -#include "nav2_util/robot_utils.hpp" -#include "nav2_util/lifecycle_node.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_costmap_2d/cost_values.hpp" -namespace lazy_theta_star_b { - typedef int id; - using namespace std; +typedef int id; -#define SMALL 0.001 #define INF_COST 10000000.0 -#define LETHAL_COST 100 - - template - struct pts { - ptsT x, y; - }; - - struct pos { - id pos_id; - double *g; - double *f; - }; - - struct tree_node { - int x, y; - double g = INF_COST; - double h = INF_COST; - id parentId; - int closed = 0; //0 - unexplored, 1 - closed, -1 - open - double f = INF_COST; - int counter = 0; - int got_here = 0; - }; - - - class LazyThetaStarB : public nav2_core::GlobalPlanner { - - public: - - int lethal_cost = LETHAL_COST; - - std::shared_ptr tf_; - nav2_util::LifecycleNode::SharedPtr node_; - nav2_costmap_2d::Costmap2D *costmap_; - std::string global_frame_, name_; - - double interpolation_dist; - int how_many_corners_; - pts src_{}, dst_{}; - - //stores the cell data - vector data; - - //stores the raw path given by the planner on the basis of their index in the vector data - vector path; - nav_msgs::msg::Path global_path; - - // is the priority queue - vector pq; - int sizeX = 0; - int sizeY = 0; - int how_many_corners; - pts src{}, dst{}; - - - void configure( - rclcpp_lifecycle::LifecycleNode::SharedPtr parent, - std::string name, std::shared_ptr tf, - std::shared_ptr costmap_ros) override; - - void cleanup() override; - - void activate() override; - - void deactivate() override; - - nav_msgs::msg::Path createPlan( - const geometry_msgs::msg::PoseStamped &start, - const geometry_msgs::msg::PoseStamped &goal) override; - - - //The Line of Sight checking algorithm, takes in the points, and follows the line joining points - bool los_check(int x0, int y0, int x1, int y1); - - //Gives out the Euclidean Distance - double dist(double ax, double ay, double bx, double by) { - return sqrt(pow(ax - bx, 2) + pow(ay - by, 2)); - } - - //to pop the minimum value of the queue, it is ever so slightly faster than the stl one, - //based on the implementation by Peter Sanders (2000) - void binary_heap_del_min(); - - //to compare between values in thet priority queue - static bool comp(pos p1, pos p2) { - return (*(p1.f) != *(p2.f)) ? (*(p1.f) > *(p2.f)) : (*(p1.g) > *(p2.g)); - } - - nav_msgs::msg::Path linearInterpolation(vector lp, double dist_bw_points); - - pts tf_map_to_world(int x, int y) ; - - bool withinLimits(int x, int y); - - bool isSafe(int cx, int cy); - - void push_to_pq(id id_this); - - void clearRobotCell(int mx, int my); - - void backtrace(vector *waypt, id curr_id); - - //stores the index at which the node data is stored for a particular co-ordinate - vector posn; - - //it sets/resets all the values within posn to 0 - void initializePosn(); - - //functions used to maintain indices - void addIndex(int cx, int cy, id index); - id getIndex(int cx, int cy); - - //nav_msgs::msg::Path AutoLinearInterpolation(vector lp); - }; - -} - +#define LETHAL_COST 127 + +template +struct pts +{ + ptsT x, y; +}; + +struct pos +{ + id pos_id; + double * g; + double * f; +}; + +struct tree_node +{ + int x, y; + double g = INF_COST; + double h = INF_COST; + id parentId; + int closed = 0; // 0 - unexplored, 1 - closed, -1 - open + double f = INF_COST; + int got_here = 0; +}; + +namespace lazyThetaStarB +{ + +class LazyThetaStarB +{ +public: + int lethal_cost = LETHAL_COST; + + nav2_util::LifecycleNode::SharedPtr node_; + nav2_costmap_2d::Costmap2D * costmap_; + + // stores the cell data + std::vector data; + + // stores the raw path given by the planner on the basis of their index in the std::vector data + std::vector path; + + // is the priority queue + std::vector pq; + int sizeX = 0; + int sizeY = 0; + int how_many_corners; + pts src{}, dst{}; + + // The part containing the algorithm + void makePlan(std::vector & x_path, std::vector & y_path); + + // The Line of Sight checking algorithm, takes in the points, and follows the line joining points + bool losCheck(int x0, int y0, int x1, int y1); + + // to pop the minimum value of the queue, it is ever so slightly faster than the stl one, + // based on the implementation by Peter Sanders (2000) + void binaryHeapDelMin(); + + // to compare between values in the priority queue + static bool comp(pos p1, pos p2) + { + return (*(p1.f) != *(p2.f)) ? (*(p1.f) > *(p2.f)) : (*(p1.g) > *(p2.g)); + } + + double dist(int & ax, int & ay, int & bx, int & by) + { + return sqrt(pow(ax - bx, 2) + pow(ay - by, 2)); + } + + bool withinLimits(const int & x, const int & y); + bool isSafe(const int & cx, const int & cy); + void pushToPq(id & idThis); + void clearRobotCell(int mx, int my); + void backtrace(std::vector * waypt, id curr_id); + + // stores the index at which the node data is stored for a particular co-ordinate + std::vector posn; + + // it sets/resets all the values within posn to 0 + void initializePosn(); + + // functions used to maintain indices + void addIndex(const int & cx, const int & cy, const id & index); + id getIndex(const int & cx, const int & cy); + + bool isSafe2(int cx, int cy); +}; + +} // namespace lazyThetaStarB +#endif // LAZY_THETA_STAR_B__LAZY_THETA_STAR_B_H_ diff --git a/lazy_theta_star_b/include/lazy_theta_star_b/planner.h b/lazy_theta_star_b/include/lazy_theta_star_b/planner.h new file mode 100644 index 0000000000..118c8dbbfa --- /dev/null +++ b/lazy_theta_star_b/include/lazy_theta_star_b/planner.h @@ -0,0 +1,75 @@ +// Copyright 2020 Anshumaan Singh +// +// 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. + +#ifndef LAZY_THETA_STAR_B__PLANNER_H_ +#define LAZY_THETA_STAR_B__PLANNER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "nav2_core/global_planner.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_util/node_utils.hpp" +#include "lazy_theta_star_b/lazy_theta_star_b.h" + +namespace planner +{ + +template +struct pts +{ + ptsT x, y; +}; + +class planner : public nav2_core::GlobalPlanner +{ +public: + std::shared_ptr tf_; + nav2_util::LifecycleNode::SharedPtr node_; + nav2_costmap_2d::Costmap2D * costmap_; + + std::string globalFrame_, name_; + int how_many_corners_; + double interpolation_dist_; + std::vector x, y; + std::unique_ptr planner_; + + void configure( + rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmapRos) override; + + void cleanup() override; + void activate() override; + void deactivate() override; + + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) override; + + nav_msgs::msg::Path linearInterpolation(double distBwPoints); +}; + +} // namespace planner +#endif // LAZY_THETA_STAR_B__PLANNER_H_ diff --git a/lazy_theta_star_b/package.xml b/lazy_theta_star_b/package.xml index 7e06686661..fa361b2860 100644 --- a/lazy_theta_star_b/package.xml +++ b/lazy_theta_star_b/package.xml @@ -2,7 +2,7 @@ lazy_theta_star_b - 0.0.1 + 0.4.0 TODO: Package description anshu-man Apache-2.0 @@ -23,6 +23,7 @@ ament_lint_auto ament_lint_common + ament_cmake_gtest ament_cmake diff --git a/lazy_theta_star_b/src/lazy_theta_star_b.cpp b/lazy_theta_star_b/src/lazy_theta_star_b.cpp index 8c5985399f..7e5aafdb4f 100644 --- a/lazy_theta_star_b/src/lazy_theta_star_b.cpp +++ b/lazy_theta_star_b/src/lazy_theta_star_b.cpp @@ -1,529 +1,390 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2020 Anshumaan Singh - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Anshumaan Singh - *********************************************************************/ -#include "lazy_theta_star_b/lazy_theta_star_b.h" -#include "nav2_util/node_utils.hpp" -#include "chrono" - -namespace lazy_theta_star_b { - void LazyThetaStarB::configure(rclcpp_lifecycle::LifecycleNode::SharedPtr parent, - std::string name, std::shared_ptr tf, - std::shared_ptr costmap_ros) { - node_ = parent; - name_ = name; - tf_ = tf; - costmap_ = costmap_ros->getCostmap(); - global_frame_ = costmap_ros->getGlobalFrameID(); - - nav2_util::declare_parameter_if_not_declared( - node_, name_ + ".how_many_corners", rclcpp::ParameterValue( - 8)); - - node_->get_parameter(name_ + ".how_many_corners", how_many_corners_); - - if (how_many_corners_ != 8) - how_many_corners_ = 4; - - nav2_util::declare_parameter_if_not_declared( - node_, name_ + ".interpolation_dist", rclcpp::ParameterValue( - 0.1)); - - node_->get_parameter(name_ + ".interpolation_dist", interpolation_dist); - - } - - void LazyThetaStarB::cleanup() { - RCLCPP_INFO( - node_->get_logger(), "CleaningUp plugin %s of type LazyThetaStarB", - name_.c_str()); - } - - void LazyThetaStarB::activate() { - RCLCPP_INFO( - node_->get_logger(), "Activating plugin %s of type LazyThetaStarB", - name_.c_str()); - } - - void LazyThetaStarB::deactivate() { - RCLCPP_INFO( - node_->get_logger(), "Deactivating plugin %s of type LazyThetaStarB", - name_.c_str()); - } - - nav_msgs::msg::Path LazyThetaStarB::createPlan( - const geometry_msgs::msg::PoseStamped &start, - const geometry_msgs::msg::PoseStamped &goal) { - - - RCLCPP_INFO( - node_->get_logger(), "GOT THE SOURCE AND DESTINATION ------ (%f, %f) && (%f, %f)", - start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y); - - costmap_->worldToMap(start.pose.position.x, start.pose.position.y, reinterpret_cast(src.x), - reinterpret_cast(src.y)); - - - costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, reinterpret_cast(dst.x), - reinterpret_cast(dst.y)); - src.x = int(src.x); - src.y = int(src.y); - - dst.x = int(dst.x); - dst.y = int(dst.y); - - data.clear(); - pq.clear(); - path.clear(); - posn.clear(); - lethal_cost = LETHAL_COST; - - sizeX = costmap_->getSizeInCellsX(); - sizeY = costmap_->getSizeInCellsY(); - - //has been added so that if the robot is stuck at a place with costmap cost higher than 127, then only for that run, the planner be able to give a path - if (costmap_->getCost(src.x, src.y) >= lethal_cost && costmap_->getCost(src.x, src.y) <= 253) { - lethal_cost = costmap_->getCost(src.x, src.y); - } - - initializePosn(); - data.reserve(int((sizeX) * (sizeY) * 0.01)); - - clearRobotCell(src.x, src.y); - - if (!isSafe(src.x, src.y) || !isSafe(dst.x, dst.y) || !withinLimits(src.x, src.y) || - !withinLimits(dst.x, dst.y)) { - RCLCPP_INFO(node_->get_logger(), "THE SOURCE OR DESTINATION IS EITHER NOT WITHIN THE SIZE MAP OR IS OBSTRUCTED"); - global_path.poses.clear(); - return global_path; - - } +// Copyright 2020 Anshumaan Singh +// +// 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. - data.push_back({(src.x), (src.y), 0, dist(src.x, src.y, dst.x, dst.y), 0, -1, - dist(src.x, src.y, dst.x, dst.y)}); - - addIndex(src.x, src.y, 0); - - pq.push_back({0, &(data[0].g), &(data[0].f)}); - //added as a buffer, since my binary_heap_del_min starts from the index 1 instead of 0 - pq.push_back({0, &(data[0].g), &(data[0].f)}); - - - //The algorithm begins here - id curr_id = 0; - id id_gen = 1; - id last_id = 0; - int cy; - int cx; - cx = (data[curr_id].x); - cy = (data[curr_id].y); - - if (!los_check(src.x, src.y, dst.x, dst.y)) { - - while (pq.size() > 1) { - - data[curr_id].counter++; - - int moves[8][2] = {{cx, cy + 1}, - {cx + 1, cy}, - {cx, cy - 1}, - {cx - 1, cy}, - {cx + 1, cy + 1}, - {cx + 1, cy - 1}, - {cx - 1, cy - 1}, - {cx - 1, cy + 1}}; - - if (how_many_corners != 8) { - how_many_corners = 4; - } +#include +#include "lazy_theta_star_b/lazy_theta_star_b.h" - int mx, my; - id m_id; - id curr_par = (data[curr_id].parentId); - - // The condition if current point is the destination - if (cx == dst.x && cy == dst.y) { - if (!(los_check(cx, cy, data[curr_par].x, data[curr_par].y))) { - float min_dist = INF_COST; - int min_dist_id = curr_par; - - for (int i = 0; i < how_many_corners; i++) { - mx = moves[i][0]; - my = moves[i][1]; - if (withinLimits(mx, my)) { - m_id = getIndex(mx, my); - if (m_id != 0) { - if (data[m_id].f < min_dist) { - min_dist = data[m_id].f; - min_dist_id = m_id; - } - } - } - data[curr_id].parentId = min_dist_id; - data[curr_id].g = data[min_dist_id].g + dist(cx, cy, data[min_dist_id].x, data[min_dist_id].y); - data[curr_id].f = data[curr_id].g + data[curr_id].h; - } - } - break; - } - - //lazy checking for the parent - if (!(los_check(cx, cy, data[curr_par].x, data[curr_par].y)) && isSafe(cx, cy) && - isSafe(data[curr_par].x, data[curr_par].y)) { - - double min_dist = INF_COST; - int min_dist_id = curr_par; - - for (int i = 0; i < how_many_corners; i++) { - mx = moves[i][0]; - my = moves[i][1]; - if (withinLimits(mx, my)) { - m_id = getIndex(mx, my); - if (m_id != 0) { - if (data[m_id].f < min_dist) { - min_dist = data[m_id].f; - min_dist_id = m_id; - } - } - } - data[curr_id].parentId = min_dist_id; - data[curr_id].g = data[min_dist_id].g + dist(cx, cy, data[min_dist_id].x, data[min_dist_id].y); - data[curr_id].f = data[curr_id].g + data[curr_id].h; - } +namespace lazyThetaStarB +{ + +void LazyThetaStarB::makePlan(std::vector & x_path, std::vector & y_path) +{ + RCLCPP_INFO( + node_->get_logger(), "Path Search Begins"); + + x_path.clear(); + y_path.clear(); + id id_gen = 0; + sizeX = costmap_->getSizeInCellsX(); + sizeY = costmap_->getSizeInCellsY(); + posn.clear(); + initializePosn(); + + std::cout << posn[0] << '\n'; + // has been added so that if the robot is stuck at a place with costmap cost higher than 127 + // then only for that run, the planner be able to give a path + if (costmap_->getCost(src.x, src.y) >= lethal_cost && costmap_->getCost(src.x, src.y) <= 253) { + lethal_cost = costmap_->getCost(src.x, src.y); + } + + data.reserve(static_cast( (sizeX) * (sizeY) * 0.01 ) ); + clearRobotCell(src.x, src.y); + + RCLCPP_INFO( + node_->get_logger(), "GOT THE SOURCE AND DESTINATION ------ %i, %i && %i, %i", + src.x, src.y, dst.x, dst.y); + + if (!isSafe(src.x, src.y) || !isSafe(dst.x, dst.y) || !withinLimits(src.x, src.y) || + !withinLimits(dst.x, dst.y)) + { +// std::cout << !isSafe(src.x, src.y) << '\t' << !isSafe(dst.x, dst.y) << '\t' << !withinLimits( +// src.x, src.y) << '\t' << +// !withinLimits(dst.x, dst.y) << '\n'; + RCLCPP_INFO(node_->get_logger(), "NO PATH POSSIBLE!!!!!!"); + } + + data.push_back({(src.x), (src.y), 0, dist(src.x, src.y, dst.x, dst.y), 0, -1, + dist(src.x, src.y, dst.x, dst.y)}); + + addIndex(src.x, src.y, id_gen); + + pq.push_back({0, &(data[0].g), &(data[0].f)}); + pq.push_back({0, &(data[0].g), &(data[0].f)}); + // added as a buffer, since my binaryHeapDelMin requires for the start index to 1 instead of 0 + + // The algorithm begins here + id curr_id = 0; + id_gen++; + + const int moves[8][2] = { + {0, +1}, + {0, -1}, + {-1, 0}, + {+1, 0}, + {+1, -1}, + {-1, -1}, + {-1, +1}, + {+1, +1}, + }; + + int cy; + int cx; + cx = (data[curr_id].x); + cy = (data[curr_id].y); + + if (!losCheck(src.x, src.y, dst.x, dst.y)) { + + while (pq.size() > 1) { + + +// int moves[8][2] = {{cx, cy + 1}, +// {cx + 1, cy}, +// {cx, cy - 1}, +// {cx - 1, cy}, +// {cx + 1, cy + 1}, +// {cx + 1, cy - 1}, +// {cx - 1, cy - 1}, +// {cx - 1, cy + 1}}; + int mx, my; + id m_id; + id curr_par = (data[curr_id].parentId); + std::cout << "THE CURRENT POINT IS : " << cx << '\t' << cy << '\n'; + + // The condition if current point is the destination + if (cx == dst.x && cy == dst.y) { + std::cout<<"Got IT!!!!!!!!!"<<'\n'; + if (!(losCheck(cx, cy, data[curr_par].x, data[curr_par].y))) { + float min_dist = INF_COST; + int min_dist_id = curr_par; + std::cout<<"NEED IT!!!!!!!!!"<<'\n'; + + for (int i = 0; i < how_many_corners; i++) { + std::cout<<"IT!!!!!!!!!"<<'\n'; + mx = cx + moves[i][0]; + my = cy + moves[i][1]; + if (withinLimits(mx, my)) { + m_id = getIndex(mx, my); + if (m_id != 0) { + if (data[m_id].f < min_dist) { + min_dist = data[m_id].f; + min_dist_id = m_id; } - curr_par = data[curr_id].parentId; - - for (int i = 0; i < how_many_corners; i++) { - mx = moves[i][0]; - my = moves[i][1]; - - if (mx == data[curr_par].x && my == data[curr_par].y) - continue; - - if (withinLimits(mx, my)) { - double g_cost = data[curr_par].g + dist(mx, my, data[curr_par].x, data[curr_par].y); - double h_cost, cal_cost; - m_id = getIndex(mx, my); - if (m_id == 0 && isSafe(mx, my)) { - h_cost = dist(mx, my, dst.x, dst.y); - cal_cost = g_cost + h_cost; - data.push_back({int(mx), int(my), g_cost, h_cost, curr_par, -1, cal_cost}); - addIndex(mx, my, id_gen); - push_to_pq(id_gen); - id_gen++; - continue; - } else if (m_id != 0) { - h_cost = data[m_id].h; - cal_cost = g_cost + h_cost; - if (data[m_id].f > cal_cost) { - data[m_id].g = g_cost; - data[m_id].f = cal_cost; - data[m_id].parentId = curr_par; - if (data[m_id].closed == 1) { - data[m_id].closed = -1; - push_to_pq(m_id); - } - } - continue; - } - } - } - - last_id = curr_id; - do { - curr_id = pq[1].pos_id; - cx = data[curr_id].x; - cy = data[curr_id].y; - } while (!isSafe(cx, cy)); - binary_heap_del_min(); - data[curr_id].closed = -1; - + } } - - if(pq.size() == 1){ - RCLCPP_INFO(node_->get_logger(), "NO PATH POSSIBLE"); - } - - } else { - RCLCPP_INFO(node_->get_logger(), "STRAIGHT LINE PATH"); - data.push_back({(dst.x), (dst.y), dist(cx, cy, src.x, src.y), 0, 0}); - curr_id = 1; + data[curr_id].parentId = min_dist_id; + data[curr_id].g = data[min_dist_id].g + + dist(cx, cy, data[min_dist_id].x, data[min_dist_id].y); + data[curr_id].f = data[curr_id].g + data[curr_id].h; + } } - - RCLCPP_INFO(node_->get_logger(), "REACHED DEST %i, %i", dst.x, dst.y); - - backtrace(&path, curr_id); - - cout << data.size() << '\n'; - - cout << "Publishing the Path" << '\n'; - - //reset all the values - global_path = linearInterpolation(path, interpolation_dist); - - geometry_msgs::msg::PoseStamped p_end; - p_end.pose.position.x = goal.pose.position.x; - p_end.pose.position.y = goal.pose.position.y; - p_end.header.stamp = node_->now(); - p_end.header.frame_id = global_frame_; - global_path.poses.push_back(p_end); - - return global_path; - } - - void LazyThetaStarB::initializePosn() { - if (pq.size() != abs(sizeX * sizeY)) { - for (int i = 0; i < sizeY * sizeX; i++) { - posn.push_back(0); - } - } else { - for (int i = 0; i < sizeY * sizeX; i++) { - posn[i] = 0; + break; + } + + if (!(losCheck(cx, cy, data[curr_par].x, data[curr_par].y))) { + float min_dist = INF_COST; + int min_dist_id = curr_par; + + for (int i = 0; i < how_many_corners; i++) { + mx = cx + moves[i][0]; + my = cy + moves[i][1]; + if (withinLimits(mx, my)) { + m_id = getIndex(mx, my); + if (m_id != 0) { + if (data[m_id].f < min_dist) { + min_dist = data[m_id].f; + min_dist_id = m_id; + } } + } + data[curr_id].parentId = min_dist_id; + data[curr_id].g = data[min_dist_id].g + dist(cx, cy, data[min_dist_id].x, + data[min_dist_id].y); + data[curr_id].f = data[curr_id].g + data[curr_id].h; } - } - - void LazyThetaStarB::addIndex(int cx, int cy, id index) { - posn[sizeX * cy + cx] = index; - } - - id LazyThetaStarB::getIndex(int cx, int cy) { - return posn[sizeX * cy + cx]; - } + } + curr_par = data[curr_id].parentId; - void LazyThetaStarB::backtrace(vector *waypt, id curr_id) { - vector waypt_rev; - do { - waypt_rev.push_back(curr_id); - curr_id = data[curr_id].parentId; - } while (curr_id != 0); - waypt_rev.push_back(0); + for (int i = 0; i < how_many_corners; i++) { + mx = cx + moves[i][0]; + my = cy + moves[i][1]; - for (int i = waypt_rev.size() - 1; i >= 0; i--) { - waypt->push_back(waypt_rev[i]); + if (mx == data[curr_par].x && my == data[curr_par].y) { + continue; } - } - - void LazyThetaStarB::binary_heap_del_min() { - int hole = 1; - int succ = 2, size = pq.size() - 1, sz = size; - //the main bottom up part where it compares and assigns the smallest value to the hole - //remember that for this part the heap starts at index 1 - while (succ < sz) { - double k1 = *(pq[succ].f); - double k2 = *(pq[succ + 1].f); - if (k1 > k2) { - succ++; - pq[hole] = pq[succ]; - } else { - pq[hole] = pq[succ]; + if (withinLimits(mx, my)) { + float g_cost = data[curr_par].g + dist(mx, my, data[curr_par].x, data[curr_par].y); + float h_cost, cal_cost; + m_id = getIndex(mx, my); + if (m_id == 0 && isSafe(mx, my)) { + h_cost = dist(mx, my, dst.x, dst.y); + cal_cost = g_cost + h_cost; + data.push_back({mx, my, g_cost, h_cost, curr_par, -1, cal_cost}); + addIndex(mx, my, id_gen); + pushToPq(id_gen); + id_gen++; + continue; + } else if (m_id != 0) { + h_cost = data[m_id].h; + cal_cost = g_cost + h_cost; + if (data[m_id].f > cal_cost) { + data[m_id].g = g_cost; + data[m_id].f = cal_cost; + data[m_id].parentId = curr_par; + if (data[m_id].closed == 1) { + data[m_id].closed = -1; + pushToPq(m_id); + } } - hole = succ; - succ <<= 1; - } - //this part checks if the value to be used to fill the last row's hole is small or not - //if not small then it slides the small value up till it reaches the apt position - double bubble = *(pq[sz].f); - int pred = hole >> 1; - while (*(pq[pred].f) > bubble) { - pq[hole] = pq[pred]; - hole = pred; - pred >>= 1; + continue; + } } - //this part simply assigns the last value in the heap to the hole after checking it - //for maintaining the heap data structure - pq[hole] = pq[sz]; - pq.pop_back(); + } + + do { + curr_id = pq[1].pos_id; + cx = data[curr_id].x; + cy = data[curr_id].y; + } while (!isSafe(cx, cy)); + binaryHeapDelMin(); + data[curr_id].closed = -1; } - bool LazyThetaStarB::los_check(int x0, int y0, int x1, int y1) { + } else { + RCLCPP_INFO(node_->get_logger(), "Straight Path"); + data.push_back({(dst.x), (dst.y), dist(cx, cy, src.x, src.y), 0, 0}); + curr_id = 1; + } - int dy = y1 - y0, dx = x1 - x0, f = 0; - int sx, sy; + RCLCPP_INFO(node_->get_logger(), "REACHED DEST %i, %i", dst.x, dst.y); - if (dy < 0) { - dy = -dy; - sy = -1; - } else { - sy = 1; - } + backtrace(&path, curr_id); - if (dx < 0) { - dx = -dx; - sx = -1; - } else { - sx = 1; - } + for (auto & p : path) { + pts map_coords; + costmap_->mapToWorld(data[p].x, data[p].y, map_coords.x, map_coords.y); + RCLCPP_INFO(node_->get_logger(), "%f, %f", map_coords.x, map_coords.y); + x_path.push_back(map_coords.x); + y_path.push_back(map_coords.y); + } + std::cout << data.size() << '\n'; - if (dx >= dy) { - while (x0 != x1) { - f += dy; - if (f >= dx) { - if (!isSafe(x0 + (sx - 1) / 2, y0 + (sy - 1) / 2)) - return false; - y0 += sy; - f -= dx; - } - if (f != 0 && !isSafe(x0 + (sx - 1) / 2, y0 + (sy - 1) / 2)) - return false; - if (dy == 0 && !isSafe(x0 + (sx - 1) / 2, y0) && !isSafe(x0 + (sx - 1) / 2, y0 - 1)) - return false; - x0 += sx; - } - } else { - while (y0 != y1) { - f = f + dx; - if (f >= dy) { - if (!isSafe(x0 + (sx - 1) / 2, y0 + (sy - 1) / 2)) - return false; - x0 += sx; - f -= dy; - } - if (f != 0 && !isSafe(x0 + (sx - 1) / 2, y0 + (sy - 1) / 2)) - return false; - if (dx == 0 && !isSafe(x0, y0 + (sy - 1) / 2) && !isSafe(x0 - 1, y0 + (sy - 1) / 2)) - return false; - y0 += sy; - } - } - - return true; + path.clear(); + data.clear(); - } - - void LazyThetaStarB::push_to_pq(id id_this) { - pq.push_back({id_this, &(data[id_this].g), &(data[id_this].f)}); - push_heap(pq.begin() + 1, pq.end(), comp); - } + pq.clear(); + lethal_cost = LETHAL_COST; + std::cout << "Publishing the Path" << '\n'; +} - bool LazyThetaStarB::isSafe(int cx, int cy) { - return (costmap_->getCost(cx, cy) < lethal_cost); +void LazyThetaStarB::binaryHeapDelMin() +{ + int succ = 2; + int size = pq.size() - 1; + int sz = size; + int hole = 1; + // the main bottom up part where it compares and assigns the smallest value to the hole + // remember that for this part the heap starts at index 1 + while (succ < sz) { + double k1 = *(pq[succ].f); + double k2 = *(pq[succ + 1].f); + if (k1 > k2) { + succ++; + pq[hole] = pq[succ]; + } else { + pq[hole] = pq[succ]; } + hole = succ; + succ <<= 1; + } + // this part checks if the value to be used to fill the last row's hole is small or not + // if not small then it slides the small value up till it reaches the apt position + double bubble = *(pq[sz].f); + int pred = hole >> 1; + while (*(pq[pred].f) > bubble) { + pq[hole] = pq[pred]; + hole = pred; + pred >>= 1; + } + // this part simply assigns the last value in the heap to the hole after checking it + // for maintaining the heap data structure + pq[hole] = pq[sz]; + pq.pop_back(); +} - bool LazyThetaStarB::withinLimits(int x, int y) { - return (x > 0 && x <= sizeX && y > 0 && y <= sizeY); +// TODO(Anshhu-man567): FIND A WAY TO PASS THE VALUES BY REFERENCE WHEN CHECKING +bool LazyThetaStarB::losCheck(int x0, int y0, int x1, int y1) +{ + int dy = y1 - y0, dx = x1 - x0, f = 0; + int sx, sy; + + if (dy < 0) { + dy = -dy; + sy = -1; + } else { + sy = 1; + } + + if (dx < 0) { + dx = -dx; + sx = -1; + } else { + sx = 1; + } + + if (dx >= dy) { + while (x0 != x1) { + f += dy; + if (f >= dx) { + if (!isSafe2(x0 + (sx - 1) / 2, y0 + (sy - 1) / 2)) { + return false; + } + y0 += sy; + f -= dx; + } + if (f != 0 && !isSafe2(x0 + (sx - 1) / 2, y0 + (sy - 1) / 2)) { + return false; + } + if (dy == 0 && !isSafe2(x0 + (sx - 1) / 2, y0) && !isSafe2(x0 + (sx - 1) / 2, y0 - 1)) { + return false; + } + x0 += sx; } - - pts LazyThetaStarB::tf_map_to_world(int x, int y) { - pts pt{}; - pt.x = double( - costmap_->getOriginX() + (x + 0.5) * costmap_->getResolution()); - pt.y = double( - costmap_->getOriginY() + (y + 0.5) * costmap_->getResolution()); - // 0.5 added to display the points at the centre of the grid cell - return pt; + } else { + while (y0 != y1) { + f = f + dx; + if (f >= dy) { + if (!isSafe2(x0 + (sx - 1) / 2, y0 + (sy - 1) / 2)) { + return false; + } + x0 += sx; + f -= dy; + } + if (f != 0 && !isSafe2(x0 + (sx - 1) / 2, y0 + (sy - 1) / 2)) { + return false; + } + if (dx == 0 && !isSafe2(x0, y0 + (sy - 1) / 2) && !isSafe2(x0 - 1, y0 + (sy - 1) / 2)) { + return false; + } + y0 += sy; } + } + return true; +} - nav_msgs::msg::Path LazyThetaStarB::linearInterpolation(vector lp, double dist_bw_points) { - - nav_msgs::msg::Path pa; - pa.header.frame_id = "map"; - for (int i = 0; i < int(lp.size() - 1); i++) { - geometry_msgs::msg::PoseStamped p; - pts pt_copy = tf_map_to_world(data[lp[i]].x, data[lp[i]].y); - - - p.pose.position.x = pt_copy.x; - p.pose.position.y = pt_copy.y; - p.header.stamp = node_->now(); - p.header.frame_id = global_frame_; - pa.poses.push_back(p); - - pts pt2_copy = tf_map_to_world(data[lp[i + 1]].x, data[lp[i + 1]].y); - - double distance = double(dist(pt2_copy.x, pt2_copy.y, pt_copy.x, pt_copy.y)); - int loops = int(distance / dist_bw_points); - double sin_alpha = (pt2_copy.y - pt_copy.y) / distance; - double cos_alpha = (pt2_copy.x - pt_copy.x) / distance; - for (int j = 1; j < loops; j++) { - geometry_msgs::msg::PoseStamped p1; - p1.pose.position.x = pt_copy.x + j * dist_bw_points * cos_alpha; - p1.pose.position.y = pt_copy.y + j * dist_bw_points * sin_alpha; - p1.header.stamp = node_->now(); - p1.header.frame_id = global_frame_; - pa.poses.push_back(p1); - } - geometry_msgs::msg::PoseStamped p2; - p2.pose.position.x = pt2_copy.x; - p2.pose.position.y = pt2_copy.y; - p2.header.stamp = node_->now(); - p2.header.frame_id = global_frame_; - pa.poses.push_back(p2); - - } +void LazyThetaStarB::initializePosn() +{ + for (int i = 0; i < sizeY * sizeX; i++) { + posn.push_back(0); + } +} +void LazyThetaStarB::addIndex(const int & cx, const int & cy, const id & index) +{ + posn[sizeX * cy + cx] = index; +} - return pa; +id LazyThetaStarB::getIndex(const int & cx, const int & cy) +{ + return posn[sizeX * cy + cx]; +} +void LazyThetaStarB::backtrace(std::vector * waypt, id curr_id) +{ + std::vector wayptRev; + do { + wayptRev.push_back(curr_id); + curr_id = data[curr_id].parentId; + if (data[curr_id].got_here == 1) { + std::cout + << "------------------------------------------------------> got the same error <-------------------------------------------------------" + << '\n'; + break; } + data[curr_id].got_here = 1; + } while (curr_id != 0); + wayptRev.push_back(0); + for (int i = wayptRev.size() - 1; i >= 0; i--) { + waypt->push_back(wayptRev[i]); + } +} - void LazyThetaStarB::clearRobotCell(int mx, int my) { - costmap_->setCost(mx, my, nav2_costmap_2d::FREE_SPACE); - } +void LazyThetaStarB::pushToPq(id & idThis) +{ + pq.push_back({idThis, &(data[idThis].g), &(data[idThis].f)}); + push_heap(pq.begin() + 1, pq.end(), comp); +} +bool LazyThetaStarB::isSafe(const int & cx, const int & cy) +{ + return costmap_->getCost(cx, cy) < lethal_cost; +} - //Code to preferably not to look at +bool LazyThetaStarB::isSafe2(int cx, int cy) +{ + return costmap_->getCost(cx, cy) < lethal_cost; +} - //this finds the shortest distance between the points of the raw path given by the planner -// nav_msgs::msg::Path LazyThetaStarB::AutoLinearInterpolation(vector lp) { -// -// nav_msgs::msg::Path the_path; -// if (lp.size() > 2) { -// double min_dist = INF_COST; -// -// for (int i = 0; i < int(lp.size() - 1); i++) { -// pts pt_copy = tf_map_to_world(data[lp[i]].x, data[lp[i]].y); -// pts pt2_copy = tf_map_to_world(data[lp[i + 1]].x, data[lp[i + 1]].y) -// double dist_curr = dist(pt_copy.x, pt_copy.y, pt2_copy.x, pt2_copy.y); -// -// if (dist_curr < min_dist) -// min_dist = dist_curr; -// } -// -// the_path = linearInterpolation(lp, min_dist); -// -// } else { -// the_path = linearInterpolation(lp, 0.01); -// } -// RCLCPP_INFO(node_->get_logger(), "SENT BACK THE PATH!!!!!!"); -// return the_path; -// } +bool LazyThetaStarB::withinLimits(const int & x, const int & y) +{ + return x > 0 && x <= sizeX && y > 0 && y <= sizeY; } -#include "pluginlib/class_list_macros.hpp" +void LazyThetaStarB::clearRobotCell(int mx, int my) +{ + costmap_->setCost(mx, my, nav2_costmap_2d::FREE_SPACE); +} -PLUGINLIB_EXPORT_CLASS(lazy_theta_star_b::LazyThetaStarB, nav2_core::GlobalPlanner) +} // namespace lazyThetaStarB diff --git a/lazy_theta_star_b/src/planner.cpp b/lazy_theta_star_b/src/planner.cpp new file mode 100644 index 0000000000..3a14b9c160 --- /dev/null +++ b/lazy_theta_star_b/src/planner.cpp @@ -0,0 +1,145 @@ +// Copyright 2020 Anshumaan Singh +// +// 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. + +#include +#include +#include +#include "lazy_theta_star_b/planner.h" + +namespace planner +{ +void planner::configure( + rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmapRos) +{ + planner_ = std::make_unique(); + node_ = parent; + name_ = name; + tf_ = tf; + planner_->costmap_ = costmapRos->getCostmap(); + costmap_ = costmapRos->getCostmap(); + globalFrame_ = costmapRos->getGlobalFrameID(); + + nav2_util::declare_parameter_if_not_declared( + node_, name_ + ".howManyCorners", rclcpp::ParameterValue( + 8)); + + node_->get_parameter(name_ + ".howManyCorners", how_many_corners_); + + if (how_many_corners_ != 8) { + how_many_corners_ = 4; + } + + nav2_util::declare_parameter_if_not_declared( + node_, name_ + ".interpolation_dist_", rclcpp::ParameterValue( + 0.1)); + + node_->get_parameter(name_ + ".interpolation_dist_", interpolation_dist_); +} + +void planner::cleanup() +{ + RCLCPP_INFO( + node_->get_logger(), "CleaningUp plugin %s of type planner", + name_.c_str()); +} + +void planner::activate() +{ + RCLCPP_INFO( + node_->get_logger(), "Activating plugin %s of type planner", + name_.c_str()); +} + +void planner::deactivate() +{ + RCLCPP_INFO( + node_->get_logger(), "Deactivating plugin %s of type planner", + name_.c_str()); +} + +nav_msgs::msg::Path planner::createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) +{ + nav_msgs::msg::Path global_path; + + RCLCPP_INFO(node_->get_logger(), "STARTED!!!"); + + planner_->costmap_->worldToMap(start.pose.position.x, start.pose.position.y, + reinterpret_cast(planner_->src.x), + reinterpret_cast(planner_->src.y)); + + planner_->costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, + reinterpret_cast(planner_->dst.x), + reinterpret_cast(planner_->dst.y)); + + planner_->node_ = node_; +// planner_->costmap_ = costmap_; + planner_->how_many_corners = how_many_corners_; + RCLCPP_INFO(node_->get_logger(), "STOPPED APPARENTLY!!!"); + planner_->makePlan(x, y); + RCLCPP_INFO(node_->get_logger(), "NOT REALLY THOUGH!!!"); + + global_path = linearInterpolation(interpolation_dist_); + x.clear(); + y.clear(); + geometry_msgs::msg::PoseStamped pEnd; + pEnd.pose.position.x = goal.pose.position.x; + pEnd.pose.position.y = goal.pose.position.y; + global_path.poses.push_back(pEnd); + + global_path.header.stamp = node_->now(); + global_path.header.frame_id = globalFrame_; + + return global_path; +} + +nav_msgs::msg::Path planner::linearInterpolation(double distBwPoints) +{ + nav_msgs::msg::Path pa; + pa.header.frame_id = "map"; + for (int i = 0; i < static_cast(x.size() - 1); i++) { + geometry_msgs::msg::PoseStamped p; + + pts ptCopy = {x[i], y[i]}; + p.pose.position.x = ptCopy.x; + p.pose.position.y = ptCopy.y; + pa.poses.push_back(p); + + pts pt2Copy = {x[i + 1], y[i + 1]}; + + double distance = std::hypot(pt2Copy.x - ptCopy.x, pt2Copy.y - ptCopy.y); + int loops = static_cast(distance / distBwPoints); + double sinAlpha = (pt2Copy.y - ptCopy.y) / distance; + double cosAlpha = (pt2Copy.x - ptCopy.x) / distance; + + for (int j = 1; j < loops; j++) { + p.pose.position.x = ptCopy.x + j * distBwPoints * cosAlpha; + p.pose.position.y = ptCopy.y + j * distBwPoints * sinAlpha; + pa.poses.push_back(p); + } + + p.pose.position.x = pt2Copy.x; + p.pose.position.y = pt2Copy.y; + pa.poses.push_back(p); + } + + return pa; +} + +} // namespace planner +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(planner::planner, nav2_core::GlobalPlanner) From d4dd2631e99e4754aa07242e0bc61e89dcb1c50f Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Sat, 4 Jul 2020 09:31:26 +0530 Subject: [PATCH 06/81] restructured files --- lazy_theta_star_b/src/lazy_theta_star_b.cpp | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) diff --git a/lazy_theta_star_b/src/lazy_theta_star_b.cpp b/lazy_theta_star_b/src/lazy_theta_star_b.cpp index 7e5aafdb4f..f36860bc65 100644 --- a/lazy_theta_star_b/src/lazy_theta_star_b.cpp +++ b/lazy_theta_star_b/src/lazy_theta_star_b.cpp @@ -86,22 +86,13 @@ void LazyThetaStarB::makePlan(std::vector & x_path, std::vector & if (!losCheck(src.x, src.y, dst.x, dst.y)) { while (pq.size() > 1) { - - -// int moves[8][2] = {{cx, cy + 1}, -// {cx + 1, cy}, -// {cx, cy - 1}, -// {cx - 1, cy}, -// {cx + 1, cy + 1}, -// {cx + 1, cy - 1}, -// {cx - 1, cy - 1}, -// {cx - 1, cy + 1}}; + int mx, my; id m_id; id curr_par = (data[curr_id].parentId); std::cout << "THE CURRENT POINT IS : " << cx << '\t' << cy << '\n'; - // The condition if current point is the destination + // The condition if current point is the destination if (cx == dst.x && cy == dst.y) { std::cout<<"Got IT!!!!!!!!!"<<'\n'; if (!(losCheck(cx, cy, data[curr_par].x, data[curr_par].y))) { From d6f8b8fc85f7973f9226a6c0a199aee93300fb85 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Sat, 4 Jul 2020 09:47:34 +0530 Subject: [PATCH 07/81] Update CMakeLists.txt --- lazy_theta_star_b/CMakeLists.txt | 15 +-------------- 1 file changed, 1 insertion(+), 14 deletions(-) diff --git a/lazy_theta_star_b/CMakeLists.txt b/lazy_theta_star_b/CMakeLists.txt index dc04e6d324..31d86b63cb 100644 --- a/lazy_theta_star_b/CMakeLists.txt +++ b/lazy_theta_star_b/CMakeLists.txt @@ -1,16 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(lazy_theta_star_b) -set(CMAKE_C_STANDARD 99) -set(CMAKE_CXX_STANDARD 14) - - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -O3) -endif() - # find dependencies find_package(ament_cmake REQUIRED) +find_package(nav2_common REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_lifecycle REQUIRED) @@ -72,12 +65,6 @@ install(FILES global_planner_plugin.xml 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() endif() From a11fbd9965c1d1d5974833a23fde7ce1c2e99b9d Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Sat, 4 Jul 2020 09:49:42 +0530 Subject: [PATCH 08/81] removed unnecessary comments --- lazy_theta_star_b/src/lazy_theta_star_b.cpp | 6 +----- lazy_theta_star_b/src/planner.cpp | 4 ---- 2 files changed, 1 insertion(+), 9 deletions(-) diff --git a/lazy_theta_star_b/src/lazy_theta_star_b.cpp b/lazy_theta_star_b/src/lazy_theta_star_b.cpp index f36860bc65..6dcfc9d61e 100644 --- a/lazy_theta_star_b/src/lazy_theta_star_b.cpp +++ b/lazy_theta_star_b/src/lazy_theta_star_b.cpp @@ -48,9 +48,6 @@ void LazyThetaStarB::makePlan(std::vector & x_path, std::vector & if (!isSafe(src.x, src.y) || !isSafe(dst.x, dst.y) || !withinLimits(src.x, src.y) || !withinLimits(dst.x, dst.y)) { -// std::cout << !isSafe(src.x, src.y) << '\t' << !isSafe(dst.x, dst.y) << '\t' << !withinLimits( -// src.x, src.y) << '\t' << -// !withinLimits(dst.x, dst.y) << '\n'; RCLCPP_INFO(node_->get_logger(), "NO PATH POSSIBLE!!!!!!"); } @@ -86,13 +83,12 @@ void LazyThetaStarB::makePlan(std::vector & x_path, std::vector & if (!losCheck(src.x, src.y, dst.x, dst.y)) { while (pq.size() > 1) { - int mx, my; id m_id; id curr_par = (data[curr_id].parentId); std::cout << "THE CURRENT POINT IS : " << cx << '\t' << cy << '\n'; - // The condition if current point is the destination + // The condition if current point is the destination if (cx == dst.x && cy == dst.y) { std::cout<<"Got IT!!!!!!!!!"<<'\n'; if (!(losCheck(cx, cy, data[curr_par].x, data[curr_par].y))) { diff --git a/lazy_theta_star_b/src/planner.cpp b/lazy_theta_star_b/src/planner.cpp index 3a14b9c160..96dd0ef041 100644 --- a/lazy_theta_star_b/src/planner.cpp +++ b/lazy_theta_star_b/src/planner.cpp @@ -76,8 +76,6 @@ nav_msgs::msg::Path planner::createPlan( { nav_msgs::msg::Path global_path; - RCLCPP_INFO(node_->get_logger(), "STARTED!!!"); - planner_->costmap_->worldToMap(start.pose.position.x, start.pose.position.y, reinterpret_cast(planner_->src.x), reinterpret_cast(planner_->src.y)); @@ -89,9 +87,7 @@ nav_msgs::msg::Path planner::createPlan( planner_->node_ = node_; // planner_->costmap_ = costmap_; planner_->how_many_corners = how_many_corners_; - RCLCPP_INFO(node_->get_logger(), "STOPPED APPARENTLY!!!"); planner_->makePlan(x, y); - RCLCPP_INFO(node_->get_logger(), "NOT REALLY THOUGH!!!"); global_path = linearInterpolation(interpolation_dist_); x.clear(); From d6f9584b70f40221bc152d8e4cedc23823138f6b Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Sat, 4 Jul 2020 09:59:56 +0530 Subject: [PATCH 09/81] removed comments --- lazy_theta_star_b/src/lazy_theta_star_b.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/lazy_theta_star_b/src/lazy_theta_star_b.cpp b/lazy_theta_star_b/src/lazy_theta_star_b.cpp index 6dcfc9d61e..6dfb710be3 100644 --- a/lazy_theta_star_b/src/lazy_theta_star_b.cpp +++ b/lazy_theta_star_b/src/lazy_theta_star_b.cpp @@ -333,13 +333,6 @@ void LazyThetaStarB::backtrace(std::vector * waypt, id curr_id) do { wayptRev.push_back(curr_id); curr_id = data[curr_id].parentId; - if (data[curr_id].got_here == 1) { - std::cout - << "------------------------------------------------------> got the same error <-------------------------------------------------------" - << '\n'; - break; - } - data[curr_id].got_here = 1; } while (curr_id != 0); wayptRev.push_back(0); From ca4ac17f7161a6472e5c364bdc78c035746a850c Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Wed, 15 Jul 2020 18:50:30 +0530 Subject: [PATCH 10/81] Delete planner.cpp --- lazy_theta_star_b/src/planner.cpp | 141 ------------------------------ 1 file changed, 141 deletions(-) delete mode 100644 lazy_theta_star_b/src/planner.cpp diff --git a/lazy_theta_star_b/src/planner.cpp b/lazy_theta_star_b/src/planner.cpp deleted file mode 100644 index 96dd0ef041..0000000000 --- a/lazy_theta_star_b/src/planner.cpp +++ /dev/null @@ -1,141 +0,0 @@ -// Copyright 2020 Anshumaan Singh -// -// 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. - -#include -#include -#include -#include "lazy_theta_star_b/planner.h" - -namespace planner -{ -void planner::configure( - rclcpp_lifecycle::LifecycleNode::SharedPtr parent, - std::string name, std::shared_ptr tf, - std::shared_ptr costmapRos) -{ - planner_ = std::make_unique(); - node_ = parent; - name_ = name; - tf_ = tf; - planner_->costmap_ = costmapRos->getCostmap(); - costmap_ = costmapRos->getCostmap(); - globalFrame_ = costmapRos->getGlobalFrameID(); - - nav2_util::declare_parameter_if_not_declared( - node_, name_ + ".howManyCorners", rclcpp::ParameterValue( - 8)); - - node_->get_parameter(name_ + ".howManyCorners", how_many_corners_); - - if (how_many_corners_ != 8) { - how_many_corners_ = 4; - } - - nav2_util::declare_parameter_if_not_declared( - node_, name_ + ".interpolation_dist_", rclcpp::ParameterValue( - 0.1)); - - node_->get_parameter(name_ + ".interpolation_dist_", interpolation_dist_); -} - -void planner::cleanup() -{ - RCLCPP_INFO( - node_->get_logger(), "CleaningUp plugin %s of type planner", - name_.c_str()); -} - -void planner::activate() -{ - RCLCPP_INFO( - node_->get_logger(), "Activating plugin %s of type planner", - name_.c_str()); -} - -void planner::deactivate() -{ - RCLCPP_INFO( - node_->get_logger(), "Deactivating plugin %s of type planner", - name_.c_str()); -} - -nav_msgs::msg::Path planner::createPlan( - const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) -{ - nav_msgs::msg::Path global_path; - - planner_->costmap_->worldToMap(start.pose.position.x, start.pose.position.y, - reinterpret_cast(planner_->src.x), - reinterpret_cast(planner_->src.y)); - - planner_->costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, - reinterpret_cast(planner_->dst.x), - reinterpret_cast(planner_->dst.y)); - - planner_->node_ = node_; -// planner_->costmap_ = costmap_; - planner_->how_many_corners = how_many_corners_; - planner_->makePlan(x, y); - - global_path = linearInterpolation(interpolation_dist_); - x.clear(); - y.clear(); - geometry_msgs::msg::PoseStamped pEnd; - pEnd.pose.position.x = goal.pose.position.x; - pEnd.pose.position.y = goal.pose.position.y; - global_path.poses.push_back(pEnd); - - global_path.header.stamp = node_->now(); - global_path.header.frame_id = globalFrame_; - - return global_path; -} - -nav_msgs::msg::Path planner::linearInterpolation(double distBwPoints) -{ - nav_msgs::msg::Path pa; - pa.header.frame_id = "map"; - for (int i = 0; i < static_cast(x.size() - 1); i++) { - geometry_msgs::msg::PoseStamped p; - - pts ptCopy = {x[i], y[i]}; - p.pose.position.x = ptCopy.x; - p.pose.position.y = ptCopy.y; - pa.poses.push_back(p); - - pts pt2Copy = {x[i + 1], y[i + 1]}; - - double distance = std::hypot(pt2Copy.x - ptCopy.x, pt2Copy.y - ptCopy.y); - int loops = static_cast(distance / distBwPoints); - double sinAlpha = (pt2Copy.y - ptCopy.y) / distance; - double cosAlpha = (pt2Copy.x - ptCopy.x) / distance; - - for (int j = 1; j < loops; j++) { - p.pose.position.x = ptCopy.x + j * distBwPoints * cosAlpha; - p.pose.position.y = ptCopy.y + j * distBwPoints * sinAlpha; - pa.poses.push_back(p); - } - - p.pose.position.x = pt2Copy.x; - p.pose.position.y = pt2Copy.y; - pa.poses.push_back(p); - } - - return pa; -} - -} // namespace planner -#include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(planner::planner, nav2_core::GlobalPlanner) From dd1563c68051b73eeba201cb83b5ec41e42ab4d4 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Wed, 15 Jul 2020 18:50:40 +0530 Subject: [PATCH 11/81] Delete lazy_theta_star_b.cpp --- lazy_theta_star_b/src/lazy_theta_star_b.cpp | 370 -------------------- 1 file changed, 370 deletions(-) delete mode 100644 lazy_theta_star_b/src/lazy_theta_star_b.cpp diff --git a/lazy_theta_star_b/src/lazy_theta_star_b.cpp b/lazy_theta_star_b/src/lazy_theta_star_b.cpp deleted file mode 100644 index 6dfb710be3..0000000000 --- a/lazy_theta_star_b/src/lazy_theta_star_b.cpp +++ /dev/null @@ -1,370 +0,0 @@ -// Copyright 2020 Anshumaan Singh -// -// 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. - -#include -#include "lazy_theta_star_b/lazy_theta_star_b.h" - -namespace lazyThetaStarB -{ - -void LazyThetaStarB::makePlan(std::vector & x_path, std::vector & y_path) -{ - RCLCPP_INFO( - node_->get_logger(), "Path Search Begins"); - - x_path.clear(); - y_path.clear(); - id id_gen = 0; - sizeX = costmap_->getSizeInCellsX(); - sizeY = costmap_->getSizeInCellsY(); - posn.clear(); - initializePosn(); - - std::cout << posn[0] << '\n'; - // has been added so that if the robot is stuck at a place with costmap cost higher than 127 - // then only for that run, the planner be able to give a path - if (costmap_->getCost(src.x, src.y) >= lethal_cost && costmap_->getCost(src.x, src.y) <= 253) { - lethal_cost = costmap_->getCost(src.x, src.y); - } - - data.reserve(static_cast( (sizeX) * (sizeY) * 0.01 ) ); - clearRobotCell(src.x, src.y); - - RCLCPP_INFO( - node_->get_logger(), "GOT THE SOURCE AND DESTINATION ------ %i, %i && %i, %i", - src.x, src.y, dst.x, dst.y); - - if (!isSafe(src.x, src.y) || !isSafe(dst.x, dst.y) || !withinLimits(src.x, src.y) || - !withinLimits(dst.x, dst.y)) - { - RCLCPP_INFO(node_->get_logger(), "NO PATH POSSIBLE!!!!!!"); - } - - data.push_back({(src.x), (src.y), 0, dist(src.x, src.y, dst.x, dst.y), 0, -1, - dist(src.x, src.y, dst.x, dst.y)}); - - addIndex(src.x, src.y, id_gen); - - pq.push_back({0, &(data[0].g), &(data[0].f)}); - pq.push_back({0, &(data[0].g), &(data[0].f)}); - // added as a buffer, since my binaryHeapDelMin requires for the start index to 1 instead of 0 - - // The algorithm begins here - id curr_id = 0; - id_gen++; - - const int moves[8][2] = { - {0, +1}, - {0, -1}, - {-1, 0}, - {+1, 0}, - {+1, -1}, - {-1, -1}, - {-1, +1}, - {+1, +1}, - }; - - int cy; - int cx; - cx = (data[curr_id].x); - cy = (data[curr_id].y); - - if (!losCheck(src.x, src.y, dst.x, dst.y)) { - - while (pq.size() > 1) { - int mx, my; - id m_id; - id curr_par = (data[curr_id].parentId); - std::cout << "THE CURRENT POINT IS : " << cx << '\t' << cy << '\n'; - - // The condition if current point is the destination - if (cx == dst.x && cy == dst.y) { - std::cout<<"Got IT!!!!!!!!!"<<'\n'; - if (!(losCheck(cx, cy, data[curr_par].x, data[curr_par].y))) { - float min_dist = INF_COST; - int min_dist_id = curr_par; - std::cout<<"NEED IT!!!!!!!!!"<<'\n'; - - for (int i = 0; i < how_many_corners; i++) { - std::cout<<"IT!!!!!!!!!"<<'\n'; - mx = cx + moves[i][0]; - my = cy + moves[i][1]; - if (withinLimits(mx, my)) { - m_id = getIndex(mx, my); - if (m_id != 0) { - if (data[m_id].f < min_dist) { - min_dist = data[m_id].f; - min_dist_id = m_id; - } - } - } - data[curr_id].parentId = min_dist_id; - data[curr_id].g = data[min_dist_id].g + - dist(cx, cy, data[min_dist_id].x, data[min_dist_id].y); - data[curr_id].f = data[curr_id].g + data[curr_id].h; - } - } - break; - } - - if (!(losCheck(cx, cy, data[curr_par].x, data[curr_par].y))) { - float min_dist = INF_COST; - int min_dist_id = curr_par; - - for (int i = 0; i < how_many_corners; i++) { - mx = cx + moves[i][0]; - my = cy + moves[i][1]; - if (withinLimits(mx, my)) { - m_id = getIndex(mx, my); - if (m_id != 0) { - if (data[m_id].f < min_dist) { - min_dist = data[m_id].f; - min_dist_id = m_id; - } - } - } - data[curr_id].parentId = min_dist_id; - data[curr_id].g = data[min_dist_id].g + dist(cx, cy, data[min_dist_id].x, - data[min_dist_id].y); - data[curr_id].f = data[curr_id].g + data[curr_id].h; - } - } - curr_par = data[curr_id].parentId; - - for (int i = 0; i < how_many_corners; i++) { - mx = cx + moves[i][0]; - my = cy + moves[i][1]; - - if (mx == data[curr_par].x && my == data[curr_par].y) { - continue; - } - - if (withinLimits(mx, my)) { - float g_cost = data[curr_par].g + dist(mx, my, data[curr_par].x, data[curr_par].y); - float h_cost, cal_cost; - m_id = getIndex(mx, my); - if (m_id == 0 && isSafe(mx, my)) { - h_cost = dist(mx, my, dst.x, dst.y); - cal_cost = g_cost + h_cost; - data.push_back({mx, my, g_cost, h_cost, curr_par, -1, cal_cost}); - addIndex(mx, my, id_gen); - pushToPq(id_gen); - id_gen++; - continue; - } else if (m_id != 0) { - h_cost = data[m_id].h; - cal_cost = g_cost + h_cost; - if (data[m_id].f > cal_cost) { - data[m_id].g = g_cost; - data[m_id].f = cal_cost; - data[m_id].parentId = curr_par; - if (data[m_id].closed == 1) { - data[m_id].closed = -1; - pushToPq(m_id); - } - } - continue; - } - } - } - - do { - curr_id = pq[1].pos_id; - cx = data[curr_id].x; - cy = data[curr_id].y; - } while (!isSafe(cx, cy)); - binaryHeapDelMin(); - data[curr_id].closed = -1; - } - - } else { - RCLCPP_INFO(node_->get_logger(), "Straight Path"); - data.push_back({(dst.x), (dst.y), dist(cx, cy, src.x, src.y), 0, 0}); - curr_id = 1; - } - - RCLCPP_INFO(node_->get_logger(), "REACHED DEST %i, %i", dst.x, dst.y); - - backtrace(&path, curr_id); - - for (auto & p : path) { - pts map_coords; - costmap_->mapToWorld(data[p].x, data[p].y, map_coords.x, map_coords.y); - RCLCPP_INFO(node_->get_logger(), "%f, %f", map_coords.x, map_coords.y); - x_path.push_back(map_coords.x); - y_path.push_back(map_coords.y); - } - std::cout << data.size() << '\n'; - - path.clear(); - data.clear(); - - pq.clear(); - lethal_cost = LETHAL_COST; - std::cout << "Publishing the Path" << '\n'; -} - -void LazyThetaStarB::binaryHeapDelMin() -{ - int succ = 2; - int size = pq.size() - 1; - int sz = size; - int hole = 1; - // the main bottom up part where it compares and assigns the smallest value to the hole - // remember that for this part the heap starts at index 1 - while (succ < sz) { - double k1 = *(pq[succ].f); - double k2 = *(pq[succ + 1].f); - if (k1 > k2) { - succ++; - pq[hole] = pq[succ]; - } else { - pq[hole] = pq[succ]; - } - hole = succ; - succ <<= 1; - } - // this part checks if the value to be used to fill the last row's hole is small or not - // if not small then it slides the small value up till it reaches the apt position - double bubble = *(pq[sz].f); - int pred = hole >> 1; - while (*(pq[pred].f) > bubble) { - pq[hole] = pq[pred]; - hole = pred; - pred >>= 1; - } - // this part simply assigns the last value in the heap to the hole after checking it - // for maintaining the heap data structure - pq[hole] = pq[sz]; - pq.pop_back(); -} - -// TODO(Anshhu-man567): FIND A WAY TO PASS THE VALUES BY REFERENCE WHEN CHECKING -bool LazyThetaStarB::losCheck(int x0, int y0, int x1, int y1) -{ - int dy = y1 - y0, dx = x1 - x0, f = 0; - int sx, sy; - - if (dy < 0) { - dy = -dy; - sy = -1; - } else { - sy = 1; - } - - if (dx < 0) { - dx = -dx; - sx = -1; - } else { - sx = 1; - } - - if (dx >= dy) { - while (x0 != x1) { - f += dy; - if (f >= dx) { - if (!isSafe2(x0 + (sx - 1) / 2, y0 + (sy - 1) / 2)) { - return false; - } - y0 += sy; - f -= dx; - } - if (f != 0 && !isSafe2(x0 + (sx - 1) / 2, y0 + (sy - 1) / 2)) { - return false; - } - if (dy == 0 && !isSafe2(x0 + (sx - 1) / 2, y0) && !isSafe2(x0 + (sx - 1) / 2, y0 - 1)) { - return false; - } - x0 += sx; - } - } else { - while (y0 != y1) { - f = f + dx; - if (f >= dy) { - if (!isSafe2(x0 + (sx - 1) / 2, y0 + (sy - 1) / 2)) { - return false; - } - x0 += sx; - f -= dy; - } - if (f != 0 && !isSafe2(x0 + (sx - 1) / 2, y0 + (sy - 1) / 2)) { - return false; - } - if (dx == 0 && !isSafe2(x0, y0 + (sy - 1) / 2) && !isSafe2(x0 - 1, y0 + (sy - 1) / 2)) { - return false; - } - y0 += sy; - } - } - return true; -} - -void LazyThetaStarB::initializePosn() -{ - for (int i = 0; i < sizeY * sizeX; i++) { - posn.push_back(0); - } -} - -void LazyThetaStarB::addIndex(const int & cx, const int & cy, const id & index) -{ - posn[sizeX * cy + cx] = index; -} - -id LazyThetaStarB::getIndex(const int & cx, const int & cy) -{ - return posn[sizeX * cy + cx]; -} - -void LazyThetaStarB::backtrace(std::vector * waypt, id curr_id) -{ - std::vector wayptRev; - do { - wayptRev.push_back(curr_id); - curr_id = data[curr_id].parentId; - } while (curr_id != 0); - wayptRev.push_back(0); - - for (int i = wayptRev.size() - 1; i >= 0; i--) { - waypt->push_back(wayptRev[i]); - } -} - -void LazyThetaStarB::pushToPq(id & idThis) -{ - pq.push_back({idThis, &(data[idThis].g), &(data[idThis].f)}); - push_heap(pq.begin() + 1, pq.end(), comp); -} - -bool LazyThetaStarB::isSafe(const int & cx, const int & cy) -{ - return costmap_->getCost(cx, cy) < lethal_cost; -} - -bool LazyThetaStarB::isSafe2(int cx, int cy) -{ - return costmap_->getCost(cx, cy) < lethal_cost; -} - -bool LazyThetaStarB::withinLimits(const int & x, const int & y) -{ - return x > 0 && x <= sizeX && y > 0 && y <= sizeY; -} - -void LazyThetaStarB::clearRobotCell(int mx, int my) -{ - costmap_->setCost(mx, my, nav2_costmap_2d::FREE_SPACE); -} - -} // namespace lazyThetaStarB From e099e083392da53f4f474e65e43e76a3e24d898c Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Wed, 15 Jul 2020 18:50:52 +0530 Subject: [PATCH 12/81] Delete lazy_theta_star_b.h --- .../lazy_theta_star_b/lazy_theta_star_b.h | 122 ------------------ 1 file changed, 122 deletions(-) delete mode 100644 lazy_theta_star_b/include/lazy_theta_star_b/lazy_theta_star_b.h diff --git a/lazy_theta_star_b/include/lazy_theta_star_b/lazy_theta_star_b.h b/lazy_theta_star_b/include/lazy_theta_star_b/lazy_theta_star_b.h deleted file mode 100644 index 9a2641f4d1..0000000000 --- a/lazy_theta_star_b/include/lazy_theta_star_b/lazy_theta_star_b.h +++ /dev/null @@ -1,122 +0,0 @@ -// Copyright 2020 Anshumaan Singh -// -// 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. - -#ifndef LAZY_THETA_STAR_B__LAZY_THETA_STAR_B_H_ -#define LAZY_THETA_STAR_B__LAZY_THETA_STAR_B_H_ - -#include -#include -#include -#include -#include -#include -#include "rclcpp/rclcpp.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "nav2_costmap_2d/cost_values.hpp" - -typedef int id; - -#define INF_COST 10000000.0 -#define LETHAL_COST 127 - -template -struct pts -{ - ptsT x, y; -}; - -struct pos -{ - id pos_id; - double * g; - double * f; -}; - -struct tree_node -{ - int x, y; - double g = INF_COST; - double h = INF_COST; - id parentId; - int closed = 0; // 0 - unexplored, 1 - closed, -1 - open - double f = INF_COST; - int got_here = 0; -}; - -namespace lazyThetaStarB -{ - -class LazyThetaStarB -{ -public: - int lethal_cost = LETHAL_COST; - - nav2_util::LifecycleNode::SharedPtr node_; - nav2_costmap_2d::Costmap2D * costmap_; - - // stores the cell data - std::vector data; - - // stores the raw path given by the planner on the basis of their index in the std::vector data - std::vector path; - - // is the priority queue - std::vector pq; - int sizeX = 0; - int sizeY = 0; - int how_many_corners; - pts src{}, dst{}; - - // The part containing the algorithm - void makePlan(std::vector & x_path, std::vector & y_path); - - // The Line of Sight checking algorithm, takes in the points, and follows the line joining points - bool losCheck(int x0, int y0, int x1, int y1); - - // to pop the minimum value of the queue, it is ever so slightly faster than the stl one, - // based on the implementation by Peter Sanders (2000) - void binaryHeapDelMin(); - - // to compare between values in the priority queue - static bool comp(pos p1, pos p2) - { - return (*(p1.f) != *(p2.f)) ? (*(p1.f) > *(p2.f)) : (*(p1.g) > *(p2.g)); - } - - double dist(int & ax, int & ay, int & bx, int & by) - { - return sqrt(pow(ax - bx, 2) + pow(ay - by, 2)); - } - - bool withinLimits(const int & x, const int & y); - bool isSafe(const int & cx, const int & cy); - void pushToPq(id & idThis); - void clearRobotCell(int mx, int my); - void backtrace(std::vector * waypt, id curr_id); - - // stores the index at which the node data is stored for a particular co-ordinate - std::vector posn; - - // it sets/resets all the values within posn to 0 - void initializePosn(); - - // functions used to maintain indices - void addIndex(const int & cx, const int & cy, const id & index); - id getIndex(const int & cx, const int & cy); - - bool isSafe2(int cx, int cy); -}; - -} // namespace lazyThetaStarB -#endif // LAZY_THETA_STAR_B__LAZY_THETA_STAR_B_H_ From efca9625efc55e27023edfb1f544b30e21b1a686 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Wed, 15 Jul 2020 18:51:01 +0530 Subject: [PATCH 13/81] Delete planner.h --- .../include/lazy_theta_star_b/planner.h | 75 ------------------- 1 file changed, 75 deletions(-) delete mode 100644 lazy_theta_star_b/include/lazy_theta_star_b/planner.h diff --git a/lazy_theta_star_b/include/lazy_theta_star_b/planner.h b/lazy_theta_star_b/include/lazy_theta_star_b/planner.h deleted file mode 100644 index 118c8dbbfa..0000000000 --- a/lazy_theta_star_b/include/lazy_theta_star_b/planner.h +++ /dev/null @@ -1,75 +0,0 @@ -// Copyright 2020 Anshumaan Singh -// -// 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. - -#ifndef LAZY_THETA_STAR_B__PLANNER_H_ -#define LAZY_THETA_STAR_B__PLANNER_H_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include "rclcpp/rclcpp.hpp" -#include "nav2_core/global_planner.hpp" -#include "nav_msgs/msg/path.hpp" -#include "nav2_util/robot_utils.hpp" -#include "nav2_util/lifecycle_node.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "nav2_costmap_2d/cost_values.hpp" -#include "nav2_util/node_utils.hpp" -#include "lazy_theta_star_b/lazy_theta_star_b.h" - -namespace planner -{ - -template -struct pts -{ - ptsT x, y; -}; - -class planner : public nav2_core::GlobalPlanner -{ -public: - std::shared_ptr tf_; - nav2_util::LifecycleNode::SharedPtr node_; - nav2_costmap_2d::Costmap2D * costmap_; - - std::string globalFrame_, name_; - int how_many_corners_; - double interpolation_dist_; - std::vector x, y; - std::unique_ptr planner_; - - void configure( - rclcpp_lifecycle::LifecycleNode::SharedPtr parent, - std::string name, std::shared_ptr tf, - std::shared_ptr costmapRos) override; - - void cleanup() override; - void activate() override; - void deactivate() override; - - nav_msgs::msg::Path createPlan( - const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) override; - - nav_msgs::msg::Path linearInterpolation(double distBwPoints); -}; - -} // namespace planner -#endif // LAZY_THETA_STAR_B__PLANNER_H_ From 28925f86ac468bc2ddf989576964483f639f9f79 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Wed, 15 Jul 2020 18:51:10 +0530 Subject: [PATCH 14/81] Delete CMakeLists.txt --- lazy_theta_star_b/CMakeLists.txt | 75 -------------------------------- 1 file changed, 75 deletions(-) delete mode 100644 lazy_theta_star_b/CMakeLists.txt diff --git a/lazy_theta_star_b/CMakeLists.txt b/lazy_theta_star_b/CMakeLists.txt deleted file mode 100644 index 31d86b63cb..0000000000 --- a/lazy_theta_star_b/CMakeLists.txt +++ /dev/null @@ -1,75 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(lazy_theta_star_b) - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(nav2_common REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(visualization_msgs REQUIRED) -find_package(nav2_util REQUIRED) -find_package(nav2_msgs REQUIRED) -find_package(builtin_interfaces REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(nav2_costmap_2d REQUIRED) -find_package(pluginlib REQUIRED) -find_package(nav2_core REQUIRED) - -include_directories( - include -) - -set(library_name ${PROJECT_NAME}_plugin) - -set(dependencies - rclcpp - rclcpp_action - rclcpp_lifecycle - std_msgs - visualization_msgs - nav2_util - nav2_msgs - builtin_interfaces - tf2_ros - nav2_costmap_2d - nav2_core - pluginlib - ) - -add_library(${library_name} SHARED - src/planner.cpp - src/lazy_theta_star_b.cpp - ) -ament_target_dependencies(${library_name} - ${dependencies} - ) - -target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - -pluginlib_export_plugin_description_file(nav2_core global_planner_plugin.xml) - -install(TARGETS ${library_name} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} - ) - -install(DIRECTORY include/ - DESTINATION include/ - ) - -install(FILES global_planner_plugin.xml - DESTINATION share/${PROJECT_NAME} - ) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - - -ament_export_include_directories(include) -ament_export_libraries(${library_name}) -ament_export_dependencies(${dependencies}) -ament_package() From c35ea2bfc0359bd78c39d364e1e5e0f566588789 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Wed, 15 Jul 2020 18:51:21 +0530 Subject: [PATCH 15/81] Delete global_planner_plugin.xml --- lazy_theta_star_b/global_planner_plugin.xml | 5 ----- 1 file changed, 5 deletions(-) delete mode 100644 lazy_theta_star_b/global_planner_plugin.xml diff --git a/lazy_theta_star_b/global_planner_plugin.xml b/lazy_theta_star_b/global_planner_plugin.xml deleted file mode 100644 index 5eb448e56b..0000000000 --- a/lazy_theta_star_b/global_planner_plugin.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - The implementation of lazy theta_star - - \ No newline at end of file From d7f50d081641fa0a545d7fdb5184c57ee0e6c026 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Wed, 15 Jul 2020 18:51:28 +0530 Subject: [PATCH 16/81] Delete package.xml --- lazy_theta_star_b/package.xml | 32 -------------------------------- 1 file changed, 32 deletions(-) delete mode 100644 lazy_theta_star_b/package.xml diff --git a/lazy_theta_star_b/package.xml b/lazy_theta_star_b/package.xml deleted file mode 100644 index fa361b2860..0000000000 --- a/lazy_theta_star_b/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - lazy_theta_star_b - 0.4.0 - TODO: Package description - anshu-man - Apache-2.0 - - ament_cmake - rclcpp - nav2_core - rclcpp_action - rclcpp_lifecycle - std_msgs - visualization_msgs - nav2_util - nav2_msgs - builtin_interfaces - tf2_ros - nav2_costmap_2d - pluginlib - - ament_lint_auto - ament_lint_common - ament_cmake_gtest - - - ament_cmake - - - From 99efed3524ecf493c7b58e0e8d416cc5efc3d28d Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Wed, 15 Jul 2020 18:57:05 +0530 Subject: [PATCH 17/81] replaced the files - refactored code - improved reliability - have to write a code similar to that in nav2_system_tests, to test it (working on it) --- lazy_theta_star_planner/CMakeLists.txt | 77 ++++ .../global_planner_plugin.xml | 5 + .../lazy_theta_star2.h | 144 +++++++ .../lazy_theta_star_planner.h | 77 ++++ lazy_theta_star_planner/package.xml | 31 ++ .../src/lazy_theta_star2.cpp | 364 ++++++++++++++++++ .../src/lazy_theta_star_planner.cpp | 151 ++++++++ 7 files changed, 849 insertions(+) create mode 100644 lazy_theta_star_planner/CMakeLists.txt create mode 100644 lazy_theta_star_planner/global_planner_plugin.xml create mode 100644 lazy_theta_star_planner/include/lazy_theta_star_planner/lazy_theta_star2.h create mode 100644 lazy_theta_star_planner/include/lazy_theta_star_planner/lazy_theta_star_planner.h create mode 100644 lazy_theta_star_planner/package.xml create mode 100644 lazy_theta_star_planner/src/lazy_theta_star2.cpp create mode 100644 lazy_theta_star_planner/src/lazy_theta_star_planner.cpp diff --git a/lazy_theta_star_planner/CMakeLists.txt b/lazy_theta_star_planner/CMakeLists.txt new file mode 100644 index 0000000000..3e77dd3e6f --- /dev/null +++ b/lazy_theta_star_planner/CMakeLists.txt @@ -0,0 +1,77 @@ +cmake_minimum_required(VERSION 3.5) +project(lazy_theta_star_planner) + +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav2_util REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(ament_cmake_gtest REQUIRED) + +include_directories( + include +) + +set(library_name ${PROJECT_NAME}_plugin) + +set(dependencies + ament_cmake + builtin_interfaces + nav2_common + nav2_core + nav2_costmap_2d + nav2_msgs + nav2_util + pluginlib + rclcpp + rclcpp_action + rclcpp_lifecycle + tf2_ros + ) + +add_library(${library_name} SHARED + src/lazy_theta_star2.cpp + src/lazy_theta_star_planner.cpp + ) + +ament_target_dependencies(${library_name} + ${dependencies} + ) + +target_compile_definitions(${library_name} PUBLIC "PLUGINLIB_DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file(nav2_core global_planner_plugin.xml) + +install(TARGETS ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} + ) + +install(DIRECTORY include/ + DESTINATION include/ + ) + +install(FILES global_planner_plugin.xml + DESTINATION share/${PROJECT_NAME} + ) + + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + + +ament_export_include_directories(include) +ament_export_libraries(${library_name}) +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/lazy_theta_star_planner/global_planner_plugin.xml b/lazy_theta_star_planner/global_planner_plugin.xml new file mode 100644 index 0000000000..f6f6ef3288 --- /dev/null +++ b/lazy_theta_star_planner/global_planner_plugin.xml @@ -0,0 +1,5 @@ + + + The implementation of lazy theta_star + + \ No newline at end of file diff --git a/lazy_theta_star_planner/include/lazy_theta_star_planner/lazy_theta_star2.h b/lazy_theta_star_planner/include/lazy_theta_star_planner/lazy_theta_star2.h new file mode 100644 index 0000000000..ac7a31720f --- /dev/null +++ b/lazy_theta_star_planner/include/lazy_theta_star_planner/lazy_theta_star2.h @@ -0,0 +1,144 @@ +// Copyright 2020 Anshumaan Singh +// +// 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. + +#ifndef LAZY_THETA_STAR_B__LAZY_THETA_STAR_H_ +#define LAZY_THETA_STAR_B__LAZY_THETA_STAR_H_ + +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/cost_values.hpp" + +typedef int id; +typedef double cost; +typedef int map_pts; +typedef double world_pts; + +#define INF_COST DBL_MAX +#define LETHAL_COST 252 + +template +struct coords +{ + coordsT x, y; +}; + +struct pos +{ + id pos_id; + cost * f; +}; + +struct tree_node +{ + map_pts x, y; + cost g = INF_COST; + cost h = INF_COST; + id parent_id; + int closed = 0; + cost f = INF_COST; +// int counter = 0; + int got_here = 0; +}; + +namespace lazyThetaStar +{ +class LazyThetaStar +{ +public: + nav2_costmap_2d::Costmap2D * costmap_; + nav2_util::LifecycleNode::SharedPtr node_; + + std::vector posn; + std::vector data; + std::vector pq; + int sizeX, sizeY; + int how_many_corners; + id id_gen; + coords src{}, dst{}; + int lethal_cost = LETHAL_COST; + + const map_pts moves[8][2] = {{0, 1}, + {0, -1}, + {1, 0}, + {-1, 0}, + {1, -1}, + {-1, 1}, + {1, 1}, + {-1, -1}}; + + LazyThetaStar(); + + bool getPath(std::vector> & raw_path); + + + bool losCheck(map_pts & x0, map_pts & y0, map_pts & x1, map_pts & y1) const; + + cost dist(map_pts & ax, map_pts & ay, map_pts & bx, map_pts & by) + { + return sqrt(pow(ax - bx, 2) + pow(ay - by, 2)); + } + + // TODO : FIND A WAY TO ADD REFERENCES TO THIS FUNCTION + bool isSafe(map_pts cx, map_pts cy) const + { + return costmap_->getCost(cx, cy) < lethal_cost; + } + + bool withinLimits(map_pts & cx, map_pts & cy) const + { + return cx >= 0 && cx < sizeX && cy >= 0 && cy < sizeY; + } + + void addIndex(map_pts cx, map_pts cy, id id_this) + { + posn[sizeX * cy + cx] = id_this; + } + + void getIndex(map_pts & cx, map_pts & cy, id & id_this) + { + id_this = posn[sizeX * cy + cx]; + } + void initializePosn(); + + void binaryHeapDelMin(); + + void pushToPq(id id_this); + + static bool comp(pos & p1, pos & p2) + { + return *(p1.f) > *(p2.f); + } + + void backtrace(std::vector> & raw_points, id & curr_id); + + void resetParent(map_pts & cx, map_pts & cy, id & curr_id); + + bool isGoal(map_pts & cx, map_pts & cy); + + void setNeighbors(map_pts & cx, map_pts & cy, id & curr_id); + + void initializeStuff(); + + void clearStuff(); + + void getNextNode(id & curr_id); + +}; +} // namespace lazyThetaStar + +#endif //LAZY_THETA_STAR_B_LAZY_THETA_STAR_H diff --git a/lazy_theta_star_planner/include/lazy_theta_star_planner/lazy_theta_star_planner.h b/lazy_theta_star_planner/include/lazy_theta_star_planner/lazy_theta_star_planner.h new file mode 100644 index 0000000000..e263bd1670 --- /dev/null +++ b/lazy_theta_star_planner/include/lazy_theta_star_planner/lazy_theta_star_planner.h @@ -0,0 +1,77 @@ +// Copyright 2020 Anshumaan Singh +// +// 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. + +#ifndef LAZY_THETA_STAR_PLANNER__LAZY_THETA_STAR_PLANNER_H_ +#define LAZY_THETA_STAR_PLANNER__LAZY_THETA_STAR_PLANNER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "nav2_core/global_planner.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_util/node_utils.hpp" +#include "lazy_theta_star2.h" + +namespace lazyThetaStarPlanner +{ + +class LazyThetaStarPlanner : public nav2_core::GlobalPlanner +{ +public: + std::shared_ptr tf_; + nav2_util::LifecycleNode::SharedPtr node_; + nav2_costmap_2d::Costmap2D * costmap_; + + std::string globalFrame_, name_; + int how_many_corners_; + double interpolation_dist_; + std::unique_ptr planner_; + + + unsigned int src_[2], dst_[2]; + + void configure( + rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmapRos) override; + + void cleanup() override; + + void activate() override; + + void deactivate() override; + + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) override; + + nav_msgs::msg::Path linearInterpolation( + std::vector> & raw_path, + double dist_bw_points); + +}; + +} // namespace planner + +#endif //LAZY_THETA_STAR_PLANNER__LAZY_THETA_STAR_PLANNER_H_ diff --git a/lazy_theta_star_planner/package.xml b/lazy_theta_star_planner/package.xml new file mode 100644 index 0000000000..682a84b703 --- /dev/null +++ b/lazy_theta_star_planner/package.xml @@ -0,0 +1,31 @@ + + + + lazy_theta_star_planner + 0.0.1 + TODO: Package description + anshu-man + Apache-2.0 + + ament_cmake + rclcpp + nav2_core + rclcpp_action + rclcpp_lifecycle + std_msgs + visualization_msgs + nav2_util + nav2_msgs + builtin_interfaces + tf2_ros + nav2_costmap_2d + pluginlib + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + diff --git a/lazy_theta_star_planner/src/lazy_theta_star2.cpp b/lazy_theta_star_planner/src/lazy_theta_star2.cpp new file mode 100644 index 0000000000..12bd6a87a5 --- /dev/null +++ b/lazy_theta_star_planner/src/lazy_theta_star2.cpp @@ -0,0 +1,364 @@ +// Copyright 2020 Anshumaan Singh +// +// 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. + +#include "lazy_theta_star_planner/lazy_theta_star2.h" +#include + +namespace lazyThetaStar +{ + +LazyThetaStar::LazyThetaStar() +{ + sizeX = 0; + sizeY = 0; +} + +bool LazyThetaStar::getPath(std::vector> & raw_path) +{ + RCLCPP_INFO(node_->get_logger(), "Path Planning Begins...."); + + initializeStuff(); + + std::cout << src.x << '\t' << src.y << '\t' << dst.x << '\t' << dst.y << '\n'; + + data.push_back({src.x, src.y, 0, dist(src.x, src.y, dst.x, dst.y), + 0, 1, dist(src.x, src.y, dst.x, dst.y)}); + addIndex(src.x, src.y, id_gen); + pushToPq(id_gen); + pushToPq(id_gen); + // extra one added due to binaryHeapDelMin starting from index 1 + + id_gen++; + id curr_id = 0; +// map_pts cx = src.x, cy = src.y; + + if (!losCheck(src.x, src.y, dst.x, dst.y)) { + + while (pq.size() > 0) { + + map_pts cx = data[curr_id].x; + map_pts cy = data[curr_id].y; + std::cout << "the current point is : \t\t\t" << cx << '\t' << cy << '\t' << data[curr_id].f << + '\n'; + id curr_par = data[curr_id].parent_id; + + if (isGoal(cx, cy)) { + if (!(losCheck(cx, cy, data[curr_par].x, data[curr_par].y))) { + resetParent(cx, cy, curr_id); + } + break; + } + + std::cout << "ITS PARENTS WERE : " << data[curr_par].x << '\t' << data[curr_par].y << '\t' << + curr_par << '\n'; + if (!(losCheck(cx, cy, data[curr_par].x, data[curr_par].y))) { + resetParent(cx, cy, curr_id); + } + + setNeighbors(cx, cy, curr_id); + + getNextNode(curr_id); + } + + if (pq.size() < 1) { + RCLCPP_INFO(node_->get_logger(), "No Path Found !!!!!!!!!!!"); + raw_path.clear(); + return false; + } + + } else { + RCLCPP_INFO(node_->get_logger(), "Straight Line Path Found!"); + data.push_back({dst.x, dst.y, dist(src.x, src.y, dst.x, dst.y), 0, 0, 1, + dist(src.x, src.y, dst.x, dst.y)}); + curr_id = id_gen; + } + + RCLCPP_INFO(node_->get_logger(), "REACHED DEST %i, %i", data[curr_id].x, data[curr_id].y); + + backtrace(raw_path, curr_id); + + RCLCPP_INFO(node_->get_logger(), "Out of it with path_length %i", raw_path.size()); + return true; +} + +void LazyThetaStar::initializePosn() +{ + posn.clear(); + for (int i = 0; i < sizeX * sizeY; i++) { + posn.push_back(-1); + } +} + +bool LazyThetaStar::losCheck(map_pts & x0, map_pts & y0, map_pts & x1, map_pts & y1) const +{ + int dy = y1 - y0, dx = x1 - x0, f = 0; + int sx, sy; + int cx = x0, cy = y0; + + if (dy < 0) { + dy = -dy; + sy = -1; + } else { + sy = 1; + } + + if (dx < 0) { + dx = -dx; + sx = -1; + } else { + sx = 1; + } + + if (dx >= dy) { + while (cx != x1) { + f += dy; + if (f >= dx) { + if (!isSafe(cx + (sx - 1) / 2, cy + (sy - 1) / 2)) { + return false; + } + cy += sy; + f -= dx; + } + if (f != 0 && !isSafe(cx + (sx - 1) / 2, cy + (sy - 1) / 2)) { + return false; + } + if (dy == 0 && !isSafe(cx + (sx - 1) / 2, cy) && !isSafe(cx + (sx - 1) / 2, cy - 1)) { + return false; + } + cx += sx; + } + } else { + while (cy != y1) { + f = f + dx; + if (f >= dy) { + if (!isSafe(cx + (sx - 1) / 2, cy + (sy - 1) / 2)) { + return false; + } + cx += sx; + f -= dy; + } + if (f != 0 && !isSafe(cx + (sx - 1) / 2, cy + (sy - 1) / 2)) { + return false; + } + if (dx == 0 && !isSafe(cx, cy + (sy - 1) / 2) && !isSafe(cx - 1, cy + (sy - 1) / 2)) { + return false; + } + cy += sy; + } + } + return true; +} + +void LazyThetaStar::binaryHeapDelMin() +{ + pop_heap(pq.begin() + 1, pq.end(), comp); + pq.pop_back(); +// int hole = 1; +// int succ = 2, size = pq.size() - 1, sz = size; +// // the main bottom up part where it compares and assigns the smallest value to the hole +// // remember that for this part the heap starts at index 1 +// +// while (succ < sz) { +// double k1 = *(pq[succ].f); +// double k2 = *(pq[succ + 1].f); +// // std::cout< k2) { +// // std::cout<<"came 1"<<'\n'; +// succ++; +// // data[hole].key = k2; +// pq[hole] = pq[succ]; +// } else { +// // std::cout<<"came 2"<<'\n'; +// // data[hole].key = k1; +// pq[hole] = pq[succ]; +// } +// hole = succ; +// succ <<= 1; +// } +// +// // this part checks if the value to be used to fill the last row's hole is small or not +// // if not small then it slides the small value up till it reaches the apt position +// double bubble = *(pq[sz].f); +// int pred = hole >> 1; +// // std::cout<<"HERE"<<'\t'< bubble) { +// // std::cout<<"HERE"<<'\t'<>= 1; +// } +// // std::cout<<"HERE AT:"<<'\t'<> & raw_points, id & curr_id) +{ + std::vector> path_rev; + do { + coords world; + costmap_->mapToWorld(data[curr_id].x, data[curr_id].y, world.x, world.y); + path_rev.push_back({world.x, world.y}); + RCLCPP_INFO( + node_->get_logger(), "%i, %i ---> %f, %f", data[curr_id].x, data[curr_id].y, world.x, + world.y); + if (data[curr_id].got_here == 1) { + std::cout << + "------------------------------------------------------> got the same error <-------------------------------------------------------" + << + '\n'; + break; + } + data[curr_id].got_here = 1; + curr_id = data[curr_id].parent_id; + } while (curr_id != 0); + coords w; + costmap_->mapToWorld(data[curr_id].x, data[curr_id].y, w.x, w.y); + path_rev.push_back({w.x, w.y}); + RCLCPP_INFO( + node_->get_logger(), "%i, %i ---> %f, %f", data[curr_id].x, data[curr_id].y, w.x, w.y); + + for (int i = path_rev.size() - 1; i >= 0; i--) { + raw_points.push_back(path_rev[i]); + } +} + +void LazyThetaStar::resetParent(map_pts & cx, map_pts & cy, id & curr_id) +{ + map_pts mx, my; + id m_id; + cost min_dist = INF_COST; + id min_dist_id; + + for (int i = 0; i < how_many_corners; i++) { + mx = cx + moves[i][0]; + my = cy + moves[i][1]; + if (withinLimits(mx, my)) { + getIndex(mx, my, m_id); + if (m_id != -1) { + if (data[m_id].f < min_dist) { + min_dist = data[m_id].f; + min_dist_id = m_id; + } + } + } + } + data[curr_id].parent_id = min_dist_id; + data[curr_id].g = data[min_dist_id].g + dist(cx, cy, data[min_dist_id].x, data[min_dist_id].y); + data[curr_id].f = data[curr_id].g + data[curr_id].h; +} + +bool LazyThetaStar::isGoal(map_pts & cx, map_pts & cy) +{ + return cx == dst.x && cy == dst.y; +} + +void LazyThetaStar::setNeighbors(map_pts & cx, map_pts & cy, id & curr_id) +{ + id curr_par = data[curr_id].parent_id; + std::cout << "ITS PARENTS ARE : " << data[curr_par].x << '\t' << data[curr_par].y << '\t' << + curr_par << '\n'; + map_pts px = data[curr_par].x, py = data[curr_par].y; + cost g_cost_par = data[curr_par].g; + + map_pts mx, my; + id m_id; + for (int i = 0; i < 8; i++) { + mx = cx + moves[i][0]; + my = cy + moves[i][1]; + if (mx == px && my == py) { + continue; + } + + if (withinLimits(mx, my)) { + cost g_cost = g_cost_par + dist(mx, my, px, py); + cost h_cost, cal_cost; + getIndex(mx, my, m_id); + + if (isSafe(mx, my) && m_id == -1) { + h_cost = dist(mx, my, dst.x, dst.y); + cal_cost = g_cost + h_cost; + data.push_back({mx, my, g_cost, h_cost, curr_par, -1, cal_cost}); + pushToPq(id_gen); + std::cout << "ADDED INDEX 1:" << data[id_gen].x << '\t' << data[id_gen].y << '\t' << + data[id_gen].f << '\n'; + addIndex(mx, my, id_gen); + id_gen++; + continue; + } else if (m_id != -1) { + tree_node & curr_node = data[m_id]; + h_cost = curr_node.h; + cal_cost = g_cost + h_cost; + if (curr_node.f > cal_cost) { + curr_node.g = g_cost; + curr_node.f = cal_cost; + curr_node.parent_id = curr_par; + if (curr_node.closed == 1) { + curr_node.closed = -1; + pushToPq(m_id); + std::cout << "ADDED INDEX 2:" << data[m_id].x << '\t' << data[m_id].y << '\t' << + data[m_id].f << '\n'; + } + } + continue; + } + } + } +} + +void LazyThetaStar::initializeStuff() +{ + id_gen = 0; + sizeX = costmap_->getSizeInCellsX(); + sizeY = costmap_->getSizeInCellsY(); + std::cout << sizeX << '\t' << sizeY << '\n'; + initializePosn(); + data.reserve(static_cast(sizeX * sizeY * 0.05)); +} + +void LazyThetaStar::clearStuff() +{ + data.clear(); + pq.clear(); + posn.clear(); +} + +void LazyThetaStar::getNextNode(id & curr_id) +{ + curr_id = pq[1].pos_id; + data[curr_id].closed = -1; + binaryHeapDelMin(); +} + + +//void LazyThetaStar::tfMapToWorld( +// map_pts & mx, +// map_pts & my, +// world_pts & wx, +// world_pts & wy); +//{ +//wx = costmap_->getOriginX() + (mx) * costmap_->getResolution(); +//wy = costmap_->getOriginY() + (my) * costmap_->getResolution(); +//} + +} // namespace lazyThetaStar diff --git a/lazy_theta_star_planner/src/lazy_theta_star_planner.cpp b/lazy_theta_star_planner/src/lazy_theta_star_planner.cpp new file mode 100644 index 0000000000..502f5f70aa --- /dev/null +++ b/lazy_theta_star_planner/src/lazy_theta_star_planner.cpp @@ -0,0 +1,151 @@ +// Copyright 2020 Anshumaan Singh +// +// 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. + +#include +#include +#include +#include "lazy_theta_star_planner/lazy_theta_star_planner.h" +#include "lazy_theta_star_planner/lazy_theta_star2.h" + + +namespace lazyThetaStarPlanner +{ +void LazyThetaStarPlanner::configure( + rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmapRos) +{ + planner_ = std::make_unique(); + node_ = parent; + name_ = name; + planner_->costmap_ = costmapRos->getCostmap(); + globalFrame_ = costmapRos->getGlobalFrameID(); + + nav2_util::declare_parameter_if_not_declared( + node_, name_ + ".how_many_corners", rclcpp::ParameterValue( + 8)); + + node_->get_parameter(name_ + ".how_many_corners", how_many_corners_); + + if (how_many_corners_ != 8) { + how_many_corners_ = 4; + } + + nav2_util::declare_parameter_if_not_declared( + node_, name_ + ".interpolation_dist", rclcpp::ParameterValue( + 0.1)); + + node_->get_parameter(name_ + ".interpolation_dist", interpolation_dist_); + +} + +void LazyThetaStarPlanner::cleanup() +{ + RCLCPP_INFO( + node_->get_logger(), "CleaningUp plugin %s of type lazyThetaStarPlanner", + name_.c_str()); +} + +void LazyThetaStarPlanner::activate() +{ + RCLCPP_INFO( + node_->get_logger(), "Activating plugin %s of type lazyThetaStarPlanner", + name_.c_str()); +} + +void LazyThetaStarPlanner::deactivate() +{ + RCLCPP_INFO( + node_->get_logger(), "Deactivating plugin %s of type lazyThetaStarPlanner", + name_.c_str()); +} + +nav_msgs::msg::Path LazyThetaStarPlanner::createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) +{ + nav_msgs::msg::Path global_path; + std::vector> path; + + planner_->costmap_->worldToMap(start.pose.position.x, start.pose.position.y, src_[0], src_[1]); + planner_->src = {static_cast(src_[0]), static_cast(src_[1])}; + + planner_->costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, dst_[0], dst_[1]); + planner_->dst = {static_cast(dst_[0]), static_cast(dst_[1])}; + + RCLCPP_INFO( + node_->get_logger(), "Got the src and dst... (%i, %i) && (%i, %i)", planner_->src.x, planner_->src.y, + planner_->dst.x, planner_->dst.y); + + planner_->node_ = node_; + planner_->how_many_corners = how_many_corners_; + planner_->lethal_cost = LETHAL_COST; + + planner_->initializeStuff(); + planner_->getPath(path); + planner_->clearStuff(); + + RCLCPP_INFO(node_->get_logger(), "RECEIVED THE PATH"); + + if (path.size() > 1) { + + for (int i = 0; i < path.size(); i++) { + std::cout << path[i].x << '\t' << path[i].y << '\n'; + } + + global_path = linearInterpolation(path, 0.1); + } else { + global_path.poses.clear(); + } + path.clear(); + std::cout << global_path.poses.size() << '\n'; + global_path.header.stamp = node_->now(); + global_path.header.frame_id = globalFrame_; + RCLCPP_INFO(node_->get_logger(), "SENT OUT THE PATH"); + return global_path; +} + +nav_msgs::msg::Path LazyThetaStarPlanner::linearInterpolation( + std::vector> & raw_path, + double dist_bw_points) +{ + nav_msgs::msg::Path pa; + + for (int j = 0; j < raw_path.size() - 1; j++) { + geometry_msgs::msg::PoseStamped p; + coords pt1 = {raw_path[j].x, raw_path[j].y}; + p.pose.position.x = pt1.x; + p.pose.position.y = pt1.y; + pa.poses.push_back(p); + + coords pt2 = {raw_path[j + 1].x, raw_path[j + 1].y}; + geometry_msgs::msg::PoseStamped p1; + double distance = std::hypot(pt2.x - pt1.x, pt2.y - pt1.y); + int loops = distance / dist_bw_points; + double sin_alpha = (pt2.y - pt1.y) / distance; + double cos_alpha = (pt2.x - pt1.x) / distance; + for (int k = 1; k < loops; k++) { + p1.pose.position.x = pt1.x + k * dist_bw_points * cos_alpha; + p1.pose.position.y = pt1.y + k * dist_bw_points * sin_alpha; + pa.poses.push_back(p1); + } + } + return pa; +} + +} // namespace lazyThetaStarPlanner + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(lazyThetaStarPlanner::LazyThetaStarPlanner, nav2_core::GlobalPlanner) From f4a622b64a9861b55aac9737bae80e258d87ae8b Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Wed, 15 Jul 2020 19:03:13 +0530 Subject: [PATCH 18/81] removed comments --- .../src/lazy_theta_star2.cpp | 118 +++++++----------- 1 file changed, 46 insertions(+), 72 deletions(-) diff --git a/lazy_theta_star_planner/src/lazy_theta_star2.cpp b/lazy_theta_star_planner/src/lazy_theta_star2.cpp index 12bd6a87a5..ea52ee5602 100644 --- a/lazy_theta_star_planner/src/lazy_theta_star2.cpp +++ b/lazy_theta_star_planner/src/lazy_theta_star2.cpp @@ -41,7 +41,6 @@ bool LazyThetaStar::getPath(std::vector> & raw_path) id_gen++; id curr_id = 0; -// map_pts cx = src.x, cy = src.y; if (!losCheck(src.x, src.y, dst.x, dst.y)) { @@ -49,8 +48,6 @@ bool LazyThetaStar::getPath(std::vector> & raw_path) map_pts cx = data[curr_id].x; map_pts cy = data[curr_id].y; - std::cout << "the current point is : \t\t\t" << cx << '\t' << cy << '\t' << data[curr_id].f << - '\n'; id curr_par = data[curr_id].parent_id; if (isGoal(cx, cy)) { @@ -60,8 +57,6 @@ bool LazyThetaStar::getPath(std::vector> & raw_path) break; } - std::cout << "ITS PARENTS WERE : " << data[curr_par].x << '\t' << data[curr_par].y << '\t' << - curr_par << '\n'; if (!(losCheck(cx, cy, data[curr_par].x, data[curr_par].y))) { resetParent(cx, cy, curr_id); } @@ -87,8 +82,7 @@ bool LazyThetaStar::getPath(std::vector> & raw_path) RCLCPP_INFO(node_->get_logger(), "REACHED DEST %i, %i", data[curr_id].x, data[curr_id].y); backtrace(raw_path, curr_id); - - RCLCPP_INFO(node_->get_logger(), "Out of it with path_length %i", raw_path.size()); + return true; } @@ -162,48 +156,48 @@ bool LazyThetaStar::losCheck(map_pts & x0, map_pts & y0, map_pts & x1, map_pts & void LazyThetaStar::binaryHeapDelMin() { - pop_heap(pq.begin() + 1, pq.end(), comp); - pq.pop_back(); -// int hole = 1; -// int succ = 2, size = pq.size() - 1, sz = size; -// // the main bottom up part where it compares and assigns the smallest value to the hole -// // remember that for this part the heap starts at index 1 -// -// while (succ < sz) { -// double k1 = *(pq[succ].f); -// double k2 = *(pq[succ + 1].f); -// // std::cout< k2) { -// // std::cout<<"came 1"<<'\n'; -// succ++; -// // data[hole].key = k2; -// pq[hole] = pq[succ]; -// } else { -// // std::cout<<"came 2"<<'\n'; -// // data[hole].key = k1; -// pq[hole] = pq[succ]; -// } -// hole = succ; -// succ <<= 1; -// } -// -// // this part checks if the value to be used to fill the last row's hole is small or not -// // if not small then it slides the small value up till it reaches the apt position -// double bubble = *(pq[sz].f); -// int pred = hole >> 1; -// // std::cout<<"HERE"<<'\t'< bubble) { -// // std::cout<<"HERE"<<'\t'<>= 1; -// } -// // std::cout<<"HERE AT:"<<'\t'< k2) { + // std::cout<<"came 1"<<'\n'; + succ++; + // data[hole].key = k2; + pq[hole] = pq[succ]; + } else { + // std::cout<<"came 2"<<'\n'; + // data[hole].key = k1; + pq[hole] = pq[succ]; + } + hole = succ; + succ <<= 1; + } + + // this part checks if the value to be used to fill the last row's hole is small or not + // if not small then it slides the small value up till it reaches the apt position + double bubble = *(pq[sz].f); + int pred = hole >> 1; + // std::cout<<"HERE"<<'\t'< bubble) { + // std::cout<<"HERE"<<'\t'<>= 1; + } + // std::cout<<"HERE AT:"<<'\t'<> & raw_points, id & coords world; costmap_->mapToWorld(data[curr_id].x, data[curr_id].y, world.x, world.y); path_rev.push_back({world.x, world.y}); - RCLCPP_INFO( - node_->get_logger(), "%i, %i ---> %f, %f", data[curr_id].x, data[curr_id].y, world.x, - world.y); - if (data[curr_id].got_here == 1) { - std::cout << - "------------------------------------------------------> got the same error <-------------------------------------------------------" - << - '\n'; - break; - } - data[curr_id].got_here = 1; curr_id = data[curr_id].parent_id; } while (curr_id != 0); coords w; costmap_->mapToWorld(data[curr_id].x, data[curr_id].y, w.x, w.y); path_rev.push_back({w.x, w.y}); - RCLCPP_INFO( - node_->get_logger(), "%i, %i ---> %f, %f", data[curr_id].x, data[curr_id].y, w.x, w.y); - + for (int i = path_rev.size() - 1; i >= 0; i--) { raw_points.push_back(path_rev[i]); } @@ -276,8 +257,6 @@ bool LazyThetaStar::isGoal(map_pts & cx, map_pts & cy) void LazyThetaStar::setNeighbors(map_pts & cx, map_pts & cy, id & curr_id) { id curr_par = data[curr_id].parent_id; - std::cout << "ITS PARENTS ARE : " << data[curr_par].x << '\t' << data[curr_par].y << '\t' << - curr_par << '\n'; map_pts px = data[curr_par].x, py = data[curr_par].y; cost g_cost_par = data[curr_par].g; @@ -300,8 +279,6 @@ void LazyThetaStar::setNeighbors(map_pts & cx, map_pts & cy, id & curr_id) cal_cost = g_cost + h_cost; data.push_back({mx, my, g_cost, h_cost, curr_par, -1, cal_cost}); pushToPq(id_gen); - std::cout << "ADDED INDEX 1:" << data[id_gen].x << '\t' << data[id_gen].y << '\t' << - data[id_gen].f << '\n'; addIndex(mx, my, id_gen); id_gen++; continue; @@ -315,9 +292,7 @@ void LazyThetaStar::setNeighbors(map_pts & cx, map_pts & cy, id & curr_id) curr_node.parent_id = curr_par; if (curr_node.closed == 1) { curr_node.closed = -1; - pushToPq(m_id); - std::cout << "ADDED INDEX 2:" << data[m_id].x << '\t' << data[m_id].y << '\t' << - data[m_id].f << '\n'; + pushToPq(m_id); } } continue; @@ -330,8 +305,7 @@ void LazyThetaStar::initializeStuff() { id_gen = 0; sizeX = costmap_->getSizeInCellsX(); - sizeY = costmap_->getSizeInCellsY(); - std::cout << sizeX << '\t' << sizeY << '\n'; + sizeY = costmap_->getSizeInCellsY(); initializePosn(); data.reserve(static_cast(sizeX * sizeY * 0.05)); } From 6ad8bbdba5de15875f687df9b27bbdb95cd3e85c Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Wed, 15 Jul 2020 19:05:44 +0530 Subject: [PATCH 19/81] removed comments --- .../src/lazy_theta_star_planner.cpp | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/lazy_theta_star_planner/src/lazy_theta_star_planner.cpp b/lazy_theta_star_planner/src/lazy_theta_star_planner.cpp index 502f5f70aa..fbc700c80d 100644 --- a/lazy_theta_star_planner/src/lazy_theta_star_planner.cpp +++ b/lazy_theta_star_planner/src/lazy_theta_star_planner.cpp @@ -96,14 +96,7 @@ nav_msgs::msg::Path LazyThetaStarPlanner::createPlan( planner_->getPath(path); planner_->clearStuff(); - RCLCPP_INFO(node_->get_logger(), "RECEIVED THE PATH"); - if (path.size() > 1) { - - for (int i = 0; i < path.size(); i++) { - std::cout << path[i].x << '\t' << path[i].y << '\n'; - } - global_path = linearInterpolation(path, 0.1); } else { global_path.poses.clear(); @@ -117,7 +110,7 @@ nav_msgs::msg::Path LazyThetaStarPlanner::createPlan( } nav_msgs::msg::Path LazyThetaStarPlanner::linearInterpolation( - std::vector> & raw_path, + std::vector > & raw_path, double dist_bw_points) { nav_msgs::msg::Path pa; @@ -147,5 +140,4 @@ nav_msgs::msg::Path LazyThetaStarPlanner::linearInterpolation( } // namespace lazyThetaStarPlanner #include "pluginlib/class_list_macros.hpp" - PLUGINLIB_EXPORT_CLASS(lazyThetaStarPlanner::LazyThetaStarPlanner, nav2_core::GlobalPlanner) From 6376718827d032d4680c8ee8dd472f5767c29f03 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Wed, 15 Jul 2020 23:19:28 +0530 Subject: [PATCH 20/81] Update lazy_theta_star2.cpp --- .../src/lazy_theta_star2.cpp | 22 +++++++++---------- 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/lazy_theta_star_planner/src/lazy_theta_star2.cpp b/lazy_theta_star_planner/src/lazy_theta_star2.cpp index ea52ee5602..f898bbfc85 100644 --- a/lazy_theta_star_planner/src/lazy_theta_star2.cpp +++ b/lazy_theta_star_planner/src/lazy_theta_star2.cpp @@ -43,9 +43,7 @@ bool LazyThetaStar::getPath(std::vector> & raw_path) id curr_id = 0; if (!losCheck(src.x, src.y, dst.x, dst.y)) { - while (pq.size() > 0) { - map_pts cx = data[curr_id].x; map_pts cy = data[curr_id].y; id curr_par = data[curr_id].parent_id; @@ -325,14 +323,14 @@ void LazyThetaStar::getNextNode(id & curr_id) } -//void LazyThetaStar::tfMapToWorld( -// map_pts & mx, -// map_pts & my, -// world_pts & wx, -// world_pts & wy); -//{ -//wx = costmap_->getOriginX() + (mx) * costmap_->getResolution(); -//wy = costmap_->getOriginY() + (my) * costmap_->getResolution(); -//} +// void LazyThetaStar::tfMapToWorld( +// map_pts & mx, +// map_pts & my, +// world_pts & wx, +// world_pts & wy); +// { +// wx = costmap_->getOriginX() + (mx) * costmap_->getResolution(); +// wy = costmap_->getOriginY() + (my) * costmap_->getResolution(); +// } -} // namespace lazyThetaStar +} // namespace lazyThetaStar From 61ceb55e3bb7b83978480ed7182623c9d1191ad4 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Fri, 17 Jul 2020 01:35:55 +0530 Subject: [PATCH 21/81] update files - replaced manual management of priority queue with stl priority queue - added the parameter ".lethal_cost" - removed unnecessary parameters passed to the functions --- .../src/lazy_theta_star2.cpp | 193 ++++++------------ .../src/lazy_theta_star_planner.cpp | 20 +- 2 files changed, 72 insertions(+), 141 deletions(-) diff --git a/lazy_theta_star_planner/src/lazy_theta_star2.cpp b/lazy_theta_star_planner/src/lazy_theta_star2.cpp index f898bbfc85..11302b813b 100644 --- a/lazy_theta_star_planner/src/lazy_theta_star2.cpp +++ b/lazy_theta_star_planner/src/lazy_theta_star2.cpp @@ -35,36 +35,35 @@ bool LazyThetaStar::getPath(std::vector> & raw_path) data.push_back({src.x, src.y, 0, dist(src.x, src.y, dst.x, dst.y), 0, 1, dist(src.x, src.y, dst.x, dst.y)}); addIndex(src.x, src.y, id_gen); - pushToPq(id_gen); - pushToPq(id_gen); - // extra one added due to binaryHeapDelMin starting from index 1 + pq.push({id_gen, &(data[id_gen].f)}); id_gen++; id curr_id = 0; if (!losCheck(src.x, src.y, dst.x, dst.y)) { - while (pq.size() > 0) { - map_pts cx = data[curr_id].x; - map_pts cy = data[curr_id].y; - id curr_par = data[curr_id].parent_id; - - if (isGoal(cx, cy)) { - if (!(losCheck(cx, cy, data[curr_par].x, data[curr_par].y))) { - resetParent(cx, cy, curr_id); + while (!pq.empty()) { + + tree_node curr_data = data[curr_id]; + curr_data.closed = -1; + id curr_par = curr_data.parent_id; + + if (isGoal(curr_data.x, curr_data.y)) { + if (!(losCheck(curr_data.x, curr_data.y, data[curr_par].x, data[curr_par].y))) { + resetParent(curr_data); } break; } - if (!(losCheck(cx, cy, data[curr_par].x, data[curr_par].y))) { - resetParent(cx, cy, curr_id); + if (!(losCheck(curr_data.x, curr_data.y, data[curr_par].x, data[curr_par].y))) { + resetParent(curr_data); } - setNeighbors(cx, cy, curr_id); + setNeighbors(curr_data); - getNextNode(curr_id); + curr_id = pq.top().pos_id; + pq.pop(); } - - if (pq.size() < 1) { + if (pq.empty()) { RCLCPP_INFO(node_->get_logger(), "No Path Found !!!!!!!!!!!"); raw_path.clear(); return false; @@ -80,7 +79,7 @@ bool LazyThetaStar::getPath(std::vector> & raw_path) RCLCPP_INFO(node_->get_logger(), "REACHED DEST %i, %i", data[curr_id].x, data[curr_id].y); backtrace(raw_path, curr_id); - + return true; } @@ -94,38 +93,25 @@ void LazyThetaStar::initializePosn() bool LazyThetaStar::losCheck(map_pts & x0, map_pts & y0, map_pts & x1, map_pts & y1) const { - int dy = y1 - y0, dx = x1 - x0, f = 0; - int sx, sy; + int dy = abs(y1 - y0), dx = abs(x1 - x0), f = 0; + int sx = x1 > x0 ? 1 : -1, sy = y1 > y0 ? 1 : -1; int cx = x0, cy = y0; - - if (dy < 0) { - dy = -dy; - sy = -1; - } else { - sy = 1; - } - - if (dx < 0) { - dx = -dx; - sx = -1; - } else { - sx = 1; - } + int ux = (sx - 1) / 2, uy = (sy - 1) / 2; if (dx >= dy) { while (cx != x1) { f += dy; if (f >= dx) { - if (!isSafe(cx + (sx - 1) / 2, cy + (sy - 1) / 2)) { + if (!isSafe(cx + ux, cy + uy)) { return false; } cy += sy; f -= dx; } - if (f != 0 && !isSafe(cx + (sx - 1) / 2, cy + (sy - 1) / 2)) { + if (f != 0 && !isSafe(cx + ux, cy + uy)) { return false; } - if (dy == 0 && !isSafe(cx + (sx - 1) / 2, cy) && !isSafe(cx + (sx - 1) / 2, cy - 1)) { + if (dy == 0 && !isSafe(cx + ux, cy) && !isSafe(cx + ux, cy - 1)) { return false; } cx += sx; @@ -134,16 +120,16 @@ bool LazyThetaStar::losCheck(map_pts & x0, map_pts & y0, map_pts & x1, map_pts & while (cy != y1) { f = f + dx; if (f >= dy) { - if (!isSafe(cx + (sx - 1) / 2, cy + (sy - 1) / 2)) { + if (!isSafe(cx + ux, cy + uy)) { return false; } cx += sx; f -= dy; } - if (f != 0 && !isSafe(cx + (sx - 1) / 2, cy + (sy - 1) / 2)) { + if (f != 0 && !isSafe(cx + ux, cy + uy)) { return false; } - if (dx == 0 && !isSafe(cx, cy + (sy - 1) / 2) && !isSafe(cx - 1, cy + (sy - 1) / 2)) { + if (dx == 0 && !isSafe(cx, cy + uy) && !isSafe(cx - 1, cy + uy)) { return false; } cy += sy; @@ -152,58 +138,6 @@ bool LazyThetaStar::losCheck(map_pts & x0, map_pts & y0, map_pts & x1, map_pts & return true; } -void LazyThetaStar::binaryHeapDelMin() -{ -// pop_heap(pq.begin() + 1, pq.end(), comp); -// pq.pop_back(); - int hole = 1; - int succ = 2, size = pq.size() - 1, sz = size; - // the main bottom up part where it compares and assigns the smallest value to the hole - // remember that for this part the heap starts at index 1 - - while (succ < sz) { - double k1 = *(pq[succ].f); - double k2 = *(pq[succ + 1].f); - // std::cout< k2) { - // std::cout<<"came 1"<<'\n'; - succ++; - // data[hole].key = k2; - pq[hole] = pq[succ]; - } else { - // std::cout<<"came 2"<<'\n'; - // data[hole].key = k1; - pq[hole] = pq[succ]; - } - hole = succ; - succ <<= 1; - } - - // this part checks if the value to be used to fill the last row's hole is small or not - // if not small then it slides the small value up till it reaches the apt position - double bubble = *(pq[sz].f); - int pred = hole >> 1; - // std::cout<<"HERE"<<'\t'< bubble) { - // std::cout<<"HERE"<<'\t'<>= 1; - } - // std::cout<<"HERE AT:"<<'\t'<> & raw_points, id & curr_id) { std::vector> path_rev; @@ -216,13 +150,13 @@ void LazyThetaStar::backtrace(std::vector> & raw_points, id & coords w; costmap_->mapToWorld(data[curr_id].x, data[curr_id].y, w.x, w.y); path_rev.push_back({w.x, w.y}); - + for (int i = path_rev.size() - 1; i >= 0; i--) { raw_points.push_back(path_rev[i]); } } -void LazyThetaStar::resetParent(map_pts & cx, map_pts & cy, id & curr_id) +void LazyThetaStar::resetParent(tree_node & curr_data) { map_pts mx, my; id m_id; @@ -230,8 +164,9 @@ void LazyThetaStar::resetParent(map_pts & cx, map_pts & cy, id & curr_id) id min_dist_id; for (int i = 0; i < how_many_corners; i++) { - mx = cx + moves[i][0]; - my = cy + moves[i][1]; + mx = curr_data.x + moves[i][0]; + my = curr_data.y + moves[i][1]; + if (withinLimits(mx, my)) { getIndex(mx, my, m_id); if (m_id != -1) { @@ -242,55 +177,55 @@ void LazyThetaStar::resetParent(map_pts & cx, map_pts & cy, id & curr_id) } } } - data[curr_id].parent_id = min_dist_id; - data[curr_id].g = data[min_dist_id].g + dist(cx, cy, data[min_dist_id].x, data[min_dist_id].y); - data[curr_id].f = data[curr_id].g + data[curr_id].h; + curr_data.parent_id = min_dist_id; + curr_data.g = data[min_dist_id].g + + dist(curr_data.x, curr_data.y, data[min_dist_id].x, data[min_dist_id].y); + curr_data.f = curr_data.g + curr_data.h; } bool LazyThetaStar::isGoal(map_pts & cx, map_pts & cy) { return cx == dst.x && cy == dst.y; } - -void LazyThetaStar::setNeighbors(map_pts & cx, map_pts & cy, id & curr_id) +void LazyThetaStar::setNeighbors(tree_node & curr_data) { - id curr_par = data[curr_id].parent_id; - map_pts px = data[curr_par].x, py = data[curr_par].y; - cost g_cost_par = data[curr_par].g; + + tree_node & curr_par = data[curr_data.parent_id]; map_pts mx, my; id m_id; - for (int i = 0; i < 8; i++) { - mx = cx + moves[i][0]; - my = cy + moves[i][1]; - if (mx == px && my == py) { + for (int i = 0; i < how_many_corners; i++) { + mx = curr_data.x + moves[i][0]; + my = curr_data.y + moves[i][1]; + + if (mx == curr_par.x && my == curr_par.y) { continue; } if (withinLimits(mx, my)) { - cost g_cost = g_cost_par + dist(mx, my, px, py); + cost g_cost = curr_par.g + dist(mx, my, curr_par.x, curr_par.y); cost h_cost, cal_cost; getIndex(mx, my, m_id); if (isSafe(mx, my) && m_id == -1) { h_cost = dist(mx, my, dst.x, dst.y); cal_cost = g_cost + h_cost; - data.push_back({mx, my, g_cost, h_cost, curr_par, -1, cal_cost}); - pushToPq(id_gen); + data.push_back({mx, my, g_cost, h_cost, curr_data.parent_id, -1, cal_cost}); + pq.push({id_gen, &(data[id_gen].f)}); addIndex(mx, my, id_gen); id_gen++; continue; } else if (m_id != -1) { tree_node & curr_node = data[m_id]; - h_cost = curr_node.h; + h_cost = data[m_id].h; cal_cost = g_cost + h_cost; if (curr_node.f > cal_cost) { curr_node.g = g_cost; curr_node.f = cal_cost; - curr_node.parent_id = curr_par; + curr_node.parent_id = curr_data.parent_id; if (curr_node.closed == 1) { curr_node.closed = -1; - pushToPq(m_id); + pq.push({m_id, &(data[m_id].f)}); } } continue; @@ -302,8 +237,13 @@ void LazyThetaStar::setNeighbors(map_pts & cx, map_pts & cy, id & curr_id) void LazyThetaStar::initializeStuff() { id_gen = 0; + if (costmap_->getCost(src.x, + src.y) > lethal_cost && costmap_->getCost(src.x, src.y) < LETHAL_COST) + { + lethal_cost = costmap_->getCost(src.x, src.y); + } sizeX = costmap_->getSizeInCellsX(); - sizeY = costmap_->getSizeInCellsY(); + sizeY = costmap_->getSizeInCellsY(); initializePosn(); data.reserve(static_cast(sizeX * sizeY * 0.05)); } @@ -311,26 +251,7 @@ void LazyThetaStar::initializeStuff() void LazyThetaStar::clearStuff() { data.clear(); - pq.clear(); + pq = std::priority_queue, comp>(); posn.clear(); } - -void LazyThetaStar::getNextNode(id & curr_id) -{ - curr_id = pq[1].pos_id; - data[curr_id].closed = -1; - binaryHeapDelMin(); -} - - -// void LazyThetaStar::tfMapToWorld( -// map_pts & mx, -// map_pts & my, -// world_pts & wx, -// world_pts & wy); -// { -// wx = costmap_->getOriginX() + (mx) * costmap_->getResolution(); -// wy = costmap_->getOriginY() + (my) * costmap_->getResolution(); -// } - -} // namespace lazyThetaStar +} // namespace lazyThetaStar diff --git a/lazy_theta_star_planner/src/lazy_theta_star_planner.cpp b/lazy_theta_star_planner/src/lazy_theta_star_planner.cpp index fbc700c80d..9cb073a6db 100644 --- a/lazy_theta_star_planner/src/lazy_theta_star_planner.cpp +++ b/lazy_theta_star_planner/src/lazy_theta_star_planner.cpp @@ -18,7 +18,6 @@ #include "lazy_theta_star_planner/lazy_theta_star_planner.h" #include "lazy_theta_star_planner/lazy_theta_star2.h" - namespace lazyThetaStarPlanner { void LazyThetaStarPlanner::configure( @@ -48,6 +47,11 @@ void LazyThetaStarPlanner::configure( node_->get_parameter(name_ + ".interpolation_dist", interpolation_dist_); + nav2_util::declare_parameter_if_not_declared( + node_, name_ + ".lethal_cost", rclcpp::ParameterValue( + 127)); + + node_->get_parameter(name_ + ".lethal_cost", lethal_cost_); } void LazyThetaStarPlanner::cleanup() @@ -85,18 +89,23 @@ nav_msgs::msg::Path LazyThetaStarPlanner::createPlan( planner_->dst = {static_cast(dst_[0]), static_cast(dst_[1])}; RCLCPP_INFO( - node_->get_logger(), "Got the src and dst... (%i, %i) && (%i, %i)", planner_->src.x, planner_->src.y, - planner_->dst.x, planner_->dst.y); + node_->get_logger(), "Got the src and dst... (%i, %i) && (%i, %i)", + planner_->src.x, planner_->src.y, planner_->dst.x, planner_->dst.y); planner_->node_ = node_; planner_->how_many_corners = how_many_corners_; - planner_->lethal_cost = LETHAL_COST; planner_->initializeStuff(); planner_->getPath(path); planner_->clearStuff(); + RCLCPP_INFO(node_->get_logger(), "RECEIVED THE PATH"); + if (path.size() > 1) { + for (int i = 0; i < path.size(); i++) { + std::cout << path[i].x << '\t' << path[i].y << '\n'; + } + global_path = linearInterpolation(path, 0.1); } else { global_path.poses.clear(); @@ -110,7 +119,7 @@ nav_msgs::msg::Path LazyThetaStarPlanner::createPlan( } nav_msgs::msg::Path LazyThetaStarPlanner::linearInterpolation( - std::vector > & raw_path, + std::vector> & raw_path, double dist_bw_points) { nav_msgs::msg::Path pa; @@ -140,4 +149,5 @@ nav_msgs::msg::Path LazyThetaStarPlanner::linearInterpolation( } // namespace lazyThetaStarPlanner #include "pluginlib/class_list_macros.hpp" + PLUGINLIB_EXPORT_CLASS(lazyThetaStarPlanner::LazyThetaStarPlanner, nav2_core::GlobalPlanner) From 27c4d3d24b16c0140a19d66697c94a6892ea5934 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Fri, 17 Jul 2020 01:37:01 +0530 Subject: [PATCH 22/81] update files - updated the header files in accordance to their .cpp counterparts --- .../lazy_theta_star2.h | 32 +++++++------------ .../lazy_theta_star_planner.h | 10 +++--- 2 files changed, 16 insertions(+), 26 deletions(-) diff --git a/lazy_theta_star_planner/include/lazy_theta_star_planner/lazy_theta_star2.h b/lazy_theta_star_planner/include/lazy_theta_star_planner/lazy_theta_star2.h index ac7a31720f..7e380ca42a 100644 --- a/lazy_theta_star_planner/include/lazy_theta_star_planner/lazy_theta_star2.h +++ b/lazy_theta_star_planner/include/lazy_theta_star_planner/lazy_theta_star2.h @@ -51,10 +51,15 @@ struct tree_node id parent_id; int closed = 0; cost f = INF_COST; -// int counter = 0; - int got_here = 0; }; +struct comp +{ + bool operator()(pos & p1, pos & p2) + { + return *(p1.f) > *(p2.f); + } +}; namespace lazyThetaStar { class LazyThetaStar @@ -65,7 +70,7 @@ class LazyThetaStar std::vector posn; std::vector data; - std::vector pq; + std::priority_queue, comp> pq; int sizeX, sizeY; int how_many_corners; id id_gen; @@ -85,7 +90,6 @@ class LazyThetaStar bool getPath(std::vector> & raw_path); - bool losCheck(map_pts & x0, map_pts & y0, map_pts & x1, map_pts & y1) const; cost dist(map_pts & ax, map_pts & ay, map_pts & bx, map_pts & by) @@ -93,7 +97,7 @@ class LazyThetaStar return sqrt(pow(ax - bx, 2) + pow(ay - by, 2)); } - // TODO : FIND A WAY TO ADD REFERENCES TO THIS FUNCTION + // TODO (Anshu-man567) : FIND A WAY TO ADD REFERENCES TO THIS FUNCTION THAT IS IF POSSIBLE bool isSafe(map_pts cx, map_pts cy) const { return costmap_->getCost(cx, cy) < lethal_cost; @@ -115,30 +119,18 @@ class LazyThetaStar } void initializePosn(); - void binaryHeapDelMin(); - - void pushToPq(id id_this); - - static bool comp(pos & p1, pos & p2) - { - return *(p1.f) > *(p2.f); - } - void backtrace(std::vector> & raw_points, id & curr_id); - void resetParent(map_pts & cx, map_pts & cy, id & curr_id); - bool isGoal(map_pts & cx, map_pts & cy); - void setNeighbors(map_pts & cx, map_pts & cy, id & curr_id); - void initializeStuff(); void clearStuff(); - void getNextNode(id & curr_id); + void resetParent(tree_node & curr_data); + void setNeighbors(tree_node & curr_data); }; } // namespace lazyThetaStar -#endif //LAZY_THETA_STAR_B_LAZY_THETA_STAR_H +#endif // LAZY_THETA_STAR_B_LAZY_THETA_STAR_H_ diff --git a/lazy_theta_star_planner/include/lazy_theta_star_planner/lazy_theta_star_planner.h b/lazy_theta_star_planner/include/lazy_theta_star_planner/lazy_theta_star_planner.h index e263bd1670..f0f7cfb95c 100644 --- a/lazy_theta_star_planner/include/lazy_theta_star_planner/lazy_theta_star_planner.h +++ b/lazy_theta_star_planner/include/lazy_theta_star_planner/lazy_theta_star_planner.h @@ -31,7 +31,7 @@ #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_costmap_2d/cost_values.hpp" #include "nav2_util/node_utils.hpp" -#include "lazy_theta_star2.h" +#include "lazy_theta_star_planner/lazy_theta_star2.h" namespace lazyThetaStarPlanner { @@ -46,9 +46,9 @@ class LazyThetaStarPlanner : public nav2_core::GlobalPlanner std::string globalFrame_, name_; int how_many_corners_; double interpolation_dist_; + int lethal_cost_; std::unique_ptr planner_; - unsigned int src_[2], dst_[2]; void configure( @@ -69,9 +69,7 @@ class LazyThetaStarPlanner : public nav2_core::GlobalPlanner nav_msgs::msg::Path linearInterpolation( std::vector> & raw_path, double dist_bw_points); - }; +} // namespace lazyThetaStarPlanner -} // namespace planner - -#endif //LAZY_THETA_STAR_PLANNER__LAZY_THETA_STAR_PLANNER_H_ +#endif // LAZY_THETA_STAR_PLANNER__LAZY_THETA_STAR_PLANNER_H_ From 72eac931b877e57544bf2fbf75c7558141fdbaaa Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Mon, 28 Sep 2020 23:13:47 +0530 Subject: [PATCH 23/81] Delete lazy_theta_star2.h --- .../lazy_theta_star2.h | 136 ------------------ 1 file changed, 136 deletions(-) delete mode 100644 lazy_theta_star_planner/include/lazy_theta_star_planner/lazy_theta_star2.h diff --git a/lazy_theta_star_planner/include/lazy_theta_star_planner/lazy_theta_star2.h b/lazy_theta_star_planner/include/lazy_theta_star_planner/lazy_theta_star2.h deleted file mode 100644 index 7e380ca42a..0000000000 --- a/lazy_theta_star_planner/include/lazy_theta_star_planner/lazy_theta_star2.h +++ /dev/null @@ -1,136 +0,0 @@ -// Copyright 2020 Anshumaan Singh -// -// 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. - -#ifndef LAZY_THETA_STAR_B__LAZY_THETA_STAR_H_ -#define LAZY_THETA_STAR_B__LAZY_THETA_STAR_H_ - -#include -#include -#include -#include -#include "rclcpp/rclcpp.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "nav2_costmap_2d/cost_values.hpp" - -typedef int id; -typedef double cost; -typedef int map_pts; -typedef double world_pts; - -#define INF_COST DBL_MAX -#define LETHAL_COST 252 - -template -struct coords -{ - coordsT x, y; -}; - -struct pos -{ - id pos_id; - cost * f; -}; - -struct tree_node -{ - map_pts x, y; - cost g = INF_COST; - cost h = INF_COST; - id parent_id; - int closed = 0; - cost f = INF_COST; -}; - -struct comp -{ - bool operator()(pos & p1, pos & p2) - { - return *(p1.f) > *(p2.f); - } -}; -namespace lazyThetaStar -{ -class LazyThetaStar -{ -public: - nav2_costmap_2d::Costmap2D * costmap_; - nav2_util::LifecycleNode::SharedPtr node_; - - std::vector posn; - std::vector data; - std::priority_queue, comp> pq; - int sizeX, sizeY; - int how_many_corners; - id id_gen; - coords src{}, dst{}; - int lethal_cost = LETHAL_COST; - - const map_pts moves[8][2] = {{0, 1}, - {0, -1}, - {1, 0}, - {-1, 0}, - {1, -1}, - {-1, 1}, - {1, 1}, - {-1, -1}}; - - LazyThetaStar(); - - bool getPath(std::vector> & raw_path); - - bool losCheck(map_pts & x0, map_pts & y0, map_pts & x1, map_pts & y1) const; - - cost dist(map_pts & ax, map_pts & ay, map_pts & bx, map_pts & by) - { - return sqrt(pow(ax - bx, 2) + pow(ay - by, 2)); - } - - // TODO (Anshu-man567) : FIND A WAY TO ADD REFERENCES TO THIS FUNCTION THAT IS IF POSSIBLE - bool isSafe(map_pts cx, map_pts cy) const - { - return costmap_->getCost(cx, cy) < lethal_cost; - } - - bool withinLimits(map_pts & cx, map_pts & cy) const - { - return cx >= 0 && cx < sizeX && cy >= 0 && cy < sizeY; - } - - void addIndex(map_pts cx, map_pts cy, id id_this) - { - posn[sizeX * cy + cx] = id_this; - } - - void getIndex(map_pts & cx, map_pts & cy, id & id_this) - { - id_this = posn[sizeX * cy + cx]; - } - void initializePosn(); - - void backtrace(std::vector> & raw_points, id & curr_id); - - bool isGoal(map_pts & cx, map_pts & cy); - - void initializeStuff(); - - void clearStuff(); - - void resetParent(tree_node & curr_data); - - void setNeighbors(tree_node & curr_data); -}; -} // namespace lazyThetaStar - -#endif // LAZY_THETA_STAR_B_LAZY_THETA_STAR_H_ From b5b705b95d819be49908c3e81ced943f97708772 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Mon, 28 Sep 2020 23:14:03 +0530 Subject: [PATCH 24/81] Delete lazy_theta_star_planner.h --- .../lazy_theta_star_planner.h | 75 ------------------- 1 file changed, 75 deletions(-) delete mode 100644 lazy_theta_star_planner/include/lazy_theta_star_planner/lazy_theta_star_planner.h diff --git a/lazy_theta_star_planner/include/lazy_theta_star_planner/lazy_theta_star_planner.h b/lazy_theta_star_planner/include/lazy_theta_star_planner/lazy_theta_star_planner.h deleted file mode 100644 index f0f7cfb95c..0000000000 --- a/lazy_theta_star_planner/include/lazy_theta_star_planner/lazy_theta_star_planner.h +++ /dev/null @@ -1,75 +0,0 @@ -// Copyright 2020 Anshumaan Singh -// -// 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. - -#ifndef LAZY_THETA_STAR_PLANNER__LAZY_THETA_STAR_PLANNER_H_ -#define LAZY_THETA_STAR_PLANNER__LAZY_THETA_STAR_PLANNER_H_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include "rclcpp/rclcpp.hpp" -#include "nav2_core/global_planner.hpp" -#include "nav_msgs/msg/path.hpp" -#include "nav2_util/robot_utils.hpp" -#include "nav2_util/lifecycle_node.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "nav2_costmap_2d/cost_values.hpp" -#include "nav2_util/node_utils.hpp" -#include "lazy_theta_star_planner/lazy_theta_star2.h" - -namespace lazyThetaStarPlanner -{ - -class LazyThetaStarPlanner : public nav2_core::GlobalPlanner -{ -public: - std::shared_ptr tf_; - nav2_util::LifecycleNode::SharedPtr node_; - nav2_costmap_2d::Costmap2D * costmap_; - - std::string globalFrame_, name_; - int how_many_corners_; - double interpolation_dist_; - int lethal_cost_; - std::unique_ptr planner_; - - unsigned int src_[2], dst_[2]; - - void configure( - rclcpp_lifecycle::LifecycleNode::SharedPtr parent, - std::string name, std::shared_ptr tf, - std::shared_ptr costmapRos) override; - - void cleanup() override; - - void activate() override; - - void deactivate() override; - - nav_msgs::msg::Path createPlan( - const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) override; - - nav_msgs::msg::Path linearInterpolation( - std::vector> & raw_path, - double dist_bw_points); -}; -} // namespace lazyThetaStarPlanner - -#endif // LAZY_THETA_STAR_PLANNER__LAZY_THETA_STAR_PLANNER_H_ From 3349e206ee1f74690e19932bee298b669d5e578a Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Mon, 28 Sep 2020 23:14:24 +0530 Subject: [PATCH 25/81] Delete lazy_theta_star2.cpp --- .../src/lazy_theta_star2.cpp | 257 ------------------ 1 file changed, 257 deletions(-) delete mode 100644 lazy_theta_star_planner/src/lazy_theta_star2.cpp diff --git a/lazy_theta_star_planner/src/lazy_theta_star2.cpp b/lazy_theta_star_planner/src/lazy_theta_star2.cpp deleted file mode 100644 index 11302b813b..0000000000 --- a/lazy_theta_star_planner/src/lazy_theta_star2.cpp +++ /dev/null @@ -1,257 +0,0 @@ -// Copyright 2020 Anshumaan Singh -// -// 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. - -#include "lazy_theta_star_planner/lazy_theta_star2.h" -#include - -namespace lazyThetaStar -{ - -LazyThetaStar::LazyThetaStar() -{ - sizeX = 0; - sizeY = 0; -} - -bool LazyThetaStar::getPath(std::vector> & raw_path) -{ - RCLCPP_INFO(node_->get_logger(), "Path Planning Begins...."); - - initializeStuff(); - - std::cout << src.x << '\t' << src.y << '\t' << dst.x << '\t' << dst.y << '\n'; - - data.push_back({src.x, src.y, 0, dist(src.x, src.y, dst.x, dst.y), - 0, 1, dist(src.x, src.y, dst.x, dst.y)}); - addIndex(src.x, src.y, id_gen); - pq.push({id_gen, &(data[id_gen].f)}); - - id_gen++; - id curr_id = 0; - - if (!losCheck(src.x, src.y, dst.x, dst.y)) { - while (!pq.empty()) { - - tree_node curr_data = data[curr_id]; - curr_data.closed = -1; - id curr_par = curr_data.parent_id; - - if (isGoal(curr_data.x, curr_data.y)) { - if (!(losCheck(curr_data.x, curr_data.y, data[curr_par].x, data[curr_par].y))) { - resetParent(curr_data); - } - break; - } - - if (!(losCheck(curr_data.x, curr_data.y, data[curr_par].x, data[curr_par].y))) { - resetParent(curr_data); - } - - setNeighbors(curr_data); - - curr_id = pq.top().pos_id; - pq.pop(); - } - if (pq.empty()) { - RCLCPP_INFO(node_->get_logger(), "No Path Found !!!!!!!!!!!"); - raw_path.clear(); - return false; - } - - } else { - RCLCPP_INFO(node_->get_logger(), "Straight Line Path Found!"); - data.push_back({dst.x, dst.y, dist(src.x, src.y, dst.x, dst.y), 0, 0, 1, - dist(src.x, src.y, dst.x, dst.y)}); - curr_id = id_gen; - } - - RCLCPP_INFO(node_->get_logger(), "REACHED DEST %i, %i", data[curr_id].x, data[curr_id].y); - - backtrace(raw_path, curr_id); - - return true; -} - -void LazyThetaStar::initializePosn() -{ - posn.clear(); - for (int i = 0; i < sizeX * sizeY; i++) { - posn.push_back(-1); - } -} - -bool LazyThetaStar::losCheck(map_pts & x0, map_pts & y0, map_pts & x1, map_pts & y1) const -{ - int dy = abs(y1 - y0), dx = abs(x1 - x0), f = 0; - int sx = x1 > x0 ? 1 : -1, sy = y1 > y0 ? 1 : -1; - int cx = x0, cy = y0; - int ux = (sx - 1) / 2, uy = (sy - 1) / 2; - - if (dx >= dy) { - while (cx != x1) { - f += dy; - if (f >= dx) { - if (!isSafe(cx + ux, cy + uy)) { - return false; - } - cy += sy; - f -= dx; - } - if (f != 0 && !isSafe(cx + ux, cy + uy)) { - return false; - } - if (dy == 0 && !isSafe(cx + ux, cy) && !isSafe(cx + ux, cy - 1)) { - return false; - } - cx += sx; - } - } else { - while (cy != y1) { - f = f + dx; - if (f >= dy) { - if (!isSafe(cx + ux, cy + uy)) { - return false; - } - cx += sx; - f -= dy; - } - if (f != 0 && !isSafe(cx + ux, cy + uy)) { - return false; - } - if (dx == 0 && !isSafe(cx, cy + uy) && !isSafe(cx - 1, cy + uy)) { - return false; - } - cy += sy; - } - } - return true; -} - -void LazyThetaStar::backtrace(std::vector> & raw_points, id & curr_id) -{ - std::vector> path_rev; - do { - coords world; - costmap_->mapToWorld(data[curr_id].x, data[curr_id].y, world.x, world.y); - path_rev.push_back({world.x, world.y}); - curr_id = data[curr_id].parent_id; - } while (curr_id != 0); - coords w; - costmap_->mapToWorld(data[curr_id].x, data[curr_id].y, w.x, w.y); - path_rev.push_back({w.x, w.y}); - - for (int i = path_rev.size() - 1; i >= 0; i--) { - raw_points.push_back(path_rev[i]); - } -} - -void LazyThetaStar::resetParent(tree_node & curr_data) -{ - map_pts mx, my; - id m_id; - cost min_dist = INF_COST; - id min_dist_id; - - for (int i = 0; i < how_many_corners; i++) { - mx = curr_data.x + moves[i][0]; - my = curr_data.y + moves[i][1]; - - if (withinLimits(mx, my)) { - getIndex(mx, my, m_id); - if (m_id != -1) { - if (data[m_id].f < min_dist) { - min_dist = data[m_id].f; - min_dist_id = m_id; - } - } - } - } - curr_data.parent_id = min_dist_id; - curr_data.g = data[min_dist_id].g + - dist(curr_data.x, curr_data.y, data[min_dist_id].x, data[min_dist_id].y); - curr_data.f = curr_data.g + curr_data.h; -} - -bool LazyThetaStar::isGoal(map_pts & cx, map_pts & cy) -{ - return cx == dst.x && cy == dst.y; -} -void LazyThetaStar::setNeighbors(tree_node & curr_data) -{ - - tree_node & curr_par = data[curr_data.parent_id]; - - map_pts mx, my; - id m_id; - for (int i = 0; i < how_many_corners; i++) { - mx = curr_data.x + moves[i][0]; - my = curr_data.y + moves[i][1]; - - if (mx == curr_par.x && my == curr_par.y) { - continue; - } - - if (withinLimits(mx, my)) { - cost g_cost = curr_par.g + dist(mx, my, curr_par.x, curr_par.y); - cost h_cost, cal_cost; - getIndex(mx, my, m_id); - - if (isSafe(mx, my) && m_id == -1) { - h_cost = dist(mx, my, dst.x, dst.y); - cal_cost = g_cost + h_cost; - data.push_back({mx, my, g_cost, h_cost, curr_data.parent_id, -1, cal_cost}); - pq.push({id_gen, &(data[id_gen].f)}); - addIndex(mx, my, id_gen); - id_gen++; - continue; - } else if (m_id != -1) { - tree_node & curr_node = data[m_id]; - h_cost = data[m_id].h; - cal_cost = g_cost + h_cost; - if (curr_node.f > cal_cost) { - curr_node.g = g_cost; - curr_node.f = cal_cost; - curr_node.parent_id = curr_data.parent_id; - if (curr_node.closed == 1) { - curr_node.closed = -1; - pq.push({m_id, &(data[m_id].f)}); - } - } - continue; - } - } - } -} - -void LazyThetaStar::initializeStuff() -{ - id_gen = 0; - if (costmap_->getCost(src.x, - src.y) > lethal_cost && costmap_->getCost(src.x, src.y) < LETHAL_COST) - { - lethal_cost = costmap_->getCost(src.x, src.y); - } - sizeX = costmap_->getSizeInCellsX(); - sizeY = costmap_->getSizeInCellsY(); - initializePosn(); - data.reserve(static_cast(sizeX * sizeY * 0.05)); -} - -void LazyThetaStar::clearStuff() -{ - data.clear(); - pq = std::priority_queue, comp>(); - posn.clear(); -} -} // namespace lazyThetaStar From 2d549e1a981e9545c446cb3faacc33bcb1c43556 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Mon, 28 Sep 2020 23:14:38 +0530 Subject: [PATCH 26/81] Delete lazy_theta_star_planner.cpp --- .../src/lazy_theta_star_planner.cpp | 153 ------------------ 1 file changed, 153 deletions(-) delete mode 100644 lazy_theta_star_planner/src/lazy_theta_star_planner.cpp diff --git a/lazy_theta_star_planner/src/lazy_theta_star_planner.cpp b/lazy_theta_star_planner/src/lazy_theta_star_planner.cpp deleted file mode 100644 index 9cb073a6db..0000000000 --- a/lazy_theta_star_planner/src/lazy_theta_star_planner.cpp +++ /dev/null @@ -1,153 +0,0 @@ -// Copyright 2020 Anshumaan Singh -// -// 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. - -#include -#include -#include -#include "lazy_theta_star_planner/lazy_theta_star_planner.h" -#include "lazy_theta_star_planner/lazy_theta_star2.h" - -namespace lazyThetaStarPlanner -{ -void LazyThetaStarPlanner::configure( - rclcpp_lifecycle::LifecycleNode::SharedPtr parent, - std::string name, std::shared_ptr tf, - std::shared_ptr costmapRos) -{ - planner_ = std::make_unique(); - node_ = parent; - name_ = name; - planner_->costmap_ = costmapRos->getCostmap(); - globalFrame_ = costmapRos->getGlobalFrameID(); - - nav2_util::declare_parameter_if_not_declared( - node_, name_ + ".how_many_corners", rclcpp::ParameterValue( - 8)); - - node_->get_parameter(name_ + ".how_many_corners", how_many_corners_); - - if (how_many_corners_ != 8) { - how_many_corners_ = 4; - } - - nav2_util::declare_parameter_if_not_declared( - node_, name_ + ".interpolation_dist", rclcpp::ParameterValue( - 0.1)); - - node_->get_parameter(name_ + ".interpolation_dist", interpolation_dist_); - - nav2_util::declare_parameter_if_not_declared( - node_, name_ + ".lethal_cost", rclcpp::ParameterValue( - 127)); - - node_->get_parameter(name_ + ".lethal_cost", lethal_cost_); -} - -void LazyThetaStarPlanner::cleanup() -{ - RCLCPP_INFO( - node_->get_logger(), "CleaningUp plugin %s of type lazyThetaStarPlanner", - name_.c_str()); -} - -void LazyThetaStarPlanner::activate() -{ - RCLCPP_INFO( - node_->get_logger(), "Activating plugin %s of type lazyThetaStarPlanner", - name_.c_str()); -} - -void LazyThetaStarPlanner::deactivate() -{ - RCLCPP_INFO( - node_->get_logger(), "Deactivating plugin %s of type lazyThetaStarPlanner", - name_.c_str()); -} - -nav_msgs::msg::Path LazyThetaStarPlanner::createPlan( - const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) -{ - nav_msgs::msg::Path global_path; - std::vector> path; - - planner_->costmap_->worldToMap(start.pose.position.x, start.pose.position.y, src_[0], src_[1]); - planner_->src = {static_cast(src_[0]), static_cast(src_[1])}; - - planner_->costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, dst_[0], dst_[1]); - planner_->dst = {static_cast(dst_[0]), static_cast(dst_[1])}; - - RCLCPP_INFO( - node_->get_logger(), "Got the src and dst... (%i, %i) && (%i, %i)", - planner_->src.x, planner_->src.y, planner_->dst.x, planner_->dst.y); - - planner_->node_ = node_; - planner_->how_many_corners = how_many_corners_; - - planner_->initializeStuff(); - planner_->getPath(path); - planner_->clearStuff(); - - RCLCPP_INFO(node_->get_logger(), "RECEIVED THE PATH"); - - if (path.size() > 1) { - for (int i = 0; i < path.size(); i++) { - std::cout << path[i].x << '\t' << path[i].y << '\n'; - } - - global_path = linearInterpolation(path, 0.1); - } else { - global_path.poses.clear(); - } - path.clear(); - std::cout << global_path.poses.size() << '\n'; - global_path.header.stamp = node_->now(); - global_path.header.frame_id = globalFrame_; - RCLCPP_INFO(node_->get_logger(), "SENT OUT THE PATH"); - return global_path; -} - -nav_msgs::msg::Path LazyThetaStarPlanner::linearInterpolation( - std::vector> & raw_path, - double dist_bw_points) -{ - nav_msgs::msg::Path pa; - - for (int j = 0; j < raw_path.size() - 1; j++) { - geometry_msgs::msg::PoseStamped p; - coords pt1 = {raw_path[j].x, raw_path[j].y}; - p.pose.position.x = pt1.x; - p.pose.position.y = pt1.y; - pa.poses.push_back(p); - - coords pt2 = {raw_path[j + 1].x, raw_path[j + 1].y}; - geometry_msgs::msg::PoseStamped p1; - double distance = std::hypot(pt2.x - pt1.x, pt2.y - pt1.y); - int loops = distance / dist_bw_points; - double sin_alpha = (pt2.y - pt1.y) / distance; - double cos_alpha = (pt2.x - pt1.x) / distance; - for (int k = 1; k < loops; k++) { - p1.pose.position.x = pt1.x + k * dist_bw_points * cos_alpha; - p1.pose.position.y = pt1.y + k * dist_bw_points * sin_alpha; - pa.poses.push_back(p1); - } - } - return pa; -} - -} // namespace lazyThetaStarPlanner - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS(lazyThetaStarPlanner::LazyThetaStarPlanner, nav2_core::GlobalPlanner) From 61d905e624ea0cf8ba8611fd2c65ff238baf419f Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Mon, 28 Sep 2020 23:14:49 +0530 Subject: [PATCH 27/81] Delete CMakeLists.txt --- lazy_theta_star_planner/CMakeLists.txt | 77 -------------------------- 1 file changed, 77 deletions(-) delete mode 100644 lazy_theta_star_planner/CMakeLists.txt diff --git a/lazy_theta_star_planner/CMakeLists.txt b/lazy_theta_star_planner/CMakeLists.txt deleted file mode 100644 index 3e77dd3e6f..0000000000 --- a/lazy_theta_star_planner/CMakeLists.txt +++ /dev/null @@ -1,77 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(lazy_theta_star_planner) - -find_package(ament_cmake REQUIRED) -find_package(builtin_interfaces REQUIRED) -find_package(nav2_common REQUIRED) -find_package(nav2_core REQUIRED) -find_package(nav2_costmap_2d REQUIRED) -find_package(nav2_msgs REQUIRED) -find_package(nav2_util REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(ament_cmake_gtest REQUIRED) - -include_directories( - include -) - -set(library_name ${PROJECT_NAME}_plugin) - -set(dependencies - ament_cmake - builtin_interfaces - nav2_common - nav2_core - nav2_costmap_2d - nav2_msgs - nav2_util - pluginlib - rclcpp - rclcpp_action - rclcpp_lifecycle - tf2_ros - ) - -add_library(${library_name} SHARED - src/lazy_theta_star2.cpp - src/lazy_theta_star_planner.cpp - ) - -ament_target_dependencies(${library_name} - ${dependencies} - ) - -target_compile_definitions(${library_name} PUBLIC "PLUGINLIB_DISABLE_BOOST_FUNCTIONS") - -pluginlib_export_plugin_description_file(nav2_core global_planner_plugin.xml) - -install(TARGETS ${library_name} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} - ) - -install(DIRECTORY include/ - DESTINATION include/ - ) - -install(FILES global_planner_plugin.xml - DESTINATION share/${PROJECT_NAME} - ) - - - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - - -ament_export_include_directories(include) -ament_export_libraries(${library_name}) -ament_export_dependencies(${dependencies}) -ament_package() From c70970a8411061b74d7b6167c6b267bb00108a46 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Mon, 28 Sep 2020 23:15:21 +0530 Subject: [PATCH 28/81] Delete package.xml --- lazy_theta_star_planner/package.xml | 31 ----------------------------- 1 file changed, 31 deletions(-) delete mode 100644 lazy_theta_star_planner/package.xml diff --git a/lazy_theta_star_planner/package.xml b/lazy_theta_star_planner/package.xml deleted file mode 100644 index 682a84b703..0000000000 --- a/lazy_theta_star_planner/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - lazy_theta_star_planner - 0.0.1 - TODO: Package description - anshu-man - Apache-2.0 - - ament_cmake - rclcpp - nav2_core - rclcpp_action - rclcpp_lifecycle - std_msgs - visualization_msgs - nav2_util - nav2_msgs - builtin_interfaces - tf2_ros - nav2_costmap_2d - pluginlib - - ament_lint_auto - ament_lint_common - - - ament_cmake - - - From ded0dc9616bfa8d8db88aaeb5e158a152b056af7 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Mon, 28 Sep 2020 23:15:29 +0530 Subject: [PATCH 29/81] Delete global_planner_plugin.xml --- lazy_theta_star_planner/global_planner_plugin.xml | 5 ----- 1 file changed, 5 deletions(-) delete mode 100644 lazy_theta_star_planner/global_planner_plugin.xml diff --git a/lazy_theta_star_planner/global_planner_plugin.xml b/lazy_theta_star_planner/global_planner_plugin.xml deleted file mode 100644 index f6f6ef3288..0000000000 --- a/lazy_theta_star_planner/global_planner_plugin.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - The implementation of lazy theta_star - - \ No newline at end of file From 6cf939154cf911abc2e6648d5a69866e2f60e1c3 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Mon, 28 Sep 2020 23:24:42 +0530 Subject: [PATCH 30/81] upload the changed code Changes from last time are: - the code has been changed to the Lazy Theta* P variant, in order to account for the costmap traversal costs - parameters are available to change the weights of the costmap traversal cost (weight = 1.75, as of now) and the distance function (weight = 1.0, as of now - --- lazy_theta_star_p_planner/CMakeLists.txt | 79 ++++++ .../global_planner_plugin.xml | 5 + .../lazy_theta_star_p.hpp | 237 ++++++++++++++++ .../lazy_theta_star_p_planner.hpp | 80 ++++++ lazy_theta_star_p_planner/package.xml | 30 ++ .../src/lazy_theta_star_p.cpp | 262 ++++++++++++++++++ .../src/lazy_theta_star_p_planner.cpp | 190 +++++++++++++ 7 files changed, 883 insertions(+) create mode 100644 lazy_theta_star_p_planner/CMakeLists.txt create mode 100644 lazy_theta_star_p_planner/global_planner_plugin.xml create mode 100644 lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p.hpp create mode 100644 lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p_planner.hpp create mode 100644 lazy_theta_star_p_planner/package.xml create mode 100644 lazy_theta_star_p_planner/src/lazy_theta_star_p.cpp create mode 100644 lazy_theta_star_p_planner/src/lazy_theta_star_p_planner.cpp diff --git a/lazy_theta_star_p_planner/CMakeLists.txt b/lazy_theta_star_p_planner/CMakeLists.txt new file mode 100644 index 0000000000..d9aaafe6a3 --- /dev/null +++ b/lazy_theta_star_p_planner/CMakeLists.txt @@ -0,0 +1,79 @@ +cmake_minimum_required(VERSION 3.5) +project(lazy_theta_star_p_planner) + +set(CMAKE_BUILD_TYPE=Release) +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav2_util REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(ament_cmake_gtest REQUIRED) + + +nav2_package() + +include_directories( + include +) + +set(library_name ${PROJECT_NAME}_plugin) + +set(dependencies + ament_cmake + builtin_interfaces + nav2_common + nav2_core + nav2_costmap_2d + nav2_msgs + nav2_util + pluginlib + rclcpp + rclcpp_action + rclcpp_lifecycle + tf2_ros + ) + + +add_library(${library_name} SHARED + src/lazy_theta_star_p.cpp + src/lazy_theta_star_p_planner.cpp + ) + +ament_target_dependencies(${library_name} ${dependencies} ) + +target_compile_definitions(${library_name} PUBLIC "PLUGINLIB_DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file(nav2_core global_planner_plugin.xml) + +install(TARGETS ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} + ) + +install(DIRECTORY include/ + DESTINATION include/ + ) + +install(FILES global_planner_plugin.xml + DESTINATION share/${PROJECT_NAME} + ) + + +if (BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif () + + +ament_export_include_directories(include) +ament_export_libraries(${library_name}) +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/lazy_theta_star_p_planner/global_planner_plugin.xml b/lazy_theta_star_p_planner/global_planner_plugin.xml new file mode 100644 index 0000000000..aa9f9da961 --- /dev/null +++ b/lazy_theta_star_p_planner/global_planner_plugin.xml @@ -0,0 +1,5 @@ + + + this is empty + + diff --git a/lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p.hpp b/lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p.hpp new file mode 100644 index 0000000000..74188f30b9 --- /dev/null +++ b/lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p.hpp @@ -0,0 +1,237 @@ +// Copyright 2020 Anshumaan Singh +// +// 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. + +#ifndef LAZY_THETA_STAR_P_PLANNER__LAZY_THETA_STAR_P_HPP_ +#define LAZY_THETA_STAR_P_PLANNER__LAZY_THETA_STAR_P_HPP_ + +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + +typedef int id; +typedef double cost; +typedef int map_pts; +typedef double world_pts; +const double INF_COST = DBL_MAX; +const int LETHAL_COST = 252; + +struct coordsM +{ + map_pts x, y; +}; + +struct coordsW +{ + world_pts x, y; +}; + +struct pos +{ + id pos_id; + cost f; +}; + +struct tree_node +{ + map_pts x, y; + cost g = INF_COST; + cost h = INF_COST; + id parent_id; + bool is_in_queue = false; + cost f = INF_COST; +}; + +struct comp +{ + bool operator()(pos & p1, pos & p2) + { + return (p1.f) > (p2.f); + } +}; + +namespace lazyThetaStarP +{ +class LazyThetaStarP +{ +private: + // for the coordinates (x,y), we store at node_position[size_x * y + x], + // the index at which the data of the node is present in nodes_data + std::vector node_position; + + // the vector nodes_data stores the coordinates, costs and index of the parent node, + // and whether or not the node is present in queue_ + std::vector nodes_data; + + // this is the priority queue to select the next node for expansion + std::priority_queue, comp> queue_; + + + // it is a counter like variable used to give out the index + // at which data will be stored for a node that is being expanded + id index_generated; + + /// CAN BE ADDED : + /// could make it a linear array of id's, if the coordinates are also going to be + /// denoted by an index -> size_x * y_coord + x_coord + /// this would help to reduce the number of elements in the tree_node struct and slightly faster + /// resetting of the values + const coordsM moves[8] = {{0, 1}, + {0, -1}, + {1, 0}, + {-1, 0}, + {1, -1}, + {-1, 1}, + {1, 1}, + {-1, -1}}; + + /** @brief it does a line of sight (los) check between the current node and the parent of its parent node + * if an los is found and the new costs calculated are lesser then the cost and parent node of the current node + * is updated + * @param data of the current node + */ + void resetParent(tree_node & curr_data); + + /** + * @brief this function expands the neighbors of the current node + * @param curr_data used to send the data of the current node + * @param curr_id used to send the index of the current node as stored in nodes_position + */ + void setNeighbors(const tree_node & curr_data, const id & curr_id); + + /** + * @brief it returns the path by backtracing from the goal to the start, by using their parent nodes + * @param raw_points used to return the path thus found + * @param curr_id sends in the index of the goal coordinate, as stored in nodes_position + */ + void backtrace(std::vector & raw_points, id curr_id); + + /** + * @brief performs the line of sight check using Bresenham's Algorithm, + * and has been modified to calculate the traversal cost incurred in a straight line path between + * the two points whose coordinates are (x0, y0) and (x1, y1) + * @param sl_cost is used to return the cost thus incurred + * @return true if a line of sight exists between the points + */ + bool losCheck( + const map_pts & x0, const map_pts & y0, const map_pts & x1, const map_pts & y1, + cost & sl_cost) const; + + cost dist(const map_pts & ax, const map_pts & ay, const map_pts & bx, const map_pts & by) + { + return std::hypot(ax - bx, ay - by); + } + + /** + * @brief for the point(cx, cy) its traversal cost is calculated by *()^2/()^2 + * @return the traversal cost thus calculated + */ + cost getCost(const cost & cx, const cost & cy) const + { + return costmap_tolerance_ * + costmap_->getCost(cx, cy) * costmap_->getCost(cx, cy) / LETHAL_COST / LETHAL_COST; + } + + void addIndex(const map_pts & cx, const map_pts & cy, const id & id_this) + { + node_position[size_x * cy + cx] = id_this; + } + + void getIndex(const map_pts & cx, const map_pts & cy, id & id_this) + { + id_this = node_position[size_x * cy + cx]; + } + + bool withinLimits(const map_pts & cx, const map_pts & cy) const + { + return cx >= 0 && cx < size_x && cy >= 0 && cy < size_y; + } + + bool isGoal(const map_pts & cx, const map_pts & cy) const + { + return cx == dst.x && cy == dst.y; + } + + cost maxCost(const cost & a, const cost & b) const + { + if (a > b) {return a; } else {return b; } + } + + void addToNodesData(const id & id_this) + { + if (nodes_data.size() <= static_cast(id_this)) { + nodes_data.push_back({}); + } else { + nodes_data[id_this] = {}; + } + } + + void clearQueue() + { + while (!queue_.empty()) { + queue_.pop(); + } + } + + void initializePosn(int size_inc = 0) + { + int i = 0; + if (!node_position.empty()) { + for (; i < size_x * size_y; i++) { + node_position[i] = -1; + } + } + for (; i < size_inc; i++) { + node_position.emplace_back(-1); + } + } + + void setContainers(); + +public: + + coordsM src{}, dst{}; + nav2_costmap_2d::Costmap2D * costmap_{}; + nav2_util::LifecycleNode::SharedPtr node_; + + int how_many_corners_; + int size_x, size_y; + + // parameter for cost of costmap traversal + cost costmap_tolerance_; + // parameter for distance function used for heurestic + cost euc_tolerance_2_; + // parameter for distance function used for distance function cost from the source to the current node + cost euc_tolerance_; + + LazyThetaStarP(); + + /** + * @brief the function that iteratively searces upon the nodes in the queue (open list) until the + * current node is the goal + * @param raw_path is used to return the path obtained on exectuing the algorithm + * @return true if a path is found, false if no path is found + */ + bool generatePath(std::vector & raw_path); + + bool isSafe(const map_pts & cx, const map_pts & cy) const + { + return costmap_->getCost(cx, cy) < LETHAL_COST; + } +}; +} // namespace lazyThetaStarP + +#endif // LAZY_THETA_STAR_P_PLANNER__LAZY_THETA_STAR_P_HPP_ diff --git a/lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p_planner.hpp b/lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p_planner.hpp new file mode 100644 index 0000000000..4dcf969010 --- /dev/null +++ b/lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p_planner.hpp @@ -0,0 +1,80 @@ +// Copyright 2020 Anshumaan Singh +// +// 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. + +#ifndef LAZY_THETA_STAR_P_PLANNER__LAZY_THETA_STAR_P_PLANNER_HPP_ +#define LAZY_THETA_STAR_P_PLANNER__LAZY_THETA_STAR_P_PLANNER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "nav2_core/global_planner.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_util/node_utils.hpp" +#include "lazy_theta_star_p_planner/lazy_theta_star_p.hpp" + +namespace lazyThetaStarP_Planner +{ + +class LazyThetaStarP_Planner : public nav2_core::GlobalPlanner +{ +public: + std::shared_ptr tf_; + nav2_util::LifecycleNode::SharedPtr node_; + + std::string globalFrame_, name_; + + void configure( + rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmapRos) override; + + void cleanup() override; + + void activate() override; + + void deactivate() override; + + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) override; + +private: + double interpolation_dist_; + std::unique_ptr planner_; + + void getPlan(nav_msgs::msg::Path & global_path); + + void setStartAndGoal( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal); + + + static nav_msgs::msg::Path linearInterpolation( + const std::vector & raw_path, + const double & dist_bw_points); + +}; +} // namespace lazyThetaStarP_Planner + +#endif // LAZY_THETA_STAR_P_PLANNER__LAZY_THETA_STAR_P_PLANNER_HPP_ diff --git a/lazy_theta_star_p_planner/package.xml b/lazy_theta_star_p_planner/package.xml new file mode 100644 index 0000000000..db18a82b9e --- /dev/null +++ b/lazy_theta_star_p_planner/package.xml @@ -0,0 +1,30 @@ + + + + lazy_theta_star_p_planner + 0.4.0 + TODO: Package description + Steve Macenski + Anshumaan Singh + Apache-2.0 + ament_cmake + rclcpp + nav2_core + rclcpp_action + rclcpp_lifecycle + std_msgs + visualization_msgs + nav2_util + nav2_msgs + builtin_interfaces + tf2_ros + nav2_costmap_2d + pluginlib + ament_lint_auto + ament_lint_common + + ament_cmake + + + + diff --git a/lazy_theta_star_p_planner/src/lazy_theta_star_p.cpp b/lazy_theta_star_p_planner/src/lazy_theta_star_p.cpp new file mode 100644 index 0000000000..f4bf8900d2 --- /dev/null +++ b/lazy_theta_star_p_planner/src/lazy_theta_star_p.cpp @@ -0,0 +1,262 @@ +// Copyright 2020 Anshumaan Singh +// +// 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. + +#include "lazy_theta_star_p_planner/lazy_theta_star_p.hpp" +#include + +namespace lazyThetaStarP +{ + +LazyThetaStarP::LazyThetaStarP() +{ + costmap_tolerance_ = 1.75; + euc_tolerance_2_ = 1.0; + euc_tolerance_ = 1.0; + size_x = 0; + size_y = 0; + index_generated = 0; +} + +bool LazyThetaStarP::generatePath(std::vector & raw_path) +{ + setContainers(); + RCLCPP_INFO(node_->get_logger(), "Path Planning Begins... "); + + index_generated = 0; + id curr_id = index_generated; + addToNodesData(index_generated); + nodes_data[curr_id] = {src.x, src.y, getCost(src.x, src.y), dist(src.x, src.y, dst.x, dst.y), + curr_id, true, dist(src.x, src.y, dst.x, dst.y) + (getCost(src.x, src.y))}; + queue_.push({curr_id, (nodes_data[curr_id].f)}); + addIndex(nodes_data[curr_id].x, nodes_data[curr_id].y, index_generated); + index_generated++; + + int nodes_opened = 0; + while (!queue_.empty()) { + nodes_opened++; + + if (isGoal(nodes_data[curr_id].x, nodes_data[curr_id].y)) { + break; + } + tree_node & curr_data = nodes_data[curr_id]; + resetParent(curr_data); + setNeighbors(curr_data, curr_id); + + curr_id = queue_.top().pos_id; + queue_.pop(); + } + + if (queue_.empty()) { + RCLCPP_INFO(node_->get_logger(), "No Path Found !!!!!!!!!!!"); + raw_path.clear(); + return false; + } + + std::cout << "the number of executions are: " << nodes_opened << '\n'; + RCLCPP_INFO(node_->get_logger(), + "REACHED DEST %i, %i --- %f", + nodes_data[curr_id].x, + nodes_data[curr_id].y, + nodes_data[curr_id].g); + backtrace(raw_path, curr_id); + clearQueue(); + return true; +} + +void LazyThetaStarP::resetParent(tree_node & curr_data) +{ + cost g_cost, cal_cost, los_cost = 0; + curr_data.is_in_queue = false; + tree_node & curr_par = nodes_data[curr_data.parent_id]; + tree_node & maybe_par = nodes_data[curr_par.parent_id]; + + if (losCheck(curr_data.x, curr_data.y, maybe_par.x, maybe_par.y, los_cost)) { + g_cost = maybe_par.g + + dist(curr_data.x, curr_data.y, maybe_par.x, maybe_par.y) * euc_tolerance_ + + los_cost; + cal_cost = g_cost + curr_data.h; + if (cal_cost < curr_data.f) { + curr_data.parent_id = curr_par.parent_id; + curr_data.g = g_cost; + curr_data.f = cal_cost; + } + } +} + +void LazyThetaStarP::setNeighbors(const tree_node & curr_data, const id & curr_id) +{ + map_pts mx, my; + id m_id, m_par; + cost g_cost, h_cost, cal_cost; + tree_node * curr_node; + + const tree_node & curr_par = nodes_data[curr_data.parent_id]; + for (auto & move : moves) { + mx = curr_data.x + move.x; + my = curr_data.y + move.y; + + if (mx == curr_par.x && my == curr_par.y) { + continue; + } + + if (withinLimits(mx, my)) { + if (!isSafe(mx, my)) { + continue; + } + } else { + continue; + } + + m_par = curr_id; + g_cost = curr_data.g + dist(curr_data.x, curr_data.y, mx, my) * euc_tolerance_ + + getCost(mx, my); + + getIndex(mx, my, m_id); + + if (m_id == -1) { + addToNodesData(index_generated); + m_id = index_generated; + addIndex(mx, my, m_id); + index_generated++; + } + + curr_node = &nodes_data[m_id]; + + h_cost = dist(mx, my, dst.x, dst.y) * euc_tolerance_2_; + cal_cost = g_cost + h_cost; + + if (curr_node->f > cal_cost) { + curr_node->g = g_cost; + curr_node->h = h_cost; + curr_node->f = cal_cost; + curr_node->parent_id = m_par; + if (!curr_node->is_in_queue) { + curr_node->x = mx; + curr_node->y = my; + curr_node->is_in_queue = true; + queue_.push({m_id, (curr_node->f)}); + } + } + } +} + +void LazyThetaStarP::backtrace(std::vector & raw_points, id curr_id) +{ + std::vector path_rev; + coordsW world{}; + double pusher_ = 0.5 * costmap_->getResolution(); + do { + costmap_->mapToWorld(nodes_data[curr_id].x, nodes_data[curr_id].y, world.x, world.y); + path_rev.push_back({world.x + pusher_, world.y + pusher_}); + curr_id = nodes_data[curr_id].parent_id; + } while (curr_id != 0); + costmap_->mapToWorld(nodes_data[curr_id].x, nodes_data[curr_id].y, world.x, world.y); + path_rev.push_back({world.x + pusher_, world.y + pusher_}); + + raw_points.reserve(path_rev.size()); + for (int i = static_cast(path_rev.size()) - 1; i >= 0; i--) { + raw_points.push_back(path_rev[i]); + } +} + +bool LazyThetaStarP::losCheck( + const map_pts & x0, + const map_pts & y0, + const map_pts & x1, + const map_pts & y1, + cost & sl_cost) const +{ + sl_cost = 0; + const int dy = abs(y1 - y0); + const int dx = abs(x1 - x0); + const int sx = x1 > x0 ? 1 : -1; + const int sy = y1 > y0 ? 1 : -1; + const int u_x = (sx - 1) / 2; + const int u_y = (sy - 1) / 2; + int cx, cy, f; + cx = x0; + cy = y0; + f = 0; + + if (dx >= dy) { + if (dy != 0) { + for (; cx != x1; cx += sx) { + f += dy; + if (f >= dx) { + if (!isSafe(cx + u_x, cy + u_y)) { + return false; + } + sl_cost += getCost(cx + u_x, cy + u_y); + cy += sy; + f -= dx; + } + if (f != 0 && !isSafe(cx + u_x, cy + u_y)) { + return false; + } + sl_cost += getCost(cx + u_x, cy + u_y); + } + } else { + for (cx = x0; cx < x1; cx += sx) { + if (!isSafe(cx + u_x, cy) || !isSafe(cx + u_x, cy - 1)) { + return false; + } + sl_cost += maxCost(getCost(cx + u_x, cy), getCost(cx + u_x, cy - 1)); + } + } + } else { + if (dx != 0) { + for (; cy != y1; cy += sy) { + f = f + dx; + if (f >= dy) { + if (!isSafe(cx + u_x, cy + u_y)) { + return false; + } + sl_cost += getCost(cx + u_x, cy + u_y); + cx += sx; + f -= dy; + } + if (f != 0 && !isSafe(cx + u_x, cy + u_y)) { + return false; + } + sl_cost += getCost(cx + u_x, cy + u_y); + } + } else { + for (; cy != y1; cy += sy) { + if (!isSafe(cx, cy + u_y) || !isSafe(cx - 1, cy + u_y)) { + return false; + } + sl_cost += maxCost(getCost(cx, cy + u_y), getCost(cx - 1, cy + u_y)); + } + } + } + return true; +} + +void LazyThetaStarP::setContainers() +{ + index_generated = 0; + int last_size_x = size_x; + int last_size_y = size_y; + int curr_size_x = static_cast(costmap_->getSizeInCellsX()); + int curr_size_y = static_cast(costmap_->getSizeInCellsY()); + if (last_size_x != curr_size_x || last_size_y != curr_size_y) { + initializePosn(curr_size_y * curr_size_x - last_size_y * last_size_x); + nodes_data.reserve(curr_size_x * curr_size_y); + } else { + initializePosn(); + } + size_x = curr_size_x; + size_y = curr_size_y; +} +} // namespace lazyThetaStarP diff --git a/lazy_theta_star_p_planner/src/lazy_theta_star_p_planner.cpp b/lazy_theta_star_p_planner/src/lazy_theta_star_p_planner.cpp new file mode 100644 index 0000000000..338b43a160 --- /dev/null +++ b/lazy_theta_star_p_planner/src/lazy_theta_star_p_planner.cpp @@ -0,0 +1,190 @@ +// Copyright 2020 Anshumaan Singh +// +// 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. + +#include +#include +#include +#include "lazy_theta_star_p_planner/lazy_theta_star_p_planner.hpp" +#include "lazy_theta_star_p_planner/lazy_theta_star_p.hpp" + +namespace lazyThetaStarP_Planner +{ +void LazyThetaStarP_Planner::configure( + rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmapRos) +{ + planner_ = std::make_unique(); + node_ = parent; + name_ = name; + tf_ = tf; + planner_->costmap_ = costmapRos->getCostmap(); + globalFrame_ = costmapRos->getGlobalFrameID(); + + nav2_util::declare_parameter_if_not_declared( + node_, name_ + ".how_many_corners", rclcpp::ParameterValue( + 8)); + + node_->get_parameter(name_ + ".how_many_corners", planner_->how_many_corners_); + + if (planner_->how_many_corners_ != 8 && planner_->how_many_corners_ != 4) { + planner_->how_many_corners_ = 8; + RCLCPP_WARN(node_->get_logger(), + "Your value for - .how_many_coreners was overriden, and is now set to 8"); + } + + + nav2_util::declare_parameter_if_not_declared( + node_, name_ + ".interpolation_dist", rclcpp::ParameterValue( + 0.1)); + + node_->get_parameter(name_ + ".interpolation_dist", interpolation_dist_); + +/// could be added in the future verions if required, this +/// includes the parameters for lethal_cost, parameters for distance function and the costmap traversal costs +// nav2_util::declare_parameter_if_not_declared( +// node_, name_ + ".lethal_cost", rclcpp::ParameterValue( +// 252)); +// +// node_->get_parameter(name_ + ".lethal_cost", lethal_cost_); +// +// nav2_util::declare_parameter_if_not_declared( +// node_, name_ + ".euc_tolerance", rclcpp::ParameterValue(1.0)); +// +// node_->get_parameter(name_ + ".euc_tolerance", planner_->euc_tolerance_); +// +// nav2_util::declare_parameter_if_not_declared( +// node_, name_ + ".costmap_tolerance", rclcpp::ParameterValue(1.0)); +// +// node_->get_parameter(name_ + ".costmap_tolerance", planner_->costmap_tolerance_); +} + +void LazyThetaStarP_Planner::cleanup() +{ + RCLCPP_INFO( + node_->get_logger(), "CleaningUp plugin %s of type LazyThetaStarP_Planner", + name_.c_str()); +} + +void LazyThetaStarP_Planner::activate() +{ + RCLCPP_INFO( + node_->get_logger(), "Activating plugin %s of type LazyThetaStarP_Planner", + name_.c_str()); +} + +void LazyThetaStarP_Planner::deactivate() +{ + RCLCPP_INFO( + node_->get_logger(), "Deactivating plugin %s of type LazyThetaStarP_Planner", + name_.c_str()); +} + +nav_msgs::msg::Path LazyThetaStarP_Planner::createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) +{ + nav_msgs::msg::Path global_path; + setStartAndGoal(start, goal); + RCLCPP_INFO( + node_->get_logger(), "Got the src and dst... (%i, %i) && (%i, %i)", + planner_->src.x, planner_->src.y, planner_->dst.x, planner_->dst.y); + getPlan(global_path); + return global_path; +} + +void LazyThetaStarP_Planner::getPlan(nav_msgs::msg::Path & global_path) +{ + planner_->node_ = node_; + + /// NOTE : a few lines of code is to be removed in the final version + + std::vector path; + auto start = std::chrono::steady_clock::now(); + if (!(planner_->isSafe(planner_->src.x, + planner_->src.y)) || !(planner_->isSafe(planner_->dst.x, planner_->dst.y))) + { +// RCLCPP_INFO(node_->get_logger(), "EITHER OF THE START OR GOAL POINTS ARE AN OBSTACLE! "); +// coordsW world{}; +// planner_->costmap_->mapToWorld(planner_->src.x, planner_->src.y, world.x, world.y); +// path.push_back({world.x, world.y}); +// planner_->costmap_->mapToWorld(planner_->dst.x, planner_->dst.y, world.x, world.y); +// path.push_back({world.x, world.y}); +// global_path = linearInterpolation(path, interpolation_dist_); + global_path.poses.clear(); + } else if (planner_->generatePath(path)) { + RCLCPP_INFO(node_->get_logger(), "RECEIVED THE PATH"); + global_path = linearInterpolation(path, interpolation_dist_); + } else { + global_path.poses.clear(); + } + + auto stop = std::chrono::steady_clock::now(); + auto dur = std::chrono::duration_cast(stop - start); + std::cout << "the time taken in microseconds is \t\t : " << dur.count() << '\n'; + path.clear(); + std::cout << global_path.poses.size() << '\n'; + global_path.header.stamp = node_->now(); + global_path.header.frame_id = globalFrame_; + RCLCPP_INFO(node_->get_logger(), "SENT OUT THE PATH"); + + RCLCPP_INFO(node_->get_logger(), "the cost at src : %i", + planner_->costmap_->getCost(planner_->src.x, planner_->src.y)); + RCLCPP_INFO(node_->get_logger(), "the cost at dst : %i", + planner_->costmap_->getCost(planner_->dst.x, planner_->dst.y)); +} + +nav_msgs::msg::Path LazyThetaStarP_Planner::linearInterpolation( + const std::vector & raw_path, + const double & dist_bw_points) +{ + nav_msgs::msg::Path pa; + + for (unsigned int j = 0; j < raw_path.size() - 1; j++) { + geometry_msgs::msg::PoseStamped p; + coordsW pt1 = {raw_path[j].x, raw_path[j].y}; + p.pose.position.x = pt1.x; + p.pose.position.y = pt1.y; + pa.poses.push_back(p); + + coordsW pt2 = {raw_path[j + 1].x, raw_path[j + 1].y}; + geometry_msgs::msg::PoseStamped p1; + double distance = std::hypot(pt2.x - pt1.x, pt2.y - pt1.y); + int loops = static_cast(distance / dist_bw_points); + double sin_alpha = (pt2.y - pt1.y) / distance; + double cos_alpha = (pt2.x - pt1.x) / distance; + for (int k = 1; k < loops; k++) { + p1.pose.position.x = pt1.x + k * dist_bw_points * cos_alpha; + p1.pose.position.y = pt1.y + k * dist_bw_points * sin_alpha; + pa.poses.push_back(p1); + } + } + return pa; +} +void LazyThetaStarP_Planner::setStartAndGoal( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) +{ + unsigned int src_[2], dst_[2]; + planner_->costmap_->worldToMap(start.pose.position.x, start.pose.position.y, src_[0], src_[1]); + planner_->costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, dst_[0], dst_[1]); + + planner_->src = {static_cast(src_[0]), static_cast(src_[1])}; + planner_->dst = {static_cast(dst_[0]), static_cast(dst_[1])}; +} + +} // namespace lazyThetaStarP_Planner + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(lazyThetaStarP_Planner::LazyThetaStarP_Planner, nav2_core::GlobalPlanner) From 81367d13cefc8744799ad4c217c0708ed48d5685 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Thu, 14 Jan 2021 03:20:25 +0530 Subject: [PATCH 31/81] Delete lazy_theta_star_p_planner directory --- lazy_theta_star_p_planner/CMakeLists.txt | 79 ------ .../global_planner_plugin.xml | 5 - .../lazy_theta_star_p.hpp | 237 ---------------- .../lazy_theta_star_p_planner.hpp | 80 ------ lazy_theta_star_p_planner/package.xml | 30 -- .../src/lazy_theta_star_p.cpp | 262 ------------------ .../src/lazy_theta_star_p_planner.cpp | 190 ------------- 7 files changed, 883 deletions(-) delete mode 100644 lazy_theta_star_p_planner/CMakeLists.txt delete mode 100644 lazy_theta_star_p_planner/global_planner_plugin.xml delete mode 100644 lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p.hpp delete mode 100644 lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p_planner.hpp delete mode 100644 lazy_theta_star_p_planner/package.xml delete mode 100644 lazy_theta_star_p_planner/src/lazy_theta_star_p.cpp delete mode 100644 lazy_theta_star_p_planner/src/lazy_theta_star_p_planner.cpp diff --git a/lazy_theta_star_p_planner/CMakeLists.txt b/lazy_theta_star_p_planner/CMakeLists.txt deleted file mode 100644 index d9aaafe6a3..0000000000 --- a/lazy_theta_star_p_planner/CMakeLists.txt +++ /dev/null @@ -1,79 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(lazy_theta_star_p_planner) - -set(CMAKE_BUILD_TYPE=Release) -find_package(ament_cmake REQUIRED) -find_package(builtin_interfaces REQUIRED) -find_package(nav2_common REQUIRED) -find_package(nav2_core REQUIRED) -find_package(nav2_costmap_2d REQUIRED) -find_package(nav2_msgs REQUIRED) -find_package(nav2_util REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(ament_cmake_gtest REQUIRED) - - -nav2_package() - -include_directories( - include -) - -set(library_name ${PROJECT_NAME}_plugin) - -set(dependencies - ament_cmake - builtin_interfaces - nav2_common - nav2_core - nav2_costmap_2d - nav2_msgs - nav2_util - pluginlib - rclcpp - rclcpp_action - rclcpp_lifecycle - tf2_ros - ) - - -add_library(${library_name} SHARED - src/lazy_theta_star_p.cpp - src/lazy_theta_star_p_planner.cpp - ) - -ament_target_dependencies(${library_name} ${dependencies} ) - -target_compile_definitions(${library_name} PUBLIC "PLUGINLIB_DISABLE_BOOST_FUNCTIONS") - -pluginlib_export_plugin_description_file(nav2_core global_planner_plugin.xml) - -install(TARGETS ${library_name} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} - ) - -install(DIRECTORY include/ - DESTINATION include/ - ) - -install(FILES global_planner_plugin.xml - DESTINATION share/${PROJECT_NAME} - ) - - -if (BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif () - - -ament_export_include_directories(include) -ament_export_libraries(${library_name}) -ament_export_dependencies(${dependencies}) -ament_package() diff --git a/lazy_theta_star_p_planner/global_planner_plugin.xml b/lazy_theta_star_p_planner/global_planner_plugin.xml deleted file mode 100644 index aa9f9da961..0000000000 --- a/lazy_theta_star_p_planner/global_planner_plugin.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - this is empty - - diff --git a/lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p.hpp b/lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p.hpp deleted file mode 100644 index 74188f30b9..0000000000 --- a/lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p.hpp +++ /dev/null @@ -1,237 +0,0 @@ -// Copyright 2020 Anshumaan Singh -// -// 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. - -#ifndef LAZY_THETA_STAR_P_PLANNER__LAZY_THETA_STAR_P_HPP_ -#define LAZY_THETA_STAR_P_PLANNER__LAZY_THETA_STAR_P_HPP_ - -#include -#include -#include -#include -#include "rclcpp/rclcpp.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" - -typedef int id; -typedef double cost; -typedef int map_pts; -typedef double world_pts; -const double INF_COST = DBL_MAX; -const int LETHAL_COST = 252; - -struct coordsM -{ - map_pts x, y; -}; - -struct coordsW -{ - world_pts x, y; -}; - -struct pos -{ - id pos_id; - cost f; -}; - -struct tree_node -{ - map_pts x, y; - cost g = INF_COST; - cost h = INF_COST; - id parent_id; - bool is_in_queue = false; - cost f = INF_COST; -}; - -struct comp -{ - bool operator()(pos & p1, pos & p2) - { - return (p1.f) > (p2.f); - } -}; - -namespace lazyThetaStarP -{ -class LazyThetaStarP -{ -private: - // for the coordinates (x,y), we store at node_position[size_x * y + x], - // the index at which the data of the node is present in nodes_data - std::vector node_position; - - // the vector nodes_data stores the coordinates, costs and index of the parent node, - // and whether or not the node is present in queue_ - std::vector nodes_data; - - // this is the priority queue to select the next node for expansion - std::priority_queue, comp> queue_; - - - // it is a counter like variable used to give out the index - // at which data will be stored for a node that is being expanded - id index_generated; - - /// CAN BE ADDED : - /// could make it a linear array of id's, if the coordinates are also going to be - /// denoted by an index -> size_x * y_coord + x_coord - /// this would help to reduce the number of elements in the tree_node struct and slightly faster - /// resetting of the values - const coordsM moves[8] = {{0, 1}, - {0, -1}, - {1, 0}, - {-1, 0}, - {1, -1}, - {-1, 1}, - {1, 1}, - {-1, -1}}; - - /** @brief it does a line of sight (los) check between the current node and the parent of its parent node - * if an los is found and the new costs calculated are lesser then the cost and parent node of the current node - * is updated - * @param data of the current node - */ - void resetParent(tree_node & curr_data); - - /** - * @brief this function expands the neighbors of the current node - * @param curr_data used to send the data of the current node - * @param curr_id used to send the index of the current node as stored in nodes_position - */ - void setNeighbors(const tree_node & curr_data, const id & curr_id); - - /** - * @brief it returns the path by backtracing from the goal to the start, by using their parent nodes - * @param raw_points used to return the path thus found - * @param curr_id sends in the index of the goal coordinate, as stored in nodes_position - */ - void backtrace(std::vector & raw_points, id curr_id); - - /** - * @brief performs the line of sight check using Bresenham's Algorithm, - * and has been modified to calculate the traversal cost incurred in a straight line path between - * the two points whose coordinates are (x0, y0) and (x1, y1) - * @param sl_cost is used to return the cost thus incurred - * @return true if a line of sight exists between the points - */ - bool losCheck( - const map_pts & x0, const map_pts & y0, const map_pts & x1, const map_pts & y1, - cost & sl_cost) const; - - cost dist(const map_pts & ax, const map_pts & ay, const map_pts & bx, const map_pts & by) - { - return std::hypot(ax - bx, ay - by); - } - - /** - * @brief for the point(cx, cy) its traversal cost is calculated by *()^2/()^2 - * @return the traversal cost thus calculated - */ - cost getCost(const cost & cx, const cost & cy) const - { - return costmap_tolerance_ * - costmap_->getCost(cx, cy) * costmap_->getCost(cx, cy) / LETHAL_COST / LETHAL_COST; - } - - void addIndex(const map_pts & cx, const map_pts & cy, const id & id_this) - { - node_position[size_x * cy + cx] = id_this; - } - - void getIndex(const map_pts & cx, const map_pts & cy, id & id_this) - { - id_this = node_position[size_x * cy + cx]; - } - - bool withinLimits(const map_pts & cx, const map_pts & cy) const - { - return cx >= 0 && cx < size_x && cy >= 0 && cy < size_y; - } - - bool isGoal(const map_pts & cx, const map_pts & cy) const - { - return cx == dst.x && cy == dst.y; - } - - cost maxCost(const cost & a, const cost & b) const - { - if (a > b) {return a; } else {return b; } - } - - void addToNodesData(const id & id_this) - { - if (nodes_data.size() <= static_cast(id_this)) { - nodes_data.push_back({}); - } else { - nodes_data[id_this] = {}; - } - } - - void clearQueue() - { - while (!queue_.empty()) { - queue_.pop(); - } - } - - void initializePosn(int size_inc = 0) - { - int i = 0; - if (!node_position.empty()) { - for (; i < size_x * size_y; i++) { - node_position[i] = -1; - } - } - for (; i < size_inc; i++) { - node_position.emplace_back(-1); - } - } - - void setContainers(); - -public: - - coordsM src{}, dst{}; - nav2_costmap_2d::Costmap2D * costmap_{}; - nav2_util::LifecycleNode::SharedPtr node_; - - int how_many_corners_; - int size_x, size_y; - - // parameter for cost of costmap traversal - cost costmap_tolerance_; - // parameter for distance function used for heurestic - cost euc_tolerance_2_; - // parameter for distance function used for distance function cost from the source to the current node - cost euc_tolerance_; - - LazyThetaStarP(); - - /** - * @brief the function that iteratively searces upon the nodes in the queue (open list) until the - * current node is the goal - * @param raw_path is used to return the path obtained on exectuing the algorithm - * @return true if a path is found, false if no path is found - */ - bool generatePath(std::vector & raw_path); - - bool isSafe(const map_pts & cx, const map_pts & cy) const - { - return costmap_->getCost(cx, cy) < LETHAL_COST; - } -}; -} // namespace lazyThetaStarP - -#endif // LAZY_THETA_STAR_P_PLANNER__LAZY_THETA_STAR_P_HPP_ diff --git a/lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p_planner.hpp b/lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p_planner.hpp deleted file mode 100644 index 4dcf969010..0000000000 --- a/lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p_planner.hpp +++ /dev/null @@ -1,80 +0,0 @@ -// Copyright 2020 Anshumaan Singh -// -// 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. - -#ifndef LAZY_THETA_STAR_P_PLANNER__LAZY_THETA_STAR_P_PLANNER_HPP_ -#define LAZY_THETA_STAR_P_PLANNER__LAZY_THETA_STAR_P_PLANNER_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include "rclcpp/rclcpp.hpp" -#include "nav2_core/global_planner.hpp" -#include "nav_msgs/msg/path.hpp" -#include "nav2_util/robot_utils.hpp" -#include "nav2_util/lifecycle_node.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "nav2_costmap_2d/cost_values.hpp" -#include "nav2_util/node_utils.hpp" -#include "lazy_theta_star_p_planner/lazy_theta_star_p.hpp" - -namespace lazyThetaStarP_Planner -{ - -class LazyThetaStarP_Planner : public nav2_core::GlobalPlanner -{ -public: - std::shared_ptr tf_; - nav2_util::LifecycleNode::SharedPtr node_; - - std::string globalFrame_, name_; - - void configure( - rclcpp_lifecycle::LifecycleNode::SharedPtr parent, - std::string name, std::shared_ptr tf, - std::shared_ptr costmapRos) override; - - void cleanup() override; - - void activate() override; - - void deactivate() override; - - nav_msgs::msg::Path createPlan( - const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) override; - -private: - double interpolation_dist_; - std::unique_ptr planner_; - - void getPlan(nav_msgs::msg::Path & global_path); - - void setStartAndGoal( - const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal); - - - static nav_msgs::msg::Path linearInterpolation( - const std::vector & raw_path, - const double & dist_bw_points); - -}; -} // namespace lazyThetaStarP_Planner - -#endif // LAZY_THETA_STAR_P_PLANNER__LAZY_THETA_STAR_P_PLANNER_HPP_ diff --git a/lazy_theta_star_p_planner/package.xml b/lazy_theta_star_p_planner/package.xml deleted file mode 100644 index db18a82b9e..0000000000 --- a/lazy_theta_star_p_planner/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - lazy_theta_star_p_planner - 0.4.0 - TODO: Package description - Steve Macenski - Anshumaan Singh - Apache-2.0 - ament_cmake - rclcpp - nav2_core - rclcpp_action - rclcpp_lifecycle - std_msgs - visualization_msgs - nav2_util - nav2_msgs - builtin_interfaces - tf2_ros - nav2_costmap_2d - pluginlib - ament_lint_auto - ament_lint_common - - ament_cmake - - - - diff --git a/lazy_theta_star_p_planner/src/lazy_theta_star_p.cpp b/lazy_theta_star_p_planner/src/lazy_theta_star_p.cpp deleted file mode 100644 index f4bf8900d2..0000000000 --- a/lazy_theta_star_p_planner/src/lazy_theta_star_p.cpp +++ /dev/null @@ -1,262 +0,0 @@ -// Copyright 2020 Anshumaan Singh -// -// 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. - -#include "lazy_theta_star_p_planner/lazy_theta_star_p.hpp" -#include - -namespace lazyThetaStarP -{ - -LazyThetaStarP::LazyThetaStarP() -{ - costmap_tolerance_ = 1.75; - euc_tolerance_2_ = 1.0; - euc_tolerance_ = 1.0; - size_x = 0; - size_y = 0; - index_generated = 0; -} - -bool LazyThetaStarP::generatePath(std::vector & raw_path) -{ - setContainers(); - RCLCPP_INFO(node_->get_logger(), "Path Planning Begins... "); - - index_generated = 0; - id curr_id = index_generated; - addToNodesData(index_generated); - nodes_data[curr_id] = {src.x, src.y, getCost(src.x, src.y), dist(src.x, src.y, dst.x, dst.y), - curr_id, true, dist(src.x, src.y, dst.x, dst.y) + (getCost(src.x, src.y))}; - queue_.push({curr_id, (nodes_data[curr_id].f)}); - addIndex(nodes_data[curr_id].x, nodes_data[curr_id].y, index_generated); - index_generated++; - - int nodes_opened = 0; - while (!queue_.empty()) { - nodes_opened++; - - if (isGoal(nodes_data[curr_id].x, nodes_data[curr_id].y)) { - break; - } - tree_node & curr_data = nodes_data[curr_id]; - resetParent(curr_data); - setNeighbors(curr_data, curr_id); - - curr_id = queue_.top().pos_id; - queue_.pop(); - } - - if (queue_.empty()) { - RCLCPP_INFO(node_->get_logger(), "No Path Found !!!!!!!!!!!"); - raw_path.clear(); - return false; - } - - std::cout << "the number of executions are: " << nodes_opened << '\n'; - RCLCPP_INFO(node_->get_logger(), - "REACHED DEST %i, %i --- %f", - nodes_data[curr_id].x, - nodes_data[curr_id].y, - nodes_data[curr_id].g); - backtrace(raw_path, curr_id); - clearQueue(); - return true; -} - -void LazyThetaStarP::resetParent(tree_node & curr_data) -{ - cost g_cost, cal_cost, los_cost = 0; - curr_data.is_in_queue = false; - tree_node & curr_par = nodes_data[curr_data.parent_id]; - tree_node & maybe_par = nodes_data[curr_par.parent_id]; - - if (losCheck(curr_data.x, curr_data.y, maybe_par.x, maybe_par.y, los_cost)) { - g_cost = maybe_par.g + - dist(curr_data.x, curr_data.y, maybe_par.x, maybe_par.y) * euc_tolerance_ + - los_cost; - cal_cost = g_cost + curr_data.h; - if (cal_cost < curr_data.f) { - curr_data.parent_id = curr_par.parent_id; - curr_data.g = g_cost; - curr_data.f = cal_cost; - } - } -} - -void LazyThetaStarP::setNeighbors(const tree_node & curr_data, const id & curr_id) -{ - map_pts mx, my; - id m_id, m_par; - cost g_cost, h_cost, cal_cost; - tree_node * curr_node; - - const tree_node & curr_par = nodes_data[curr_data.parent_id]; - for (auto & move : moves) { - mx = curr_data.x + move.x; - my = curr_data.y + move.y; - - if (mx == curr_par.x && my == curr_par.y) { - continue; - } - - if (withinLimits(mx, my)) { - if (!isSafe(mx, my)) { - continue; - } - } else { - continue; - } - - m_par = curr_id; - g_cost = curr_data.g + dist(curr_data.x, curr_data.y, mx, my) * euc_tolerance_ + - getCost(mx, my); - - getIndex(mx, my, m_id); - - if (m_id == -1) { - addToNodesData(index_generated); - m_id = index_generated; - addIndex(mx, my, m_id); - index_generated++; - } - - curr_node = &nodes_data[m_id]; - - h_cost = dist(mx, my, dst.x, dst.y) * euc_tolerance_2_; - cal_cost = g_cost + h_cost; - - if (curr_node->f > cal_cost) { - curr_node->g = g_cost; - curr_node->h = h_cost; - curr_node->f = cal_cost; - curr_node->parent_id = m_par; - if (!curr_node->is_in_queue) { - curr_node->x = mx; - curr_node->y = my; - curr_node->is_in_queue = true; - queue_.push({m_id, (curr_node->f)}); - } - } - } -} - -void LazyThetaStarP::backtrace(std::vector & raw_points, id curr_id) -{ - std::vector path_rev; - coordsW world{}; - double pusher_ = 0.5 * costmap_->getResolution(); - do { - costmap_->mapToWorld(nodes_data[curr_id].x, nodes_data[curr_id].y, world.x, world.y); - path_rev.push_back({world.x + pusher_, world.y + pusher_}); - curr_id = nodes_data[curr_id].parent_id; - } while (curr_id != 0); - costmap_->mapToWorld(nodes_data[curr_id].x, nodes_data[curr_id].y, world.x, world.y); - path_rev.push_back({world.x + pusher_, world.y + pusher_}); - - raw_points.reserve(path_rev.size()); - for (int i = static_cast(path_rev.size()) - 1; i >= 0; i--) { - raw_points.push_back(path_rev[i]); - } -} - -bool LazyThetaStarP::losCheck( - const map_pts & x0, - const map_pts & y0, - const map_pts & x1, - const map_pts & y1, - cost & sl_cost) const -{ - sl_cost = 0; - const int dy = abs(y1 - y0); - const int dx = abs(x1 - x0); - const int sx = x1 > x0 ? 1 : -1; - const int sy = y1 > y0 ? 1 : -1; - const int u_x = (sx - 1) / 2; - const int u_y = (sy - 1) / 2; - int cx, cy, f; - cx = x0; - cy = y0; - f = 0; - - if (dx >= dy) { - if (dy != 0) { - for (; cx != x1; cx += sx) { - f += dy; - if (f >= dx) { - if (!isSafe(cx + u_x, cy + u_y)) { - return false; - } - sl_cost += getCost(cx + u_x, cy + u_y); - cy += sy; - f -= dx; - } - if (f != 0 && !isSafe(cx + u_x, cy + u_y)) { - return false; - } - sl_cost += getCost(cx + u_x, cy + u_y); - } - } else { - for (cx = x0; cx < x1; cx += sx) { - if (!isSafe(cx + u_x, cy) || !isSafe(cx + u_x, cy - 1)) { - return false; - } - sl_cost += maxCost(getCost(cx + u_x, cy), getCost(cx + u_x, cy - 1)); - } - } - } else { - if (dx != 0) { - for (; cy != y1; cy += sy) { - f = f + dx; - if (f >= dy) { - if (!isSafe(cx + u_x, cy + u_y)) { - return false; - } - sl_cost += getCost(cx + u_x, cy + u_y); - cx += sx; - f -= dy; - } - if (f != 0 && !isSafe(cx + u_x, cy + u_y)) { - return false; - } - sl_cost += getCost(cx + u_x, cy + u_y); - } - } else { - for (; cy != y1; cy += sy) { - if (!isSafe(cx, cy + u_y) || !isSafe(cx - 1, cy + u_y)) { - return false; - } - sl_cost += maxCost(getCost(cx, cy + u_y), getCost(cx - 1, cy + u_y)); - } - } - } - return true; -} - -void LazyThetaStarP::setContainers() -{ - index_generated = 0; - int last_size_x = size_x; - int last_size_y = size_y; - int curr_size_x = static_cast(costmap_->getSizeInCellsX()); - int curr_size_y = static_cast(costmap_->getSizeInCellsY()); - if (last_size_x != curr_size_x || last_size_y != curr_size_y) { - initializePosn(curr_size_y * curr_size_x - last_size_y * last_size_x); - nodes_data.reserve(curr_size_x * curr_size_y); - } else { - initializePosn(); - } - size_x = curr_size_x; - size_y = curr_size_y; -} -} // namespace lazyThetaStarP diff --git a/lazy_theta_star_p_planner/src/lazy_theta_star_p_planner.cpp b/lazy_theta_star_p_planner/src/lazy_theta_star_p_planner.cpp deleted file mode 100644 index 338b43a160..0000000000 --- a/lazy_theta_star_p_planner/src/lazy_theta_star_p_planner.cpp +++ /dev/null @@ -1,190 +0,0 @@ -// Copyright 2020 Anshumaan Singh -// -// 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. - -#include -#include -#include -#include "lazy_theta_star_p_planner/lazy_theta_star_p_planner.hpp" -#include "lazy_theta_star_p_planner/lazy_theta_star_p.hpp" - -namespace lazyThetaStarP_Planner -{ -void LazyThetaStarP_Planner::configure( - rclcpp_lifecycle::LifecycleNode::SharedPtr parent, - std::string name, std::shared_ptr tf, - std::shared_ptr costmapRos) -{ - planner_ = std::make_unique(); - node_ = parent; - name_ = name; - tf_ = tf; - planner_->costmap_ = costmapRos->getCostmap(); - globalFrame_ = costmapRos->getGlobalFrameID(); - - nav2_util::declare_parameter_if_not_declared( - node_, name_ + ".how_many_corners", rclcpp::ParameterValue( - 8)); - - node_->get_parameter(name_ + ".how_many_corners", planner_->how_many_corners_); - - if (planner_->how_many_corners_ != 8 && planner_->how_many_corners_ != 4) { - planner_->how_many_corners_ = 8; - RCLCPP_WARN(node_->get_logger(), - "Your value for - .how_many_coreners was overriden, and is now set to 8"); - } - - - nav2_util::declare_parameter_if_not_declared( - node_, name_ + ".interpolation_dist", rclcpp::ParameterValue( - 0.1)); - - node_->get_parameter(name_ + ".interpolation_dist", interpolation_dist_); - -/// could be added in the future verions if required, this -/// includes the parameters for lethal_cost, parameters for distance function and the costmap traversal costs -// nav2_util::declare_parameter_if_not_declared( -// node_, name_ + ".lethal_cost", rclcpp::ParameterValue( -// 252)); -// -// node_->get_parameter(name_ + ".lethal_cost", lethal_cost_); -// -// nav2_util::declare_parameter_if_not_declared( -// node_, name_ + ".euc_tolerance", rclcpp::ParameterValue(1.0)); -// -// node_->get_parameter(name_ + ".euc_tolerance", planner_->euc_tolerance_); -// -// nav2_util::declare_parameter_if_not_declared( -// node_, name_ + ".costmap_tolerance", rclcpp::ParameterValue(1.0)); -// -// node_->get_parameter(name_ + ".costmap_tolerance", planner_->costmap_tolerance_); -} - -void LazyThetaStarP_Planner::cleanup() -{ - RCLCPP_INFO( - node_->get_logger(), "CleaningUp plugin %s of type LazyThetaStarP_Planner", - name_.c_str()); -} - -void LazyThetaStarP_Planner::activate() -{ - RCLCPP_INFO( - node_->get_logger(), "Activating plugin %s of type LazyThetaStarP_Planner", - name_.c_str()); -} - -void LazyThetaStarP_Planner::deactivate() -{ - RCLCPP_INFO( - node_->get_logger(), "Deactivating plugin %s of type LazyThetaStarP_Planner", - name_.c_str()); -} - -nav_msgs::msg::Path LazyThetaStarP_Planner::createPlan( - const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) -{ - nav_msgs::msg::Path global_path; - setStartAndGoal(start, goal); - RCLCPP_INFO( - node_->get_logger(), "Got the src and dst... (%i, %i) && (%i, %i)", - planner_->src.x, planner_->src.y, planner_->dst.x, planner_->dst.y); - getPlan(global_path); - return global_path; -} - -void LazyThetaStarP_Planner::getPlan(nav_msgs::msg::Path & global_path) -{ - planner_->node_ = node_; - - /// NOTE : a few lines of code is to be removed in the final version - - std::vector path; - auto start = std::chrono::steady_clock::now(); - if (!(planner_->isSafe(planner_->src.x, - planner_->src.y)) || !(planner_->isSafe(planner_->dst.x, planner_->dst.y))) - { -// RCLCPP_INFO(node_->get_logger(), "EITHER OF THE START OR GOAL POINTS ARE AN OBSTACLE! "); -// coordsW world{}; -// planner_->costmap_->mapToWorld(planner_->src.x, planner_->src.y, world.x, world.y); -// path.push_back({world.x, world.y}); -// planner_->costmap_->mapToWorld(planner_->dst.x, planner_->dst.y, world.x, world.y); -// path.push_back({world.x, world.y}); -// global_path = linearInterpolation(path, interpolation_dist_); - global_path.poses.clear(); - } else if (planner_->generatePath(path)) { - RCLCPP_INFO(node_->get_logger(), "RECEIVED THE PATH"); - global_path = linearInterpolation(path, interpolation_dist_); - } else { - global_path.poses.clear(); - } - - auto stop = std::chrono::steady_clock::now(); - auto dur = std::chrono::duration_cast(stop - start); - std::cout << "the time taken in microseconds is \t\t : " << dur.count() << '\n'; - path.clear(); - std::cout << global_path.poses.size() << '\n'; - global_path.header.stamp = node_->now(); - global_path.header.frame_id = globalFrame_; - RCLCPP_INFO(node_->get_logger(), "SENT OUT THE PATH"); - - RCLCPP_INFO(node_->get_logger(), "the cost at src : %i", - planner_->costmap_->getCost(planner_->src.x, planner_->src.y)); - RCLCPP_INFO(node_->get_logger(), "the cost at dst : %i", - planner_->costmap_->getCost(planner_->dst.x, planner_->dst.y)); -} - -nav_msgs::msg::Path LazyThetaStarP_Planner::linearInterpolation( - const std::vector & raw_path, - const double & dist_bw_points) -{ - nav_msgs::msg::Path pa; - - for (unsigned int j = 0; j < raw_path.size() - 1; j++) { - geometry_msgs::msg::PoseStamped p; - coordsW pt1 = {raw_path[j].x, raw_path[j].y}; - p.pose.position.x = pt1.x; - p.pose.position.y = pt1.y; - pa.poses.push_back(p); - - coordsW pt2 = {raw_path[j + 1].x, raw_path[j + 1].y}; - geometry_msgs::msg::PoseStamped p1; - double distance = std::hypot(pt2.x - pt1.x, pt2.y - pt1.y); - int loops = static_cast(distance / dist_bw_points); - double sin_alpha = (pt2.y - pt1.y) / distance; - double cos_alpha = (pt2.x - pt1.x) / distance; - for (int k = 1; k < loops; k++) { - p1.pose.position.x = pt1.x + k * dist_bw_points * cos_alpha; - p1.pose.position.y = pt1.y + k * dist_bw_points * sin_alpha; - pa.poses.push_back(p1); - } - } - return pa; -} -void LazyThetaStarP_Planner::setStartAndGoal( - const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) -{ - unsigned int src_[2], dst_[2]; - planner_->costmap_->worldToMap(start.pose.position.x, start.pose.position.y, src_[0], src_[1]); - planner_->costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, dst_[0], dst_[1]); - - planner_->src = {static_cast(src_[0]), static_cast(src_[1])}; - planner_->dst = {static_cast(dst_[0]), static_cast(dst_[1])}; -} - -} // namespace lazyThetaStarP_Planner - -#include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(lazyThetaStarP_Planner::LazyThetaStarP_Planner, nav2_core::GlobalPlanner) From d2326b74d4c009fcc229ea22af551043105e03ef Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Thu, 14 Jan 2021 03:31:43 +0530 Subject: [PATCH 32/81] Replace the old files - the structure of code has been changed - new functions have been added, namely : getTraversalCost, getEuclideanCost, getCellCost, isSafe[it is now an overloaded function] - documentation added for variables and functions - the parameters for the planner now consists of : how_many_corners, costmap_tolerance, euc_tolerance (documentation to added soon) - fixed a bug where the incorrect traversal cost of the node was taken --- lazy_theta_star_p_planner/CMakeLists.txt | 79 ++++++ .../global_planner_plugin.xml | 5 + .../lazy_theta_star_p.hpp | 238 ++++++++++++++++ .../lazy_theta_star_p_planner.hpp | 83 ++++++ lazy_theta_star_p_planner/package.xml | 30 +++ .../src/lazy_theta_star_p.cpp | 254 ++++++++++++++++++ .../src/lazy_theta_star_p_planner.cpp | 162 +++++++++++ 7 files changed, 851 insertions(+) create mode 100644 lazy_theta_star_p_planner/CMakeLists.txt create mode 100644 lazy_theta_star_p_planner/global_planner_plugin.xml create mode 100644 lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p.hpp create mode 100644 lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p_planner.hpp create mode 100644 lazy_theta_star_p_planner/package.xml create mode 100644 lazy_theta_star_p_planner/src/lazy_theta_star_p.cpp create mode 100644 lazy_theta_star_p_planner/src/lazy_theta_star_p_planner.cpp diff --git a/lazy_theta_star_p_planner/CMakeLists.txt b/lazy_theta_star_p_planner/CMakeLists.txt new file mode 100644 index 0000000000..d9aaafe6a3 --- /dev/null +++ b/lazy_theta_star_p_planner/CMakeLists.txt @@ -0,0 +1,79 @@ +cmake_minimum_required(VERSION 3.5) +project(lazy_theta_star_p_planner) + +set(CMAKE_BUILD_TYPE=Release) +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav2_util REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(ament_cmake_gtest REQUIRED) + + +nav2_package() + +include_directories( + include +) + +set(library_name ${PROJECT_NAME}_plugin) + +set(dependencies + ament_cmake + builtin_interfaces + nav2_common + nav2_core + nav2_costmap_2d + nav2_msgs + nav2_util + pluginlib + rclcpp + rclcpp_action + rclcpp_lifecycle + tf2_ros + ) + + +add_library(${library_name} SHARED + src/lazy_theta_star_p.cpp + src/lazy_theta_star_p_planner.cpp + ) + +ament_target_dependencies(${library_name} ${dependencies} ) + +target_compile_definitions(${library_name} PUBLIC "PLUGINLIB_DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file(nav2_core global_planner_plugin.xml) + +install(TARGETS ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} + ) + +install(DIRECTORY include/ + DESTINATION include/ + ) + +install(FILES global_planner_plugin.xml + DESTINATION share/${PROJECT_NAME} + ) + + +if (BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif () + + +ament_export_include_directories(include) +ament_export_libraries(${library_name}) +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/lazy_theta_star_p_planner/global_planner_plugin.xml b/lazy_theta_star_p_planner/global_planner_plugin.xml new file mode 100644 index 0000000000..aa9f9da961 --- /dev/null +++ b/lazy_theta_star_p_planner/global_planner_plugin.xml @@ -0,0 +1,5 @@ + + + this is empty + + diff --git a/lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p.hpp b/lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p.hpp new file mode 100644 index 0000000000..75e90f0257 --- /dev/null +++ b/lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p.hpp @@ -0,0 +1,238 @@ +// Copyright 2020 Anshumaan Singh +// +// 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. + +#ifndef LAZY_THETA_STAR_P_PLANNER__LAZY_THETA_STAR_P_HPP_ +#define LAZY_THETA_STAR_P_PLANNER__LAZY_THETA_STAR_P_HPP_ + +#include +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + +const double INF_COST = DBL_MAX; +const int LETHAL_COST = 252; + +struct coordsM +{ + int x, y; +}; + +struct coordsW +{ + double x, y; +}; + +struct pos +{ + int pos_id; + double f; +}; + +struct tree_node +{ + int x, y; + double g = INF_COST; + double h = INF_COST; + int parent_id; + bool is_in_queue = false; + double f = INF_COST; +}; + +struct comp +{ + bool operator()(pos & p1, pos & p2) + { + return (p1.f) > (p2.f); + } +}; + +/// TODO(ANSHU-MAN567): try using a pointer to the tree_node vector instead of directly using it + +namespace lazyThetaStarP +{ +class LazyThetaStarP +{ +public: + coordsM src{}, dst{}; + nav2_costmap_2d::Costmap2D * costmap_{}; + nav2_util::LifecycleNode::SharedPtr node_; + + /// weight for the costmap traversal cost + double costmap_tolerance_; + /// weight for the euclidean distance cost (as of now used for calculation of gCost and hCost) + double euc_tolerance_; + + int how_many_corners_; + int size_x_, size_y_; + + LazyThetaStarP(); + + /** + * @brief the function that iteratively searches upon the nodes in the queue (open list) until the + * current node is the goal pose or if size of queue > 0 + * @param raw_path is used to return the path obtained on exectuing the algorithm + * @return true if a path is found, false if no path is found between the start and goal pose + */ + bool generatePath(std::vector & raw_path); + + bool isSafe(const int & cx, const int & cy) const + { + return costmap_->getCost(cx + 1, cy + 1) < LETHAL_COST; + } + +private: + /// for the coordinates (x,y), we store at node_position[size_x_ * y + x], + /// the index at which the data of the node is present in nodes_data + std::vector node_position; + + /// the vector nodes_data stores the coordinates, costs and index of the parent node, + /// and whether or not the node is present in queue_ + std::vector nodes_data; + + /// this is the priority queue (open_list) to select the next node for expansion + std::priority_queue, comp> queue_; + + /// it is a counter like variable used to generate consecutive indices + /// such that the data for all the nodes (in open and closed lists) could be stored + /// consecutively in nodes_data + int index_generated; + + const coordsM moves[8] = {{0, 1}, + {0, -1}, + {1, 0}, + {-1, 0}, + {1, -1}, + {-1, 1}, + {1, 1}, + {-1, -1}}; + + tree_node * curr_node = new tree_node; + /** @brief it does a line of sight (los) check between the current node and the parent of its parent node + * if an los is found and the new costs calculated are lesser then the cost and parent node of the current node + * is updated + * @param data of the current node + */ + void resetParent(tree_node & curr_data); + + /** + * @brief this function expands the neighbors of the current node + * @param curr_data used to send the data of the current node + * @param curr_id used to send the index of the current node as stored in nodes_position + */ + void setNeighbors(const tree_node & curr_data, const int & curr_int); + + /** + * @brief it returns the path by backtracing from the goal to the start, by using their parent nodes + * @param raw_points used to return the path thus found + * @param curr_id sends in the index of the goal coordinate, as stored in nodes_position + */ + void backtrace(std::vector & raw_points, int curr_id); + + /** + * @brief performs the line of sight check using Bresenham's Algorithm, + * and has been modified to calculate the traversal cost incurred in a straight line path between + * the two points whose coordinates are (x0, y0) and (x1, y1) + * @param sl_cost is used to return the cost thus incurred + * @return true if a line of sight exists between the points + */ + bool losCheck( + const int & x0, const int & y0, const int & x1, const int & y1, + double & sl_cost); + + void initializePosn(int size_inc = 0); + + void setContainers(); + + double dist(const int & ax, const int & ay, const int & bx, const int & by) + { + return std::hypot(ax - bx, ay - by); + } + + double getEucledianCost(const int & ax, const int & ay, const int & bx, const int & by) + { + return euc_tolerance_ * dist(ax, ay, bx, by); + } + + double getCellCost(const int & cx, const int & cy) const + { + return costmap_->getCost(cx + 1, cy + 1); + } + + /** + * @brief for the point(cx, cy) its traversal cost is calculated by *()^2/()^2 + * @return the traversal cost thus calculated + */ + double getTraversalCost(const int & cx, const int & cy) + { + double curr_cost = getCellCost(cx, cy); + return costmap_tolerance_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST; + } + /** + * @brief it is an overloaded function to ease in cost calculations while performing the LOS check + * @param cx + * @param cy + * @param cost denotes the total straight line traversal cost, adds the traversal cost for the node (cx, cy) at every instance + */ + bool isSafe(const int & cx, const int & cy, double & cost) const + { + double curr_cost = getCellCost(cx, cy); + if (curr_cost < LETHAL_COST) { + cost += costmap_tolerance_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST; + return true; + } else { + return false; + } + } + + void addIndex(const int & cx, const int & cy, const int & id_this) + { + node_position[size_x_ * cy + cx] = id_this; + } + + void getIndex(const int & cx, const int & cy, int & id_this) + { + id_this = node_position[size_x_ * cy + cx]; + } + + bool withinLimits(const int & cx, const int & cy) const + { + return cx >= 0 && cx < size_x_ && cy >= 0 && cy < size_y_; + } + + bool isGoal(const int & cx, const int & cy) const + { + return cx == dst.x && cy == dst.y; + } + + void addToNodesData(const int & id_this) + { + if (nodes_data.size() <= static_cast(id_this)) { + nodes_data.push_back({}); + } else { + nodes_data[id_this] = {}; + } + } + + void clearQueue() + { + while (!queue_.empty()) + {queue_.pop();} + } +}; +} // namespace lazyThetaStarP + +#endif // LAZY_THETA_STAR_P_PLANNER__LAZY_THETA_STAR_P_HPP_ diff --git a/lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p_planner.hpp b/lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p_planner.hpp new file mode 100644 index 0000000000..9f235f47c5 --- /dev/null +++ b/lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p_planner.hpp @@ -0,0 +1,83 @@ +// Copyright 2020 Anshumaan Singh +// +// 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. + +#ifndef LAZY_THETA_STAR_P_PLANNER__LAZY_THETA_STAR_P_PLANNER_HPP_ +#define LAZY_THETA_STAR_P_PLANNER__LAZY_THETA_STAR_P_PLANNER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "nav2_core/global_planner.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_util/node_utils.hpp" +#include "lazy_theta_star_p_planner/lazy_theta_star_p.hpp" + +namespace lazyThetaStarP_Planner +{ + +class LazyThetaStarP_Planner : public nav2_core::GlobalPlanner +{ +public: + std::shared_ptr tf_; + nav2_util::LifecycleNode::SharedPtr node_; + + std::string globalFrame_, name_; + + void configure( + rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmapRos) override; + + void cleanup() override; + + void activate() override; + + void deactivate() override; + + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) override; + +private: + std::unique_ptr planner_; + + void getPlan(nav_msgs::msg::Path & global_path); + + void setStartAndGoal( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal); + + /** + * @brief linearily interpolates between the adjacent waypoints of the path + * @param raw_path is used to send in the path recieved from the planner + * @param dist_bw_points is used to send in the interpolation_resolution + * @return the final path with waypoints at a distance of the value of interpolation_resolution of each other + */ + static nav_msgs::msg::Path linearInterpolation( + const std::vector & raw_path, + const double & dist_bw_points); +}; +} // namespace lazyThetaStarP_Planner + +#endif // LAZY_THETA_STAR_P_PLANNER__LAZY_THETA_STAR_P_PLANNER_HPP_ diff --git a/lazy_theta_star_p_planner/package.xml b/lazy_theta_star_p_planner/package.xml new file mode 100644 index 0000000000..db18a82b9e --- /dev/null +++ b/lazy_theta_star_p_planner/package.xml @@ -0,0 +1,30 @@ + + + + lazy_theta_star_p_planner + 0.4.0 + TODO: Package description + Steve Macenski + Anshumaan Singh + Apache-2.0 + ament_cmake + rclcpp + nav2_core + rclcpp_action + rclcpp_lifecycle + std_msgs + visualization_msgs + nav2_util + nav2_msgs + builtin_interfaces + tf2_ros + nav2_costmap_2d + pluginlib + ament_lint_auto + ament_lint_common + + ament_cmake + + + + diff --git a/lazy_theta_star_p_planner/src/lazy_theta_star_p.cpp b/lazy_theta_star_p_planner/src/lazy_theta_star_p.cpp new file mode 100644 index 0000000000..79c0bc4ffa --- /dev/null +++ b/lazy_theta_star_p_planner/src/lazy_theta_star_p.cpp @@ -0,0 +1,254 @@ +// Copyright 2020 Anshumaan Singh +// +// 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. + +#include "lazy_theta_star_p_planner/lazy_theta_star_p.hpp" +#include + +namespace lazyThetaStarP +{ + +LazyThetaStarP::LazyThetaStarP() : costmap_tolerance_(4.0), + euc_tolerance_(6.5), + how_many_corners_(8), + size_x_(0), + size_y_(0), + index_generated(0) {} + +bool LazyThetaStarP::generatePath(std::vector & raw_path) +{ + setContainers(); + int curr_id = index_generated; + addToNodesData(index_generated); + nodes_data[curr_id] = {src.x, src.y, getTraversalCost(src.x, src.y), dist(src.x, src.y, dst.x, + dst.y), + curr_id, true, dist(src.x, src.y, dst.x, dst.y) + (getTraversalCost(src.x, src.y))}; + queue_.push({curr_id, (nodes_data[curr_id].f)}); + addIndex(nodes_data[curr_id].x, nodes_data[curr_id].y, index_generated); + index_generated++; + + int nodes_opened = 0; + + while (!queue_.empty()) { + nodes_opened++; + + if (isGoal(nodes_data[curr_id].x, nodes_data[curr_id].y)) { + break; + } + tree_node & curr_data = nodes_data[curr_id]; + resetParent(curr_data); + setNeighbors(curr_data, curr_id); + + curr_id = queue_.top().pos_id; + queue_.pop(); + } + + if (queue_.empty()) { + raw_path.clear(); + return false; + } + + backtrace(raw_path, curr_id); + clearQueue(); + + return true; +} + +void LazyThetaStarP::resetParent(tree_node & curr_data) +{ + double g_cost, los_cost = 0; + curr_data.is_in_queue = false; + tree_node & curr_par = nodes_data[curr_data.parent_id]; + tree_node & maybe_par = nodes_data[curr_par.parent_id]; + + if (losCheck(curr_data.x, curr_data.y, maybe_par.x, maybe_par.y, los_cost)) { + g_cost = maybe_par.g + + getEucledianCost(curr_data.x, curr_data.y, maybe_par.x, maybe_par.y) + + los_cost; + + if (g_cost < curr_data.g) { + curr_data.parent_id = curr_par.parent_id; + curr_data.g = g_cost; + curr_data.f = g_cost + curr_data.h; + } + } +} + +void LazyThetaStarP::setNeighbors(const tree_node & curr_data, const int & curr_id) +{ + int mx, my; + int m_id, m_par; + double g_cost, h_cost, cal_cost; + + for (int i = 0; i < how_many_corners_; i++) { + mx = curr_data.x + moves[i].x; + my = curr_data.y + moves[i].y; + + if (withinLimits(mx, my)) { + if (!isSafe(mx, my)) { + continue; + } + } else { + continue; + } + + m_par = curr_id; + g_cost = curr_data.g + getEucledianCost(curr_data.x, curr_data.y, mx, my) + + getTraversalCost(mx, my); + + getIndex(mx, my, m_id); + + if (m_id == -1) { + addToNodesData(index_generated); + m_id = index_generated; + addIndex(mx, my, m_id); + index_generated++; + } + + curr_node = &nodes_data[m_id]; + + h_cost = getEucledianCost(mx, my, dst.x, dst.y); + cal_cost = g_cost + h_cost; + + if (curr_node->f > cal_cost) { + curr_node->g = g_cost; + curr_node->h = h_cost; + curr_node->f = cal_cost; + curr_node->parent_id = m_par; + if (!curr_node->is_in_queue) { + curr_node->x = mx; + curr_node->y = my; + curr_node->is_in_queue = true; + queue_.push({m_id, (curr_node->f)}); + } + } + } +} + +void LazyThetaStarP::backtrace(std::vector & raw_points, int curr_id) +{ + std::vector path_rev; + coordsW world{}; + double pusher_ = 0.5 * costmap_->getResolution(); + do { + costmap_->mapToWorld(nodes_data[curr_id].x, nodes_data[curr_id].y, world.x, world.y); + path_rev.push_back({world.x + pusher_, world.y + pusher_}); + if (path_rev.size() > 1) { + curr_id = nodes_data[curr_id].parent_id; + } + } while (curr_id != 0); + costmap_->mapToWorld(nodes_data[curr_id].x, nodes_data[curr_id].y, world.x, world.y); + path_rev.push_back({world.x + pusher_, world.y + pusher_}); + + raw_points.reserve(path_rev.size()); + for (int i = static_cast(path_rev.size()) - 1; i >= 0; i--) { + raw_points.push_back(path_rev[i]); + } +} + +bool LazyThetaStarP::losCheck( + const int & x0, + const int & y0, + const int & x1, + const int & y1, + double & sl_cost) +{ + sl_cost = 0; + + int cx, cy; + int dy = abs(y1 - y0), dx = abs(x1 - x0), f = 0; + int sx, sy; + int lx, ly; // this variable is reserved for future use + sy = y1 > y0 ? 1 : -1; + sx = x1 > x0 ? 1 : -1; + + int u_x = (sx - 1) / 2; + int u_y = (sy - 1) / 2; + lx = x1; + ly = y1; + cx = x0; + cy = y0; + + if (dx >= dy) { + while (cx != lx) { + f += dy; + if (f >= dx) { + if (!isSafe(cx + u_x, cy + u_y, sl_cost)) { + return false; + } + cy += sy; + f -= dx; + } + if (f != 0 && !isSafe(cx + u_x, cy + u_y, sl_cost)) { + return false; + } + if (dy == 0 && !isSafe(cx + u_x, cy, sl_cost) && !isSafe(cx + u_x, cy - 1, sl_cost)) { + return false; + } + cx += sx; + } + } else { + while (cy != ly) { + f = f + dx; + if (f >= dy) { + if (!isSafe(cx + u_x, cy + u_y, sl_cost)) { + return false; + } + cx += sx; + f -= dy; + } + if (f != 0 && !isSafe(cx + u_x, cy + u_y, sl_cost)) { + return false; + } + if (dx == 0 && isSafe(cx, cy + u_y, sl_cost) && !isSafe(cx - 1, cy + u_y, sl_cost)) { + return false; + } + cy += sy; + } + } + return true; +} + +void LazyThetaStarP::setContainers() +{ + index_generated = 0; + int last_size_x = size_x_; + int last_size_y = size_y_; + int curr_size_x = static_cast(costmap_->getSizeInCellsX()); + int curr_size_y = static_cast(costmap_->getSizeInCellsY()); + if (last_size_x != curr_size_x || last_size_y != curr_size_y) { + initializePosn(curr_size_y * curr_size_x - last_size_y * last_size_x); + nodes_data.reserve(curr_size_x * curr_size_y); + } else { + initializePosn(); + } + size_x_ = curr_size_x; + size_y_ = curr_size_y; +} + +void LazyThetaStarP::initializePosn(int size_inc) +{ + int i = 0; + + if (!node_position.empty()) { + for (; i < size_x_ * size_y_; i++) { + node_position[i] = -1; + } + } + + for (; i < size_inc; i++) { + node_position.emplace_back(-1); + } +} + +} // namespace lazyThetaStarP diff --git a/lazy_theta_star_p_planner/src/lazy_theta_star_p_planner.cpp b/lazy_theta_star_p_planner/src/lazy_theta_star_p_planner.cpp new file mode 100644 index 0000000000..794b043a95 --- /dev/null +++ b/lazy_theta_star_p_planner/src/lazy_theta_star_p_planner.cpp @@ -0,0 +1,162 @@ +// Copyright 2020 Anshumaan Singh +// +// 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. + +#include +#include +#include +#include "lazy_theta_star_p_planner/lazy_theta_star_p_planner.hpp" +#include "lazy_theta_star_p_planner/lazy_theta_star_p.hpp" + +namespace lazyThetaStarP_Planner +{ +void LazyThetaStarP_Planner::configure( + rclcpp_lifecycle::LifecycleNode::SharedPtr parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmapRos) +{ + planner_ = std::make_unique(); + node_ = parent; + name_ = name; + tf_ = tf; + planner_->costmap_ = costmapRos->getCostmap(); + globalFrame_ = costmapRos->getGlobalFrameID(); + + nav2_util::declare_parameter_if_not_declared( + node_, name_ + ".how_many_corners", rclcpp::ParameterValue( + 8)); + + node_->get_parameter(name_ + ".how_many_corners", planner_->how_many_corners_); + + if (planner_->how_many_corners_ != 8 && planner_->how_many_corners_ != 4) { + planner_->how_many_corners_ = 8; + RCLCPP_WARN(node_->get_logger(), + "Your value for - .how_many_corners was overridden, and is now set to 8"); + } + + nav2_util::declare_parameter_if_not_declared( + node_, name_ + ".euc_tolerance", rclcpp::ParameterValue(1.5)); + + node_->get_parameter(name_ + ".euc_tolerance", planner_->euc_tolerance_); + + nav2_util::declare_parameter_if_not_declared( + node_, name_ + ".costmap_tolerance", rclcpp::ParameterValue(1.0)); + + node_->get_parameter(name_ + ".costmap_tolerance", planner_->costmap_tolerance_); +} + +void LazyThetaStarP_Planner::cleanup() +{ + RCLCPP_INFO( + node_->get_logger(), "CleaningUp plugin %s of type LazyThetaStarP_Planner", + name_.c_str()); +} + +void LazyThetaStarP_Planner::activate() +{ + RCLCPP_INFO( + node_->get_logger(), "Activating plugin %s of type LazyThetaStarP_Planner", + name_.c_str()); +} + +void LazyThetaStarP_Planner::deactivate() +{ + RCLCPP_INFO( + node_->get_logger(), "Deactivating plugin %s of type LazyThetaStarP_Planner", + name_.c_str()); +} + +nav_msgs::msg::Path LazyThetaStarP_Planner::createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) +{ + nav_msgs::msg::Path global_path; + setStartAndGoal(start, goal); + RCLCPP_INFO( + node_->get_logger(), "Got the src and dst... (%i, %i) && (%i, %i)", + planner_->src.x, planner_->src.y, planner_->dst.x, planner_->dst.y); + getPlan(global_path); + return global_path; +} + +void LazyThetaStarP_Planner::getPlan(nav_msgs::msg::Path & global_path) +{ + planner_->node_ = node_; + std::vector path; + + if (!(planner_->isSafe(planner_->src.x, + planner_->src.y)) || !(planner_->isSafe(planner_->dst.x, planner_->dst.y))) + { + RCLCPP_ERROR(node_->get_logger(), "EITHER OF THE START OR GOAL POINTS ARE AN OBSTACLE! "); + coordsW world{}; + planner_->costmap_->mapToWorld(planner_->src.x, planner_->src.y, world.x, world.y); + path.push_back({world.x, world.y}); + planner_->costmap_->mapToWorld(planner_->dst.x, planner_->dst.y, world.x, world.y); + path.push_back({world.x, world.y}); + global_path = linearInterpolation(path, planner_->costmap_->getResolution()); + global_path.poses.clear(); + } else if (planner_->generatePath(path)) { + RCLCPP_INFO(node_->get_logger(), "A PATH HAS BEEN GENERATED SUCCESSFULLY"); + global_path = linearInterpolation(path, planner_->costmap_->getResolution()); + } else { + RCLCPP_ERROR(node_->get_logger(), "COULD NOT GENERATE PATH"); + global_path.poses.clear(); + } + path.clear(); + global_path.header.stamp = node_->now(); + global_path.header.frame_id = globalFrame_; +} + +nav_msgs::msg::Path LazyThetaStarP_Planner::linearInterpolation( + const std::vector & raw_path, + const double & dist_bw_points) +{ + nav_msgs::msg::Path pa; + + for (unsigned int j = 0; j < raw_path.size() - 1; j++) { + geometry_msgs::msg::PoseStamped p; + coordsW pt1 = {raw_path[j].x, raw_path[j].y}; + p.pose.position.x = pt1.x; + p.pose.position.y = pt1.y; + pa.poses.push_back(p); + + coordsW pt2 = {raw_path[j + 1].x, raw_path[j + 1].y}; + geometry_msgs::msg::PoseStamped p1; + double distance = std::hypot(pt2.x - pt1.x, pt2.y - pt1.y); + int loops = static_cast(distance / dist_bw_points); + double sin_alpha = (pt2.y - pt1.y) / distance; + double cos_alpha = (pt2.x - pt1.x) / distance; + for (int k = 1; k < loops; k++) { + p1.pose.position.x = pt1.x + k * dist_bw_points * cos_alpha; + p1.pose.position.y = pt1.y + k * dist_bw_points * sin_alpha; + pa.poses.push_back(p1); + } + } + return pa; +} +void LazyThetaStarP_Planner::setStartAndGoal( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) +{ + unsigned int src_[2], dst_[2]; + planner_->costmap_->worldToMap(start.pose.position.x, start.pose.position.y, src_[0], src_[1]); + planner_->costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, dst_[0], dst_[1]); + + planner_->src = {static_cast(src_[0]), static_cast(src_[1])}; + planner_->dst = {static_cast(dst_[0]), static_cast(dst_[1])}; +} + +} // namespace lazyThetaStarP_Planner + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(lazyThetaStarP_Planner::LazyThetaStarP_Planner, nav2_core::GlobalPlanner) From 04cf677ed9eaa22ed1168a3ce162318c12114e56 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Wed, 17 Feb 2021 14:20:32 +0530 Subject: [PATCH 33/81] Delete lazy_theta_star_p_planner directory --- lazy_theta_star_p_planner/CMakeLists.txt | 79 ------ .../global_planner_plugin.xml | 5 - .../lazy_theta_star_p.hpp | 238 ---------------- .../lazy_theta_star_p_planner.hpp | 83 ------ lazy_theta_star_p_planner/package.xml | 30 --- .../src/lazy_theta_star_p.cpp | 254 ------------------ .../src/lazy_theta_star_p_planner.cpp | 162 ----------- 7 files changed, 851 deletions(-) delete mode 100644 lazy_theta_star_p_planner/CMakeLists.txt delete mode 100644 lazy_theta_star_p_planner/global_planner_plugin.xml delete mode 100644 lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p.hpp delete mode 100644 lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p_planner.hpp delete mode 100644 lazy_theta_star_p_planner/package.xml delete mode 100644 lazy_theta_star_p_planner/src/lazy_theta_star_p.cpp delete mode 100644 lazy_theta_star_p_planner/src/lazy_theta_star_p_planner.cpp diff --git a/lazy_theta_star_p_planner/CMakeLists.txt b/lazy_theta_star_p_planner/CMakeLists.txt deleted file mode 100644 index d9aaafe6a3..0000000000 --- a/lazy_theta_star_p_planner/CMakeLists.txt +++ /dev/null @@ -1,79 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(lazy_theta_star_p_planner) - -set(CMAKE_BUILD_TYPE=Release) -find_package(ament_cmake REQUIRED) -find_package(builtin_interfaces REQUIRED) -find_package(nav2_common REQUIRED) -find_package(nav2_core REQUIRED) -find_package(nav2_costmap_2d REQUIRED) -find_package(nav2_msgs REQUIRED) -find_package(nav2_util REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(ament_cmake_gtest REQUIRED) - - -nav2_package() - -include_directories( - include -) - -set(library_name ${PROJECT_NAME}_plugin) - -set(dependencies - ament_cmake - builtin_interfaces - nav2_common - nav2_core - nav2_costmap_2d - nav2_msgs - nav2_util - pluginlib - rclcpp - rclcpp_action - rclcpp_lifecycle - tf2_ros - ) - - -add_library(${library_name} SHARED - src/lazy_theta_star_p.cpp - src/lazy_theta_star_p_planner.cpp - ) - -ament_target_dependencies(${library_name} ${dependencies} ) - -target_compile_definitions(${library_name} PUBLIC "PLUGINLIB_DISABLE_BOOST_FUNCTIONS") - -pluginlib_export_plugin_description_file(nav2_core global_planner_plugin.xml) - -install(TARGETS ${library_name} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} - ) - -install(DIRECTORY include/ - DESTINATION include/ - ) - -install(FILES global_planner_plugin.xml - DESTINATION share/${PROJECT_NAME} - ) - - -if (BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif () - - -ament_export_include_directories(include) -ament_export_libraries(${library_name}) -ament_export_dependencies(${dependencies}) -ament_package() diff --git a/lazy_theta_star_p_planner/global_planner_plugin.xml b/lazy_theta_star_p_planner/global_planner_plugin.xml deleted file mode 100644 index aa9f9da961..0000000000 --- a/lazy_theta_star_p_planner/global_planner_plugin.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - this is empty - - diff --git a/lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p.hpp b/lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p.hpp deleted file mode 100644 index 75e90f0257..0000000000 --- a/lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p.hpp +++ /dev/null @@ -1,238 +0,0 @@ -// Copyright 2020 Anshumaan Singh -// -// 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. - -#ifndef LAZY_THETA_STAR_P_PLANNER__LAZY_THETA_STAR_P_HPP_ -#define LAZY_THETA_STAR_P_PLANNER__LAZY_THETA_STAR_P_HPP_ - -#include -#include -#include -#include -#include -#include "rclcpp/rclcpp.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" - -const double INF_COST = DBL_MAX; -const int LETHAL_COST = 252; - -struct coordsM -{ - int x, y; -}; - -struct coordsW -{ - double x, y; -}; - -struct pos -{ - int pos_id; - double f; -}; - -struct tree_node -{ - int x, y; - double g = INF_COST; - double h = INF_COST; - int parent_id; - bool is_in_queue = false; - double f = INF_COST; -}; - -struct comp -{ - bool operator()(pos & p1, pos & p2) - { - return (p1.f) > (p2.f); - } -}; - -/// TODO(ANSHU-MAN567): try using a pointer to the tree_node vector instead of directly using it - -namespace lazyThetaStarP -{ -class LazyThetaStarP -{ -public: - coordsM src{}, dst{}; - nav2_costmap_2d::Costmap2D * costmap_{}; - nav2_util::LifecycleNode::SharedPtr node_; - - /// weight for the costmap traversal cost - double costmap_tolerance_; - /// weight for the euclidean distance cost (as of now used for calculation of gCost and hCost) - double euc_tolerance_; - - int how_many_corners_; - int size_x_, size_y_; - - LazyThetaStarP(); - - /** - * @brief the function that iteratively searches upon the nodes in the queue (open list) until the - * current node is the goal pose or if size of queue > 0 - * @param raw_path is used to return the path obtained on exectuing the algorithm - * @return true if a path is found, false if no path is found between the start and goal pose - */ - bool generatePath(std::vector & raw_path); - - bool isSafe(const int & cx, const int & cy) const - { - return costmap_->getCost(cx + 1, cy + 1) < LETHAL_COST; - } - -private: - /// for the coordinates (x,y), we store at node_position[size_x_ * y + x], - /// the index at which the data of the node is present in nodes_data - std::vector node_position; - - /// the vector nodes_data stores the coordinates, costs and index of the parent node, - /// and whether or not the node is present in queue_ - std::vector nodes_data; - - /// this is the priority queue (open_list) to select the next node for expansion - std::priority_queue, comp> queue_; - - /// it is a counter like variable used to generate consecutive indices - /// such that the data for all the nodes (in open and closed lists) could be stored - /// consecutively in nodes_data - int index_generated; - - const coordsM moves[8] = {{0, 1}, - {0, -1}, - {1, 0}, - {-1, 0}, - {1, -1}, - {-1, 1}, - {1, 1}, - {-1, -1}}; - - tree_node * curr_node = new tree_node; - /** @brief it does a line of sight (los) check between the current node and the parent of its parent node - * if an los is found and the new costs calculated are lesser then the cost and parent node of the current node - * is updated - * @param data of the current node - */ - void resetParent(tree_node & curr_data); - - /** - * @brief this function expands the neighbors of the current node - * @param curr_data used to send the data of the current node - * @param curr_id used to send the index of the current node as stored in nodes_position - */ - void setNeighbors(const tree_node & curr_data, const int & curr_int); - - /** - * @brief it returns the path by backtracing from the goal to the start, by using their parent nodes - * @param raw_points used to return the path thus found - * @param curr_id sends in the index of the goal coordinate, as stored in nodes_position - */ - void backtrace(std::vector & raw_points, int curr_id); - - /** - * @brief performs the line of sight check using Bresenham's Algorithm, - * and has been modified to calculate the traversal cost incurred in a straight line path between - * the two points whose coordinates are (x0, y0) and (x1, y1) - * @param sl_cost is used to return the cost thus incurred - * @return true if a line of sight exists between the points - */ - bool losCheck( - const int & x0, const int & y0, const int & x1, const int & y1, - double & sl_cost); - - void initializePosn(int size_inc = 0); - - void setContainers(); - - double dist(const int & ax, const int & ay, const int & bx, const int & by) - { - return std::hypot(ax - bx, ay - by); - } - - double getEucledianCost(const int & ax, const int & ay, const int & bx, const int & by) - { - return euc_tolerance_ * dist(ax, ay, bx, by); - } - - double getCellCost(const int & cx, const int & cy) const - { - return costmap_->getCost(cx + 1, cy + 1); - } - - /** - * @brief for the point(cx, cy) its traversal cost is calculated by *()^2/()^2 - * @return the traversal cost thus calculated - */ - double getTraversalCost(const int & cx, const int & cy) - { - double curr_cost = getCellCost(cx, cy); - return costmap_tolerance_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST; - } - /** - * @brief it is an overloaded function to ease in cost calculations while performing the LOS check - * @param cx - * @param cy - * @param cost denotes the total straight line traversal cost, adds the traversal cost for the node (cx, cy) at every instance - */ - bool isSafe(const int & cx, const int & cy, double & cost) const - { - double curr_cost = getCellCost(cx, cy); - if (curr_cost < LETHAL_COST) { - cost += costmap_tolerance_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST; - return true; - } else { - return false; - } - } - - void addIndex(const int & cx, const int & cy, const int & id_this) - { - node_position[size_x_ * cy + cx] = id_this; - } - - void getIndex(const int & cx, const int & cy, int & id_this) - { - id_this = node_position[size_x_ * cy + cx]; - } - - bool withinLimits(const int & cx, const int & cy) const - { - return cx >= 0 && cx < size_x_ && cy >= 0 && cy < size_y_; - } - - bool isGoal(const int & cx, const int & cy) const - { - return cx == dst.x && cy == dst.y; - } - - void addToNodesData(const int & id_this) - { - if (nodes_data.size() <= static_cast(id_this)) { - nodes_data.push_back({}); - } else { - nodes_data[id_this] = {}; - } - } - - void clearQueue() - { - while (!queue_.empty()) - {queue_.pop();} - } -}; -} // namespace lazyThetaStarP - -#endif // LAZY_THETA_STAR_P_PLANNER__LAZY_THETA_STAR_P_HPP_ diff --git a/lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p_planner.hpp b/lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p_planner.hpp deleted file mode 100644 index 9f235f47c5..0000000000 --- a/lazy_theta_star_p_planner/include/lazy_theta_star_p_planner/lazy_theta_star_p_planner.hpp +++ /dev/null @@ -1,83 +0,0 @@ -// Copyright 2020 Anshumaan Singh -// -// 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. - -#ifndef LAZY_THETA_STAR_P_PLANNER__LAZY_THETA_STAR_P_PLANNER_HPP_ -#define LAZY_THETA_STAR_P_PLANNER__LAZY_THETA_STAR_P_PLANNER_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include "rclcpp/rclcpp.hpp" -#include "nav2_core/global_planner.hpp" -#include "nav_msgs/msg/path.hpp" -#include "nav2_util/robot_utils.hpp" -#include "nav2_util/lifecycle_node.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "nav2_costmap_2d/cost_values.hpp" -#include "nav2_util/node_utils.hpp" -#include "lazy_theta_star_p_planner/lazy_theta_star_p.hpp" - -namespace lazyThetaStarP_Planner -{ - -class LazyThetaStarP_Planner : public nav2_core::GlobalPlanner -{ -public: - std::shared_ptr tf_; - nav2_util::LifecycleNode::SharedPtr node_; - - std::string globalFrame_, name_; - - void configure( - rclcpp_lifecycle::LifecycleNode::SharedPtr parent, - std::string name, std::shared_ptr tf, - std::shared_ptr costmapRos) override; - - void cleanup() override; - - void activate() override; - - void deactivate() override; - - nav_msgs::msg::Path createPlan( - const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) override; - -private: - std::unique_ptr planner_; - - void getPlan(nav_msgs::msg::Path & global_path); - - void setStartAndGoal( - const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal); - - /** - * @brief linearily interpolates between the adjacent waypoints of the path - * @param raw_path is used to send in the path recieved from the planner - * @param dist_bw_points is used to send in the interpolation_resolution - * @return the final path with waypoints at a distance of the value of interpolation_resolution of each other - */ - static nav_msgs::msg::Path linearInterpolation( - const std::vector & raw_path, - const double & dist_bw_points); -}; -} // namespace lazyThetaStarP_Planner - -#endif // LAZY_THETA_STAR_P_PLANNER__LAZY_THETA_STAR_P_PLANNER_HPP_ diff --git a/lazy_theta_star_p_planner/package.xml b/lazy_theta_star_p_planner/package.xml deleted file mode 100644 index db18a82b9e..0000000000 --- a/lazy_theta_star_p_planner/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - lazy_theta_star_p_planner - 0.4.0 - TODO: Package description - Steve Macenski - Anshumaan Singh - Apache-2.0 - ament_cmake - rclcpp - nav2_core - rclcpp_action - rclcpp_lifecycle - std_msgs - visualization_msgs - nav2_util - nav2_msgs - builtin_interfaces - tf2_ros - nav2_costmap_2d - pluginlib - ament_lint_auto - ament_lint_common - - ament_cmake - - - - diff --git a/lazy_theta_star_p_planner/src/lazy_theta_star_p.cpp b/lazy_theta_star_p_planner/src/lazy_theta_star_p.cpp deleted file mode 100644 index 79c0bc4ffa..0000000000 --- a/lazy_theta_star_p_planner/src/lazy_theta_star_p.cpp +++ /dev/null @@ -1,254 +0,0 @@ -// Copyright 2020 Anshumaan Singh -// -// 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. - -#include "lazy_theta_star_p_planner/lazy_theta_star_p.hpp" -#include - -namespace lazyThetaStarP -{ - -LazyThetaStarP::LazyThetaStarP() : costmap_tolerance_(4.0), - euc_tolerance_(6.5), - how_many_corners_(8), - size_x_(0), - size_y_(0), - index_generated(0) {} - -bool LazyThetaStarP::generatePath(std::vector & raw_path) -{ - setContainers(); - int curr_id = index_generated; - addToNodesData(index_generated); - nodes_data[curr_id] = {src.x, src.y, getTraversalCost(src.x, src.y), dist(src.x, src.y, dst.x, - dst.y), - curr_id, true, dist(src.x, src.y, dst.x, dst.y) + (getTraversalCost(src.x, src.y))}; - queue_.push({curr_id, (nodes_data[curr_id].f)}); - addIndex(nodes_data[curr_id].x, nodes_data[curr_id].y, index_generated); - index_generated++; - - int nodes_opened = 0; - - while (!queue_.empty()) { - nodes_opened++; - - if (isGoal(nodes_data[curr_id].x, nodes_data[curr_id].y)) { - break; - } - tree_node & curr_data = nodes_data[curr_id]; - resetParent(curr_data); - setNeighbors(curr_data, curr_id); - - curr_id = queue_.top().pos_id; - queue_.pop(); - } - - if (queue_.empty()) { - raw_path.clear(); - return false; - } - - backtrace(raw_path, curr_id); - clearQueue(); - - return true; -} - -void LazyThetaStarP::resetParent(tree_node & curr_data) -{ - double g_cost, los_cost = 0; - curr_data.is_in_queue = false; - tree_node & curr_par = nodes_data[curr_data.parent_id]; - tree_node & maybe_par = nodes_data[curr_par.parent_id]; - - if (losCheck(curr_data.x, curr_data.y, maybe_par.x, maybe_par.y, los_cost)) { - g_cost = maybe_par.g + - getEucledianCost(curr_data.x, curr_data.y, maybe_par.x, maybe_par.y) + - los_cost; - - if (g_cost < curr_data.g) { - curr_data.parent_id = curr_par.parent_id; - curr_data.g = g_cost; - curr_data.f = g_cost + curr_data.h; - } - } -} - -void LazyThetaStarP::setNeighbors(const tree_node & curr_data, const int & curr_id) -{ - int mx, my; - int m_id, m_par; - double g_cost, h_cost, cal_cost; - - for (int i = 0; i < how_many_corners_; i++) { - mx = curr_data.x + moves[i].x; - my = curr_data.y + moves[i].y; - - if (withinLimits(mx, my)) { - if (!isSafe(mx, my)) { - continue; - } - } else { - continue; - } - - m_par = curr_id; - g_cost = curr_data.g + getEucledianCost(curr_data.x, curr_data.y, mx, my) + - getTraversalCost(mx, my); - - getIndex(mx, my, m_id); - - if (m_id == -1) { - addToNodesData(index_generated); - m_id = index_generated; - addIndex(mx, my, m_id); - index_generated++; - } - - curr_node = &nodes_data[m_id]; - - h_cost = getEucledianCost(mx, my, dst.x, dst.y); - cal_cost = g_cost + h_cost; - - if (curr_node->f > cal_cost) { - curr_node->g = g_cost; - curr_node->h = h_cost; - curr_node->f = cal_cost; - curr_node->parent_id = m_par; - if (!curr_node->is_in_queue) { - curr_node->x = mx; - curr_node->y = my; - curr_node->is_in_queue = true; - queue_.push({m_id, (curr_node->f)}); - } - } - } -} - -void LazyThetaStarP::backtrace(std::vector & raw_points, int curr_id) -{ - std::vector path_rev; - coordsW world{}; - double pusher_ = 0.5 * costmap_->getResolution(); - do { - costmap_->mapToWorld(nodes_data[curr_id].x, nodes_data[curr_id].y, world.x, world.y); - path_rev.push_back({world.x + pusher_, world.y + pusher_}); - if (path_rev.size() > 1) { - curr_id = nodes_data[curr_id].parent_id; - } - } while (curr_id != 0); - costmap_->mapToWorld(nodes_data[curr_id].x, nodes_data[curr_id].y, world.x, world.y); - path_rev.push_back({world.x + pusher_, world.y + pusher_}); - - raw_points.reserve(path_rev.size()); - for (int i = static_cast(path_rev.size()) - 1; i >= 0; i--) { - raw_points.push_back(path_rev[i]); - } -} - -bool LazyThetaStarP::losCheck( - const int & x0, - const int & y0, - const int & x1, - const int & y1, - double & sl_cost) -{ - sl_cost = 0; - - int cx, cy; - int dy = abs(y1 - y0), dx = abs(x1 - x0), f = 0; - int sx, sy; - int lx, ly; // this variable is reserved for future use - sy = y1 > y0 ? 1 : -1; - sx = x1 > x0 ? 1 : -1; - - int u_x = (sx - 1) / 2; - int u_y = (sy - 1) / 2; - lx = x1; - ly = y1; - cx = x0; - cy = y0; - - if (dx >= dy) { - while (cx != lx) { - f += dy; - if (f >= dx) { - if (!isSafe(cx + u_x, cy + u_y, sl_cost)) { - return false; - } - cy += sy; - f -= dx; - } - if (f != 0 && !isSafe(cx + u_x, cy + u_y, sl_cost)) { - return false; - } - if (dy == 0 && !isSafe(cx + u_x, cy, sl_cost) && !isSafe(cx + u_x, cy - 1, sl_cost)) { - return false; - } - cx += sx; - } - } else { - while (cy != ly) { - f = f + dx; - if (f >= dy) { - if (!isSafe(cx + u_x, cy + u_y, sl_cost)) { - return false; - } - cx += sx; - f -= dy; - } - if (f != 0 && !isSafe(cx + u_x, cy + u_y, sl_cost)) { - return false; - } - if (dx == 0 && isSafe(cx, cy + u_y, sl_cost) && !isSafe(cx - 1, cy + u_y, sl_cost)) { - return false; - } - cy += sy; - } - } - return true; -} - -void LazyThetaStarP::setContainers() -{ - index_generated = 0; - int last_size_x = size_x_; - int last_size_y = size_y_; - int curr_size_x = static_cast(costmap_->getSizeInCellsX()); - int curr_size_y = static_cast(costmap_->getSizeInCellsY()); - if (last_size_x != curr_size_x || last_size_y != curr_size_y) { - initializePosn(curr_size_y * curr_size_x - last_size_y * last_size_x); - nodes_data.reserve(curr_size_x * curr_size_y); - } else { - initializePosn(); - } - size_x_ = curr_size_x; - size_y_ = curr_size_y; -} - -void LazyThetaStarP::initializePosn(int size_inc) -{ - int i = 0; - - if (!node_position.empty()) { - for (; i < size_x_ * size_y_; i++) { - node_position[i] = -1; - } - } - - for (; i < size_inc; i++) { - node_position.emplace_back(-1); - } -} - -} // namespace lazyThetaStarP diff --git a/lazy_theta_star_p_planner/src/lazy_theta_star_p_planner.cpp b/lazy_theta_star_p_planner/src/lazy_theta_star_p_planner.cpp deleted file mode 100644 index 794b043a95..0000000000 --- a/lazy_theta_star_p_planner/src/lazy_theta_star_p_planner.cpp +++ /dev/null @@ -1,162 +0,0 @@ -// Copyright 2020 Anshumaan Singh -// -// 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. - -#include -#include -#include -#include "lazy_theta_star_p_planner/lazy_theta_star_p_planner.hpp" -#include "lazy_theta_star_p_planner/lazy_theta_star_p.hpp" - -namespace lazyThetaStarP_Planner -{ -void LazyThetaStarP_Planner::configure( - rclcpp_lifecycle::LifecycleNode::SharedPtr parent, - std::string name, std::shared_ptr tf, - std::shared_ptr costmapRos) -{ - planner_ = std::make_unique(); - node_ = parent; - name_ = name; - tf_ = tf; - planner_->costmap_ = costmapRos->getCostmap(); - globalFrame_ = costmapRos->getGlobalFrameID(); - - nav2_util::declare_parameter_if_not_declared( - node_, name_ + ".how_many_corners", rclcpp::ParameterValue( - 8)); - - node_->get_parameter(name_ + ".how_many_corners", planner_->how_many_corners_); - - if (planner_->how_many_corners_ != 8 && planner_->how_many_corners_ != 4) { - planner_->how_many_corners_ = 8; - RCLCPP_WARN(node_->get_logger(), - "Your value for - .how_many_corners was overridden, and is now set to 8"); - } - - nav2_util::declare_parameter_if_not_declared( - node_, name_ + ".euc_tolerance", rclcpp::ParameterValue(1.5)); - - node_->get_parameter(name_ + ".euc_tolerance", planner_->euc_tolerance_); - - nav2_util::declare_parameter_if_not_declared( - node_, name_ + ".costmap_tolerance", rclcpp::ParameterValue(1.0)); - - node_->get_parameter(name_ + ".costmap_tolerance", planner_->costmap_tolerance_); -} - -void LazyThetaStarP_Planner::cleanup() -{ - RCLCPP_INFO( - node_->get_logger(), "CleaningUp plugin %s of type LazyThetaStarP_Planner", - name_.c_str()); -} - -void LazyThetaStarP_Planner::activate() -{ - RCLCPP_INFO( - node_->get_logger(), "Activating plugin %s of type LazyThetaStarP_Planner", - name_.c_str()); -} - -void LazyThetaStarP_Planner::deactivate() -{ - RCLCPP_INFO( - node_->get_logger(), "Deactivating plugin %s of type LazyThetaStarP_Planner", - name_.c_str()); -} - -nav_msgs::msg::Path LazyThetaStarP_Planner::createPlan( - const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) -{ - nav_msgs::msg::Path global_path; - setStartAndGoal(start, goal); - RCLCPP_INFO( - node_->get_logger(), "Got the src and dst... (%i, %i) && (%i, %i)", - planner_->src.x, planner_->src.y, planner_->dst.x, planner_->dst.y); - getPlan(global_path); - return global_path; -} - -void LazyThetaStarP_Planner::getPlan(nav_msgs::msg::Path & global_path) -{ - planner_->node_ = node_; - std::vector path; - - if (!(planner_->isSafe(planner_->src.x, - planner_->src.y)) || !(planner_->isSafe(planner_->dst.x, planner_->dst.y))) - { - RCLCPP_ERROR(node_->get_logger(), "EITHER OF THE START OR GOAL POINTS ARE AN OBSTACLE! "); - coordsW world{}; - planner_->costmap_->mapToWorld(planner_->src.x, planner_->src.y, world.x, world.y); - path.push_back({world.x, world.y}); - planner_->costmap_->mapToWorld(planner_->dst.x, planner_->dst.y, world.x, world.y); - path.push_back({world.x, world.y}); - global_path = linearInterpolation(path, planner_->costmap_->getResolution()); - global_path.poses.clear(); - } else if (planner_->generatePath(path)) { - RCLCPP_INFO(node_->get_logger(), "A PATH HAS BEEN GENERATED SUCCESSFULLY"); - global_path = linearInterpolation(path, planner_->costmap_->getResolution()); - } else { - RCLCPP_ERROR(node_->get_logger(), "COULD NOT GENERATE PATH"); - global_path.poses.clear(); - } - path.clear(); - global_path.header.stamp = node_->now(); - global_path.header.frame_id = globalFrame_; -} - -nav_msgs::msg::Path LazyThetaStarP_Planner::linearInterpolation( - const std::vector & raw_path, - const double & dist_bw_points) -{ - nav_msgs::msg::Path pa; - - for (unsigned int j = 0; j < raw_path.size() - 1; j++) { - geometry_msgs::msg::PoseStamped p; - coordsW pt1 = {raw_path[j].x, raw_path[j].y}; - p.pose.position.x = pt1.x; - p.pose.position.y = pt1.y; - pa.poses.push_back(p); - - coordsW pt2 = {raw_path[j + 1].x, raw_path[j + 1].y}; - geometry_msgs::msg::PoseStamped p1; - double distance = std::hypot(pt2.x - pt1.x, pt2.y - pt1.y); - int loops = static_cast(distance / dist_bw_points); - double sin_alpha = (pt2.y - pt1.y) / distance; - double cos_alpha = (pt2.x - pt1.x) / distance; - for (int k = 1; k < loops; k++) { - p1.pose.position.x = pt1.x + k * dist_bw_points * cos_alpha; - p1.pose.position.y = pt1.y + k * dist_bw_points * sin_alpha; - pa.poses.push_back(p1); - } - } - return pa; -} -void LazyThetaStarP_Planner::setStartAndGoal( - const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) -{ - unsigned int src_[2], dst_[2]; - planner_->costmap_->worldToMap(start.pose.position.x, start.pose.position.y, src_[0], src_[1]); - planner_->costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, dst_[0], dst_[1]); - - planner_->src = {static_cast(src_[0]), static_cast(src_[1])}; - planner_->dst = {static_cast(dst_[0]), static_cast(dst_[1])}; -} - -} // namespace lazyThetaStarP_Planner - -#include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(lazyThetaStarP_Planner::LazyThetaStarP_Planner, nav2_core::GlobalPlanner) From 62127dbd70a0cecfa321f410010947c5dc65572c Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Wed, 17 Feb 2021 17:33:10 +0530 Subject: [PATCH 34/81] update the files - renamed the project to nav2_theta_star_planner from lazy_theta_star_p_planner - renamed files from lazy_theta_p_planner.hpp/.cpp to theta_star_planner.hpp/.cpp and lazy_theta_star.hpp/.cpp to theta_star.hpp/.cpp - added a readme file outlining the parameters, usage notes and images to be added soon - added parameters and renamed the parameters for the cost function (costmap_tolerance -> w_traversal_cost ; euc_tolerance -> w_euc_cost ; added a parameter for the heuristic) - replaced the SharedPtr with a WeakPtr for node - removed +1 and the pusher_ variable added to compensate for it --- nav2_theta_star_planner/CMakeLists.txt | 78 ++++++ nav2_theta_star_planner/README.md | 44 +++ .../global_planner_plugin.xml | 5 + .../nav2_theta_star_planner/theta_star.hpp | 244 +++++++++++++++++ .../theta_star_planner.hpp | 89 ++++++ nav2_theta_star_planner/package.xml | 30 ++ nav2_theta_star_planner/src/theta_star.cpp | 256 ++++++++++++++++++ .../src/theta_star_planner.cpp | 167 ++++++++++++ 8 files changed, 913 insertions(+) create mode 100644 nav2_theta_star_planner/CMakeLists.txt create mode 100644 nav2_theta_star_planner/README.md create mode 100644 nav2_theta_star_planner/global_planner_plugin.xml create mode 100644 nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp create mode 100644 nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp create mode 100644 nav2_theta_star_planner/package.xml create mode 100644 nav2_theta_star_planner/src/theta_star.cpp create mode 100644 nav2_theta_star_planner/src/theta_star_planner.cpp diff --git a/nav2_theta_star_planner/CMakeLists.txt b/nav2_theta_star_planner/CMakeLists.txt new file mode 100644 index 0000000000..b2092cda84 --- /dev/null +++ b/nav2_theta_star_planner/CMakeLists.txt @@ -0,0 +1,78 @@ +cmake_minimum_required(VERSION 3.5) +project(nav2_theta_star_planner) + +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav2_util REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(ament_cmake_gtest REQUIRED) + + +nav2_package() + +include_directories( + include +) + +set(library_name ${PROJECT_NAME}_plugin) + +set(dependencies + ament_cmake + builtin_interfaces + nav2_common + nav2_core + nav2_costmap_2d + nav2_msgs + nav2_util + pluginlib + rclcpp + rclcpp_action + rclcpp_lifecycle + tf2_ros + ) + + +add_library(${library_name} SHARED + src/theta_star.cpp + src/theta_star_planner.cpp + ) + +ament_target_dependencies(${library_name} ${dependencies} ) + +target_compile_definitions(${library_name} PUBLIC "PLUGINLIB_DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file(nav2_core global_planner_plugin.xml) + +install(TARGETS ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} + ) + +install(DIRECTORY include/ + DESTINATION include/ + ) + +install(FILES global_planner_plugin.xml + DESTINATION share/${PROJECT_NAME} + ) + + +if (BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif () + + +ament_export_include_directories(include) +ament_export_libraries(${library_name}) +ament_export_dependencies(${dependencies}) +ament_package() diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md new file mode 100644 index 0000000000..5e7d806929 --- /dev/null +++ b/nav2_theta_star_planner/README.md @@ -0,0 +1,44 @@ +#Nav2 Theta Star Planner + +The Theta Star Planner is a global planning plugin meant to be used with the Nav2 Planner Server. +The `nav2_theta_star_planner` implements a highly optimized version of the Theta Star Planner (specifically the [Lazy Theta\* P variant](http://idm-lab.org/bib/abstracts/papers/aaai10b.pdf)) meant +to plan any-angle paths using A\*. Further improvements to the path quality were made by taking into account the costs from a 2D costmap. +The planner supports differential-drive and omni-directional robots. + +##Features +- Allows to tune the path quality from being taut to being smooth (this depends on the resolution of the map) +- Uses the potential field from the costmap to penalise high cost regions +- Is well suited for smaller robots of the omni-directional and differential drive kind +- As it considers the costmap traversal cost during execution it tends to smoothen the paths automatically, thus mitigating the need to smoothen the path. (The presence of sharp turns depends on the resolution of the map, and it decreases as the map resolution increases.) + + +TODO(Anshu-man567) : add images and gifs + +##Parameters + +The parameters of the planner are : +- ``` .how_many_corners ``` : to choose between 4-connected and 8-connected graph expnasions +- ``` .w_euc_cost ``` : weight applied to make the paths taut +- ``` .w_traversal_cost ``` : weight applied to steer the path away from collisions and cost +- ``` .w_heuristic_cost ``` : weight applied to push the node expansion towards the goal + +Below are the default values of the parameters : +``` +planner_server: + ros__parameters: + planner_plugin_types: ["nav2_theta_star_planner/ThetaStarPlanner"] + use_sim_time: True + planner_plugin_ids: ["GridBased"] + GridBased: + how_many_corners: 8 + w_euc_cost: 4.0 + w_traversal_cost: 6.25 + w_heuristic_cost: 1.0 +``` + +TODO (Anshu-man567) : write usage notes / help to decide when to use + + + + + diff --git a/nav2_theta_star_planner/global_planner_plugin.xml b/nav2_theta_star_planner/global_planner_plugin.xml new file mode 100644 index 0000000000..b33665c0c7 --- /dev/null +++ b/nav2_theta_star_planner/global_planner_plugin.xml @@ -0,0 +1,5 @@ + + + An implementation of the Theta* Algorithm + + diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp new file mode 100644 index 0000000000..c3620d2be5 --- /dev/null +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp @@ -0,0 +1,244 @@ +// Copyright 2020 Anshumaan Singh +// +// 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. + +#ifndef NAV2_THETA_STAR_PLANNER__THETA_STAR_HPP_ +#define NAV2_THETA_STAR_PLANNER__THETA_STAR_HPP_ + +#include +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + +const double INF_COST = DBL_MAX; +const int LETHAL_COST = 252; + +struct coordsM +{ + int x, y; +}; + +struct coordsW +{ + double x, y; +}; + +struct pos +{ + int pos_id; + double f; +}; + +struct tree_node +{ + int x, y; + double g = INF_COST; + double h = INF_COST; + int parent_id; + bool is_in_queue = false; + double f = INF_COST; +}; + +struct comp +{ + bool operator()(pos & p1, pos & p2) + { + return (p1.f) > (p2.f); + } +}; + +/// TODO(ANSHU-MAN567): try using a pointer to the tree_node vector instead of directly using it + +namespace theta_star +{ +class ThetaStar +{ +public: + coordsM src_{}, dst_{}; + nav2_costmap_2d::Costmap2D * costmap_{}; + + /// weight for the costmap traversal cost + double w_traversal_cost_; + /// weight for the euclidean distance cost (as of now used for calculations of g_cost) + double w_euc_cost_; + /// weight for the heuristic cost (for h_cost calculcations) + double w_heuristic_cost_; + + int how_many_corners_; + int size_x_, size_y_; + + ThetaStar(); + + /** + * @brief the function that iteratively searches upon the nodes in the queue (open list) until the + * current node is the goal pose or if size of queue > 0 + * @param raw_path is used to return the path obtained on exectuing the algorithm + * @return true if a path is found, false if no path is found between the start and goal pose + */ + bool generatePath(std::vector & raw_path); + + bool isSafe(const int & cx, const int & cy) const + { + return costmap_->getCost(cx, cy) < LETHAL_COST; + } + +protected: + /// for the coordinates (x,y), we store at node_position_[size_x_ * y + x], + /// the index at which the data of the node is present in nodes_data_ + std::vector node_position_; + + /// the vector nodes_data_ stores the coordinates, costs and index of the parent node, + /// and whether or not the node is present in queue_ + std::vector nodes_data_; + + /// this is the priority queue (open_list) to select the next node for expansion + std::priority_queue, comp> queue_; + + /// it is a counter like variable used to generate consecutive indices + /// such that the data for all the nodes (in open and closed lists) could be stored + /// consecutively in nodes_data_ + int index_generated_; + + const coordsM moves[8] = {{0, 1}, + {0, -1}, + {1, 0}, + {-1, 0}, + {1, -1}, + {-1, 1}, + {1, 1}, + {-1, -1}}; + + tree_node * curr_node = new tree_node; + /** @brief it does a line of sight (los) check between the current node and the parent of its parent node + * if an los is found and the new costs calculated are lesser then the cost and parent node of the current node + * is updated + * @param data of the current node + */ + void resetParent(tree_node & curr_data); + + /** + * @brief this function expands the neighbors of the current node + * @param curr_data used to send the data of the current node + * @param curr_id used to send the index of the current node as stored in nodes_position + */ + void setNeighbors(const tree_node & curr_data, const int & curr_int); + + /** + * @brief it returns the path by backtracing from the goal to the start, by using their parent nodes + * @param raw_points used to return the path thus found + * @param curr_id sends in the index of the goal coordinate, as stored in nodes_position + */ + void backtrace(std::vector & raw_points, int curr_id); + + /** + * @brief performs the line of sight check using Bresenham's Algorithm, + * and has been modified to calculate the traversal cost incurred in a straight line path between + * the two points whose coordinates are (x0, y0) and (x1, y1) + * @param sl_cost is used to return the cost thus incurred + * @return true if a line of sight exists between the points + */ + bool losCheck( + const int & x0, const int & y0, const int & x1, const int & y1, + double & sl_cost); + + void initializePosn(int size_inc = 0); + + void setContainers(); + + double dist(const int & ax, const int & ay, const int & bx, const int & by) + { + return std::hypot(ax - bx, ay - by); + } + + double getEuclideanCost(const int & ax, const int & ay, const int & bx, const int & by) + { + return w_euc_cost_ * dist(ax, ay, bx, by); + } + + double getCellCost(const int & cx, const int & cy) const + { + return /*50 + 0.8 * */ costmap_->getCost(cx, cy); + } + + /** + * @brief for the point(cx, cy) its traversal cost is calculated by *()^2/()^2 + * @return the traversal cost thus calculated + */ + double getTraversalCost(const int & cx, const int & cy) + { + double curr_cost = getCellCost(cx, cy); + return w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST; + } + + double getHCost(const int & mx, const int & my) + { + return w_heuristic_cost_ * dist(mx, my, dst_.x, dst_.y); + } + + /** + * @brief it is an overloaded function to ease in cost calculations while performing the LOS check + * @param cx + * @param cy + * @param cost denotes the total straight line traversal cost, adds the traversal cost for the node (cx, cy) at every instance + */ + bool isSafe(const int & cx, const int & cy, double & cost) const + { + double curr_cost = getCellCost(cx, cy); + if (curr_cost < LETHAL_COST) { + cost += w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST; + return true; + } else { + return false; + } + } + + void addIndex(const int & cx, const int & cy, const int & id_this) + { + node_position_[size_x_ * cy + cx] = id_this; + } + + void getIndex(const int & cx, const int & cy, int & id_this) + { + id_this = node_position_[size_x_ * cy + cx]; + } + + bool withinLimits(const int & cx, const int & cy) const + { + return cx >= 0 && cx < size_x_ && cy >= 0 && cy < size_y_; + } + + bool isGoal(const tree_node & this_node) const + { + return this_node.x == dst_.x && this_node.y == dst_.y; + } + + void addToNodesData(const int & id_this) + { + if (nodes_data_.size() <= static_cast(id_this)) { + nodes_data_.push_back({}); + } else { + nodes_data_[id_this] = {}; + } + } + + void clearQueue() + { + queue_ = std::priority_queue, comp>(); + } +}; +} // namespace theta_star + +#endif // NAV2_THETA_STAR_PLANNER__THETA_STAR_HPP_ diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp new file mode 100644 index 0000000000..4a2ce28dbe --- /dev/null +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp @@ -0,0 +1,89 @@ +// Copyright 2020 Anshumaan Singh +// +// 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. + +#ifndef NAV2_THETA_STAR_PLANNER__THETA_STAR_PLANNER_HPP_ +#define NAV2_THETA_STAR_PLANNER__THETA_STAR_PLANNER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "nav2_core/global_planner.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_theta_star_planner/theta_star.hpp" + +namespace nav2_theta_star_planner +{ + +class ThetaStarPlanner : public nav2_core::GlobalPlanner +{ +public: + void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) override; + + void cleanup() override; + + void activate() override; + + void deactivate() override; + + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) override; + +protected: + std::shared_ptr tf_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_{rclcpp::get_logger("SmacPlanner2D")}; + std::string global_frame_, name_; + + std::unique_ptr planner_; + + void getPlan(nav_msgs::msg::Path & global_path); + + void setStartAndGoal( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal); + + /** + * @brief linearily interpolates between the adjacent waypoints of the path + * @param raw_path is used to send in the path recieved from the planner + * @param dist_bw_points is used to send in the interpolation_resolution + * @return the final path with waypoints at a distance of the value of interpolation_resolution of each other + */ + static nav_msgs::msg::Path linearInterpolation( + const std::vector & raw_path, + const double & dist_bw_points); + + bool isSafeToPlan() const + { + return !(planner_->isSafe(planner_->src_.x, + planner_->src_.y)) || !(planner_->isSafe(planner_->dst_.x, planner_->dst_.y)); + } +}; +} // namespace nav2_theta_star_planner + +#endif // NAV2_THETA_STAR_PLANNER__THETA_STAR_PLANNER_HPP_ diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml new file mode 100644 index 0000000000..27c8c8854a --- /dev/null +++ b/nav2_theta_star_planner/package.xml @@ -0,0 +1,30 @@ + + + + nav2_theta_star_planner + 0.4.0 + Theta* Global Planning Plugin + Steve Macenski + Anshumaan Singh + Apache-2.0 + ament_cmake + rclcpp + nav2_core + rclcpp_action + rclcpp_lifecycle + std_msgs + visualization_msgs + nav2_util + nav2_msgs + builtin_interfaces + tf2_ros + nav2_costmap_2d + pluginlib + ament_lint_auto + ament_lint_common + + ament_cmake + + + + diff --git a/nav2_theta_star_planner/src/theta_star.cpp b/nav2_theta_star_planner/src/theta_star.cpp new file mode 100644 index 0000000000..950b4c7704 --- /dev/null +++ b/nav2_theta_star_planner/src/theta_star.cpp @@ -0,0 +1,256 @@ +// Copyright 2020 Anshumaan Singh +// +// 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. + +#include +#include "nav2_theta_star_planner/theta_star.hpp" + +namespace theta_star +{ + +ThetaStar::ThetaStar() +: w_traversal_cost_(4.0), + w_euc_cost_(6.25), + w_heuristic_cost_(1.0), + how_many_corners_(8), + size_x_(0), + size_y_(0), + index_generated_(0) {} + +bool ThetaStar::generatePath(std::vector & raw_path) +{ + setContainers(); + int curr_id = index_generated_; + addToNodesData(index_generated_); + double src_g_cost = getTraversalCost(src_.x, src_.y), src_h_cost = getHCost(src_.x, src_.y); + nodes_data_[curr_id] = {src_.x, src_.y, src_g_cost, src_h_cost, + curr_id, true, src_g_cost + src_h_cost}; + queue_.push({curr_id, (nodes_data_[curr_id].f)}); + addIndex(nodes_data_[curr_id].x, nodes_data_[curr_id].y, index_generated_); + index_generated_++; + + int nodes_opened = 0; + + while (!queue_.empty()) { + nodes_opened++; + + tree_node & curr_data = nodes_data_[curr_id]; + if (isGoal(curr_data)) { + break; + } + + resetParent(curr_data); + setNeighbors(curr_data, curr_id); + + curr_id = queue_.top().pos_id; + queue_.pop(); + } + + if (queue_.empty()) { + raw_path.clear(); + return false; + } + + backtrace(raw_path, curr_id); + clearQueue(); + + return true; +} + +void ThetaStar::resetParent(tree_node & curr_data) +{ + double g_cost, los_cost = 0; + curr_data.is_in_queue = false; + tree_node & curr_par = nodes_data_[curr_data.parent_id]; + tree_node & maybe_par = nodes_data_[curr_par.parent_id]; + + if (losCheck(curr_data.x, curr_data.y, maybe_par.x, maybe_par.y, los_cost)) { + g_cost = maybe_par.g + + getEuclideanCost(curr_data.x, curr_data.y, maybe_par.x, maybe_par.y) + + los_cost; + + if (g_cost < curr_data.g) { + curr_data.parent_id = curr_par.parent_id; + curr_data.g = g_cost; + curr_data.f = g_cost + curr_data.h; + } + } +} + +void ThetaStar::setNeighbors(const tree_node & curr_data, const int & curr_id) +{ + int mx, my; + int m_id, m_par; + double g_cost, h_cost, cal_cost; + + for (int i = 0; i < how_many_corners_; i++) { + mx = curr_data.x + moves[i].x; + my = curr_data.y + moves[i].y; + + if (withinLimits(mx, my)) { + if (!isSafe(mx, my)) { + continue; + } + } else { + continue; + } + + m_par = curr_id; + g_cost = curr_data.g + getEuclideanCost(curr_data.x, curr_data.y, mx, my) + getTraversalCost(mx, + my); + + getIndex(mx, my, m_id); + + if (m_id == -1) { + addToNodesData(index_generated_); + m_id = index_generated_; + addIndex(mx, my, m_id); + index_generated_++; + } + + curr_node = &nodes_data_[m_id]; + + h_cost = getHCost(mx, my); + cal_cost = g_cost + h_cost; + + if (curr_node->f > cal_cost) { + curr_node->g = g_cost; + curr_node->h = h_cost; + curr_node->f = cal_cost; + curr_node->parent_id = m_par; + if (!curr_node->is_in_queue) { + curr_node->x = mx; + curr_node->y = my; + curr_node->is_in_queue = true; + queue_.push({m_id, (curr_node->f)}); + } + } + } +} + +void ThetaStar::backtrace(std::vector & raw_points, int curr_id) +{ + std::vector path_rev; + coordsW world{}; + do { + costmap_->mapToWorld(nodes_data_[curr_id].x, nodes_data_[curr_id].y, world.x, world.y); + path_rev.push_back({world.x, world.y}); + if (path_rev.size() > 1) { + curr_id = nodes_data_[curr_id].parent_id; + } + } while (curr_id != 0); + costmap_->mapToWorld(nodes_data_[curr_id].x, nodes_data_[curr_id].y, world.x, world.y); + path_rev.push_back({world.x, world.y}); + + raw_points.reserve(path_rev.size()); + for (int i = static_cast(path_rev.size()) - 1; i >= 0; i--) { + raw_points.push_back(path_rev[i]); + } +} + +bool ThetaStar::losCheck( + const int & x0, + const int & y0, + const int & x1, + const int & y1, + double & sl_cost) +{ + sl_cost = 0; + + int cx, cy; + int dy = abs(y1 - y0), dx = abs(x1 - x0), f = 0; + int sx, sy; + int lx, ly; // this variable is reserved for future use + sy = y1 > y0 ? 1 : -1; + sx = x1 > x0 ? 1 : -1; + + int u_x = (sx - 1) / 2; + int u_y = (sy - 1) / 2; + lx = x1; + ly = y1; + cx = x0; + cy = y0; + + if (dx >= dy) { + while (cx != lx) { + f += dy; + if (f >= dx) { + if (!isSafe(cx + u_x, cy + u_y, sl_cost)) { + return false; + } + cy += sy; + f -= dx; + } + if (f != 0 && !isSafe(cx + u_x, cy + u_y, sl_cost)) { + return false; + } + if (dy == 0 && !isSafe(cx + u_x, cy, sl_cost) && !isSafe(cx + u_x, cy - 1, sl_cost)) { + return false; + } + cx += sx; + } + } else { + while (cy != ly) { + f = f + dx; + if (f >= dy) { + if (!isSafe(cx + u_x, cy + u_y, sl_cost)) { + return false; + } + cx += sx; + f -= dy; + } + if (f != 0 && !isSafe(cx + u_x, cy + u_y, sl_cost)) { + return false; + } + if (dx == 0 && isSafe(cx, cy + u_y, sl_cost) && !isSafe(cx - 1, cy + u_y, sl_cost)) { + return false; + } + cy += sy; + } + } + return true; +} + +void ThetaStar::setContainers() +{ + index_generated_ = 0; + int last_size_x = size_x_; + int last_size_y = size_y_; + int curr_size_x = static_cast(costmap_->getSizeInCellsX()); + int curr_size_y = static_cast(costmap_->getSizeInCellsY()); + if (last_size_x != curr_size_x || last_size_y != curr_size_y) { + initializePosn(curr_size_y * curr_size_x - last_size_y * last_size_x); + nodes_data_.reserve(curr_size_x * curr_size_y); + } else { + initializePosn(); + } + size_x_ = curr_size_x; + size_y_ = curr_size_y; +} + +void ThetaStar::initializePosn(int size_inc) +{ + int i = 0; + + if (!node_position_.empty()) { + for (; i < size_x_ * size_y_; i++) { + node_position_[i] = -1; + } + } + + for (; i < size_inc; i++) { + node_position_.emplace_back(-1); + } +} + +} // namespace theta_star diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp new file mode 100644 index 0000000000..7f0f7b4e4d --- /dev/null +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -0,0 +1,167 @@ +// Copyright 2020 Anshumaan Singh +// +// 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. + +#include +#include +#include +#include "nav2_theta_star_planner/theta_star_planner.hpp" +#include "nav2_theta_star_planner/theta_star.hpp" + +namespace nav2_theta_star_planner +{ +void ThetaStarPlanner::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, std::shared_ptr tf, + std::shared_ptr costmap_ros) +{ + planner_ = std::make_unique(); + auto node_ = parent.lock(); + logger_ = node_->get_logger(); + clock_ = node_->get_clock(); + name_ = name; + tf_ = tf; + planner_->costmap_ = costmap_ros->getCostmap(); + global_frame_ = costmap_ros->getGlobalFrameID(); + + nav2_util::declare_parameter_if_not_declared( + node_, name_ + ".how_many_corners", rclcpp::ParameterValue( + 8)); + + node_->get_parameter(name_ + ".how_many_corners", planner_->how_many_corners_); + + if (planner_->how_many_corners_ != 8 && planner_->how_many_corners_ != 4) { + planner_->how_many_corners_ = 8; + RCLCPP_WARN(logger_, + "Your value for - .how_many_corners was overridden, and is now set to 8"); + } + + nav2_util::declare_parameter_if_not_declared( + node_, name_ + ".w_euc_cost", rclcpp::ParameterValue(4.0)); + node_->get_parameter(name_ + ".w_euc_cost", planner_->w_euc_cost_); + + nav2_util::declare_parameter_if_not_declared( + node_, name_ + ".w_traversal_cost", rclcpp::ParameterValue(6.25)); + node_->get_parameter(name_ + ".w_traversal_cost", planner_->w_traversal_cost_); + + nav2_util::declare_parameter_if_not_declared( + node_, name_ + ".w_heuristic_cost", rclcpp::ParameterValue(1.0)); + node_->get_parameter(name_ + ".w_heuristic_cost", planner_->w_heuristic_cost_); +} + +void ThetaStarPlanner::cleanup() +{ + RCLCPP_INFO( + logger_, "CleaningUp plugin %s of type nav2_theta_star_planner", + name_.c_str()); +} + +void ThetaStarPlanner::activate() +{ + RCLCPP_INFO( + logger_, "Activating plugin %s of type nav2_theta_star_planner", + name_.c_str()); +} + +void ThetaStarPlanner::deactivate() +{ + RCLCPP_INFO( + logger_, "Deactivating plugin %s of type nav2_theta_star_planner", + name_.c_str()); +} + +nav_msgs::msg::Path ThetaStarPlanner::createPlan( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) +{ + nav_msgs::msg::Path global_path; + auto start_time = std::chrono::steady_clock::now(); + setStartAndGoal(start, goal); + RCLCPP_INFO( + logger_, "Got the src_ and dst_... (%i, %i) && (%i, %i)", + planner_->src_.x, planner_->src_.y, planner_->dst_.x, planner_->dst_.y); + getPlan(global_path); + auto stop_time = std::chrono::steady_clock::now(); + auto dur = std::chrono::duration_cast(stop_time - start_time); + RCLCPP_INFO(logger_, "the time taken is : %i", dur.count()); + return global_path; +} + +void ThetaStarPlanner::getPlan(nav_msgs::msg::Path & global_path) +{ + std::vector path; + + if (isSafeToPlan()) { + RCLCPP_ERROR(logger_, "EITHER OF THE START OR GOAL POINTS ARE AN OBSTACLE! "); + coordsW world{}; + planner_->costmap_->mapToWorld(planner_->src_.x, planner_->src_.y, world.x, world.y); + path.push_back({world.x, world.y}); + planner_->costmap_->mapToWorld(planner_->dst_.x, planner_->dst_.y, world.x, world.y); + path.push_back({world.x, world.y}); + global_path = linearInterpolation(path, planner_->costmap_->getResolution()); + global_path.poses.clear(); + } else if (planner_->generatePath(path)) { + RCLCPP_INFO(logger_, "A PATH HAS BEEN GENERATED SUCCESSFULLY"); + global_path = linearInterpolation(path, planner_->costmap_->getResolution()); + } else { + RCLCPP_ERROR(logger_, "COULD NOT GENERATE PATH"); + global_path.poses.clear(); + } + path.clear(); + global_path.header.stamp = clock_->now(); + global_path.header.frame_id = global_frame_; +} + +nav_msgs::msg::Path ThetaStarPlanner::linearInterpolation( + const std::vector & raw_path, + const double & dist_bw_points) +{ + nav_msgs::msg::Path pa; + + for (unsigned int j = 0; j < raw_path.size() - 1; j++) { + geometry_msgs::msg::PoseStamped p; + coordsW pt1 = {raw_path[j].x, raw_path[j].y}; + p.pose.position.x = pt1.x; + p.pose.position.y = pt1.y; + pa.poses.push_back(p); + + coordsW pt2 = {raw_path[j + 1].x, raw_path[j + 1].y}; + geometry_msgs::msg::PoseStamped p1; + double distance = std::hypot(pt2.x - pt1.x, pt2.y - pt1.y); + int loops = static_cast(distance / dist_bw_points); + double sin_alpha = (pt2.y - pt1.y) / distance; + double cos_alpha = (pt2.x - pt1.x) / distance; + for (int k = 1; k < loops; k++) { + p1.pose.position.x = pt1.x + k * dist_bw_points * cos_alpha; + p1.pose.position.y = pt1.y + k * dist_bw_points * sin_alpha; + pa.poses.push_back(p1); + } + } + return pa; +} +void ThetaStarPlanner::setStartAndGoal( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) +{ + unsigned int src_[2], dst_[2]; + planner_->costmap_->worldToMap(start.pose.position.x, start.pose.position.y, src_[0], src_[1]); + planner_->costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, dst_[0], dst_[1]); + + planner_->src_ = {static_cast(src_[0]), static_cast(src_[1])}; + planner_->dst_ = {static_cast(dst_[0]), static_cast(dst_[1])}; +} + +} // namespace nav2_theta_star_planner + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_theta_star_planner::ThetaStarPlanner, nav2_core::GlobalPlanner) From 19a8a0141efe98e732353cdd8c99fad4023f2c95 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Wed, 17 Feb 2021 17:34:16 +0530 Subject: [PATCH 35/81] Update README.md --- nav2_theta_star_planner/README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index 5e7d806929..980877b9e1 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -1,11 +1,11 @@ -#Nav2 Theta Star Planner +# Nav2 Theta Star Planner The Theta Star Planner is a global planning plugin meant to be used with the Nav2 Planner Server. The `nav2_theta_star_planner` implements a highly optimized version of the Theta Star Planner (specifically the [Lazy Theta\* P variant](http://idm-lab.org/bib/abstracts/papers/aaai10b.pdf)) meant to plan any-angle paths using A\*. Further improvements to the path quality were made by taking into account the costs from a 2D costmap. The planner supports differential-drive and omni-directional robots. -##Features +## Features - Allows to tune the path quality from being taut to being smooth (this depends on the resolution of the map) - Uses the potential field from the costmap to penalise high cost regions - Is well suited for smaller robots of the omni-directional and differential drive kind @@ -14,7 +14,7 @@ The planner supports differential-drive and omni-directional robots. TODO(Anshu-man567) : add images and gifs -##Parameters +## Parameters The parameters of the planner are : - ``` .how_many_corners ``` : to choose between 4-connected and 8-connected graph expnasions From 3c4ef8730da51a36a1b1df5867abfedf2fb364a3 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Thu, 18 Feb 2021 07:03:03 +0530 Subject: [PATCH 36/81] update the code - linted the code - **updates to readme, are still pending** - changed the type of message from INFO to DEBUG - replaced the capital letters with the smaller ones --- nav2_theta_star_planner/CMakeLists.txt | 58 +++++++++---------- nav2_theta_star_planner/README.md | 10 ++-- .../nav2_theta_star_planner/theta_star.hpp | 4 +- .../theta_star_planner.hpp | 2 +- nav2_theta_star_planner/package.xml | 1 + nav2_theta_star_planner/src/theta_star.cpp | 4 +- .../src/theta_star_planner.cpp | 48 +++++++-------- .../theta_star_planner.xml | 5 ++ 8 files changed, 65 insertions(+), 67 deletions(-) create mode 100644 nav2_theta_star_planner/theta_star_planner.xml diff --git a/nav2_theta_star_planner/CMakeLists.txt b/nav2_theta_star_planner/CMakeLists.txt index b2092cda84..738faced51 100644 --- a/nav2_theta_star_planner/CMakeLists.txt +++ b/nav2_theta_star_planner/CMakeLists.txt @@ -19,51 +19,51 @@ find_package(ament_cmake_gtest REQUIRED) nav2_package() include_directories( - include + include ) set(library_name ${PROJECT_NAME}_plugin) set(dependencies - ament_cmake - builtin_interfaces - nav2_common - nav2_core - nav2_costmap_2d - nav2_msgs - nav2_util - pluginlib - rclcpp - rclcpp_action - rclcpp_lifecycle - tf2_ros - ) + ament_cmake + builtin_interfaces + nav2_common + nav2_core + nav2_costmap_2d + nav2_msgs + nav2_util + pluginlib + rclcpp + rclcpp_action + rclcpp_lifecycle + tf2_ros + ) add_library(${library_name} SHARED - src/theta_star.cpp - src/theta_star_planner.cpp - ) + src/theta_star.cpp + src/theta_star_planner.cpp + ) -ament_target_dependencies(${library_name} ${dependencies} ) +ament_target_dependencies(${library_name} ${dependencies}) -target_compile_definitions(${library_name} PUBLIC "PLUGINLIB_DISABLE_BOOST_FUNCTIONS") +target_compile_definitions(${library_name} PUBLIC "PLUGINLIB_DISABLE_BOOST_FUNCTIONS") -pluginlib_export_plugin_description_file(nav2_core global_planner_plugin.xml) +pluginlib_export_plugin_description_file(nav2_core theta_star_planner.xml) install(TARGETS ${library_name} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} - ) + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} + ) install(DIRECTORY include/ - DESTINATION include/ - ) + DESTINATION include/ + ) -install(FILES global_planner_plugin.xml - DESTINATION share/${PROJECT_NAME} - ) +install(FILES theta_star_planner.xml + DESTINATION share/${PROJECT_NAME} + ) if (BUILD_TESTING) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index 980877b9e1..202a446830 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -1,20 +1,20 @@ -# Nav2 Theta Star Planner +#Nav2 Theta Star Planner The Theta Star Planner is a global planning plugin meant to be used with the Nav2 Planner Server. The `nav2_theta_star_planner` implements a highly optimized version of the Theta Star Planner (specifically the [Lazy Theta\* P variant](http://idm-lab.org/bib/abstracts/papers/aaai10b.pdf)) meant to plan any-angle paths using A\*. Further improvements to the path quality were made by taking into account the costs from a 2D costmap. The planner supports differential-drive and omni-directional robots. -## Features +##Features - Allows to tune the path quality from being taut to being smooth (this depends on the resolution of the map) - Uses the potential field from the costmap to penalise high cost regions - Is well suited for smaller robots of the omni-directional and differential drive kind - As it considers the costmap traversal cost during execution it tends to smoothen the paths automatically, thus mitigating the need to smoothen the path. (The presence of sharp turns depends on the resolution of the map, and it decreases as the map resolution increases.) -TODO(Anshu-man567) : add images and gifs + -## Parameters +##Parameters The parameters of the planner are : - ``` .how_many_corners ``` : to choose between 4-connected and 8-connected graph expnasions @@ -36,7 +36,7 @@ planner_server: w_heuristic_cost: 1.0 ``` -TODO (Anshu-man567) : write usage notes / help to decide when to use + diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp index c3620d2be5..57da18ae69 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp @@ -207,7 +207,7 @@ class ThetaStar void addIndex(const int & cx, const int & cy, const int & id_this) { - node_position_[size_x_ * cy + cx] = id_this; + node_position_[size_x_ * cy + cx] = id_this; } void getIndex(const int & cx, const int & cy, int & id_this) @@ -230,7 +230,7 @@ class ThetaStar if (nodes_data_.size() <= static_cast(id_this)) { nodes_data_.push_back({}); } else { - nodes_data_[id_this] = {}; + nodes_data_[id_this] = {}; } } diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp index 4a2ce28dbe..23e011eff6 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp @@ -57,7 +57,7 @@ class ThetaStarPlanner : public nav2_core::GlobalPlanner protected: std::shared_ptr tf_; rclcpp::Clock::SharedPtr clock_; - rclcpp::Logger logger_{rclcpp::get_logger("SmacPlanner2D")}; + rclcpp::Logger logger_{rclcpp::get_logger("ThetaStarPlanner")}; std::string global_frame_, name_; std::unique_ptr planner_; diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index 27c8c8854a..7624bfebe1 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -7,6 +7,7 @@ Steve Macenski Anshumaan Singh Apache-2.0 + ament_cmake rclcpp nav2_core diff --git a/nav2_theta_star_planner/src/theta_star.cpp b/nav2_theta_star_planner/src/theta_star.cpp index 950b4c7704..28e9c3f79d 100644 --- a/nav2_theta_star_planner/src/theta_star.cpp +++ b/nav2_theta_star_planner/src/theta_star.cpp @@ -34,7 +34,7 @@ bool ThetaStar::generatePath(std::vector & raw_path) addToNodesData(index_generated_); double src_g_cost = getTraversalCost(src_.x, src_.y), src_h_cost = getHCost(src_.x, src_.y); nodes_data_[curr_id] = {src_.x, src_.y, src_g_cost, src_h_cost, - curr_id, true, src_g_cost + src_h_cost}; + curr_id, true, src_g_cost + src_h_cost}; queue_.push({curr_id, (nodes_data_[curr_id].f)}); addIndex(nodes_data_[curr_id].x, nodes_data_[curr_id].y, index_generated_); index_generated_++; @@ -244,7 +244,7 @@ void ThetaStar::initializePosn(int size_inc) if (!node_position_.empty()) { for (; i < size_x_ * size_y_; i++) { - node_position_[i] = -1; + node_position_[i] = -1; } } diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index 7f0f7b4e4d..4b29b8e472 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -26,58 +26,52 @@ void ThetaStarPlanner::configure( std::shared_ptr costmap_ros) { planner_ = std::make_unique(); - auto node_ = parent.lock(); - logger_ = node_->get_logger(); - clock_ = node_->get_clock(); + auto node = parent.lock(); + logger_ = node->get_logger(); + clock_ = node->get_clock(); name_ = name; tf_ = tf; planner_->costmap_ = costmap_ros->getCostmap(); global_frame_ = costmap_ros->getGlobalFrameID(); nav2_util::declare_parameter_if_not_declared( - node_, name_ + ".how_many_corners", rclcpp::ParameterValue( + node, name_ + ".how_many_corners", rclcpp::ParameterValue( 8)); - node_->get_parameter(name_ + ".how_many_corners", planner_->how_many_corners_); + node->get_parameter(name_ + ".how_many_corners", planner_->how_many_corners_); if (planner_->how_many_corners_ != 8 && planner_->how_many_corners_ != 4) { planner_->how_many_corners_ = 8; RCLCPP_WARN(logger_, - "Your value for - .how_many_corners was overridden, and is now set to 8"); + "Your value for - .how_many_corners was overridden, and is now set to 8"); } nav2_util::declare_parameter_if_not_declared( - node_, name_ + ".w_euc_cost", rclcpp::ParameterValue(4.0)); - node_->get_parameter(name_ + ".w_euc_cost", planner_->w_euc_cost_); + node, name_ + ".w_euc_cost", rclcpp::ParameterValue(4.0)); + node->get_parameter(name_ + ".w_euc_cost", planner_->w_euc_cost_); nav2_util::declare_parameter_if_not_declared( - node_, name_ + ".w_traversal_cost", rclcpp::ParameterValue(6.25)); - node_->get_parameter(name_ + ".w_traversal_cost", planner_->w_traversal_cost_); + node, name_ + ".w_traversal_cost", rclcpp::ParameterValue(6.25)); + node->get_parameter(name_ + ".w_traversal_cost", planner_->w_traversal_cost_); nav2_util::declare_parameter_if_not_declared( - node_, name_ + ".w_heuristic_cost", rclcpp::ParameterValue(1.0)); - node_->get_parameter(name_ + ".w_heuristic_cost", planner_->w_heuristic_cost_); + node, name_ + ".w_heuristic_cost", rclcpp::ParameterValue(1.0)); + node->get_parameter(name_ + ".w_heuristic_cost", planner_->w_heuristic_cost_); } void ThetaStarPlanner::cleanup() { - RCLCPP_INFO( - logger_, "CleaningUp plugin %s of type nav2_theta_star_planner", - name_.c_str()); + RCLCPP_INFO(logger_, "CleaningUp plugin %s of type nav2_theta_star_planner", name_.c_str()); } void ThetaStarPlanner::activate() { - RCLCPP_INFO( - logger_, "Activating plugin %s of type nav2_theta_star_planner", - name_.c_str()); + RCLCPP_INFO(logger_, "Activating plugin %s of type nav2_theta_star_planner", name_.c_str()); } void ThetaStarPlanner::deactivate() { - RCLCPP_INFO( - logger_, "Deactivating plugin %s of type nav2_theta_star_planner", - name_.c_str()); + RCLCPP_INFO(logger_, "Deactivating plugin %s of type nav2_theta_star_planner", name_.c_str()); } nav_msgs::msg::Path ThetaStarPlanner::createPlan( @@ -87,13 +81,12 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( nav_msgs::msg::Path global_path; auto start_time = std::chrono::steady_clock::now(); setStartAndGoal(start, goal); - RCLCPP_INFO( - logger_, "Got the src_ and dst_... (%i, %i) && (%i, %i)", - planner_->src_.x, planner_->src_.y, planner_->dst_.x, planner_->dst_.y); + RCLCPP_DEBUG(logger_, "Got the src_ and dst_... (%i, %i) && (%i, %i)", + planner_->src_.x, planner_->src_.y, planner_->dst_.x, planner_->dst_.y); getPlan(global_path); auto stop_time = std::chrono::steady_clock::now(); auto dur = std::chrono::duration_cast(stop_time - start_time); - RCLCPP_INFO(logger_, "the time taken is : %i", dur.count()); + RCLCPP_DEBUG(logger_, "the time taken is : %i", dur.count()); return global_path; } @@ -102,7 +95,7 @@ void ThetaStarPlanner::getPlan(nav_msgs::msg::Path & global_path) std::vector path; if (isSafeToPlan()) { - RCLCPP_ERROR(logger_, "EITHER OF THE START OR GOAL POINTS ARE AN OBSTACLE! "); + RCLCPP_ERROR(logger_, "Either of the start or goal pose are an obstacle! "); coordsW world{}; planner_->costmap_->mapToWorld(planner_->src_.x, planner_->src_.y, world.x, world.y); path.push_back({world.x, world.y}); @@ -111,10 +104,9 @@ void ThetaStarPlanner::getPlan(nav_msgs::msg::Path & global_path) global_path = linearInterpolation(path, planner_->costmap_->getResolution()); global_path.poses.clear(); } else if (planner_->generatePath(path)) { - RCLCPP_INFO(logger_, "A PATH HAS BEEN GENERATED SUCCESSFULLY"); global_path = linearInterpolation(path, planner_->costmap_->getResolution()); } else { - RCLCPP_ERROR(logger_, "COULD NOT GENERATE PATH"); + RCLCPP_ERROR(logger_, "Could not generate path between the given poses"); global_path.poses.clear(); } path.clear(); diff --git a/nav2_theta_star_planner/theta_star_planner.xml b/nav2_theta_star_planner/theta_star_planner.xml new file mode 100644 index 0000000000..bdedae4a22 --- /dev/null +++ b/nav2_theta_star_planner/theta_star_planner.xml @@ -0,0 +1,5 @@ + + + An implementation of the Theta* Algorithm + + From 8febae28b956ca8c7c542db63a387de0e43083c1 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Tue, 30 Mar 2021 20:26:41 +0530 Subject: [PATCH 37/81] update the readme file --- nav2_theta_star_planner/README.md | 81 +++++++++--- nav2_theta_star_planner/img/00-37.png | Bin 0 -> 115135 bytes .../nav2_theta_star_planner/theta_star.hpp | 118 +++++++++++++----- .../theta_star_planner.hpp | 20 ++- nav2_theta_star_planner/src/theta_star.cpp | 32 ++--- .../src/theta_star_planner.cpp | 13 +- .../theta_star_planner.xml | 2 +- 7 files changed, 188 insertions(+), 78 deletions(-) create mode 100644 nav2_theta_star_planner/img/00-37.png diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index 202a446830..fecd798320 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -1,26 +1,56 @@ #Nav2 Theta Star Planner +The Theta Star Planner is a global planning plugin meant to be used with the Nav2 Planner Server. The `nav2_theta_star_planner` implements a highly optimized version of the Theta Star Planner (specifically the [Lazy Theta\* P variant](http://idm-lab.org/bib/abstracts/papers/aaai10b.pdf)) meant to plan any-angle paths using A\*. The planner supports differential-drive and omni-directional robots. -The Theta Star Planner is a global planning plugin meant to be used with the Nav2 Planner Server. -The `nav2_theta_star_planner` implements a highly optimized version of the Theta Star Planner (specifically the [Lazy Theta\* P variant](http://idm-lab.org/bib/abstracts/papers/aaai10b.pdf)) meant -to plan any-angle paths using A\*. Further improvements to the path quality were made by taking into account the costs from a 2D costmap. -The planner supports differential-drive and omni-directional robots. - -##Features -- Allows to tune the path quality from being taut to being smooth (this depends on the resolution of the map) -- Uses the potential field from the costmap to penalise high cost regions +##Features +- Uses the costs from the costmap to penalise high cost regions - Is well suited for smaller robots of the omni-directional and differential drive kind - As it considers the costmap traversal cost during execution it tends to smoothen the paths automatically, thus mitigating the need to smoothen the path. (The presence of sharp turns depends on the resolution of the map, and it decreases as the map resolution increases.) +- Allows to control the path behavior to either be any angle directed or to be in the middle of the spaces +- The planner uses A\* search along with line of sight checks to form any-angle paths thus avoiding zig-zag paths that may be present in the usual implementation of A\* +##Metrics +For the below example the planner took ~44ms to compute the path of 87.5m - +![example.png](/img/Screenshot%20from%202021-03-27%2000-37-47.png) - - -##Parameters +The parameters were set to - `w_euc_cost: 1.0`, `w_traversal_cost: 5.0`, `w_heuristic_cost: 1.0` and the `global_costmap`'s `inflation_layer` parameters are set as - `cost_scaling_factor:5.0`, `inflation_radius: 5.5` + +##Cost Function Details +###Symbols and their meanings +**g(a)** - cost function cost for the node 'a' + +**h(a)** - heuristic function cost for the node 'a' + +**f(a)** - total cost (g(a) + h(a)) for the node 'a' + +**LETHAL_COST** - a value of the costmap traversal cost that inscribes an obstacle with +respect to a function, value = 253 + +**curr** - represents the node whose neighbours are being added to the list + +**neigh** - one of the neighboring nodes of curr + +**par** - parent node of curr + +**euc_cost(a,b)** - euclidean distance between the node type 'a' and type 'b' + +**costmap_cost(a,b)** - the costmap traversal cost (ranges from 0 - 252, 254 = unknown value) between the node type 'a' and type 'b' +### Cost function +``` +g(neigh) = g(curr) + w_euc_cost*euc_cost(curr, neigh) + w_traversal_cost*(costmap_cost(curr,neigh)/LETHAL_COST)^2 +h(neigh) = w_heuristic_cost * euc_cost(neigh, par) +f(neigh) = g(neigh) + h(neigh) +``` +Because of how the program works when the 'neigh' node is to be expanded, depending +on the result of the LOS check, (if the LOS check returns true) the value of g(neigh) might change to `g(par) + +w1*euc_cost(par, neigh) + w2*(costmap(par,neigh)/LETHAL_COST)^2` + +##Parameters The parameters of the planner are : -- ``` .how_many_corners ``` : to choose between 4-connected and 8-connected graph expnasions -- ``` .w_euc_cost ``` : weight applied to make the paths taut -- ``` .w_traversal_cost ``` : weight applied to steer the path away from collisions and cost -- ``` .w_heuristic_cost ``` : weight applied to push the node expansion towards the goal +- ` .how_many_corners ` : to choose between 4-connected and 8-connected graph expansions, the accepted values are 4 and 8 +- ` .w_euc_cost ` : weight applied on the length of the path. +- ` .w_traversal_cost ` : it tunes how harshly the nodes of high cost are penalised. From the above g(neigh) equation you can see that the cost-aware component of the cost function forms a parabolic curve, thus this parameter would, on increasing its value, make that curve steeper allowing for a greater differentiation (as the delta of costs would increase, when the graph becomes steep) among the nodes of different costs. +- ` .w_heuristic_cost ` : it has been provided to have a admissible heuristic, so the recommendation would be to change its value only when required. Usually set is at the same value as the `w_euc_cost` or 1.0 (whichever is lower), though you may increase the value of `w_heuristic_cost` to speed up the process. Below are the default values of the parameters : ``` @@ -32,11 +62,28 @@ planner_server: GridBased: how_many_corners: 8 w_euc_cost: 4.0 - w_traversal_cost: 6.25 + w_traversal_cost: 7.0 w_heuristic_cost: 1.0 ``` +Do note that the `global_costmap`'s `inflation_layer` values recommended for the above values of the parameter are - `cost_scaling_factor:1.0`, `inflation_radius: 5.5` + +##Usage Notes + +###Tuning the Parameters +Before starting off, do note that the costmap_cost(curr,neigh) component after being operated (before being multiplied to its parameter and being substituted in g(node)) varies from 0 to 1. Keep this in mind while tuning. + +This planner uses the costs associated with each cell from the `global_costmap` as a measure of the point's proximity to the obstacles. Providing a gentle potential field that covers the entirety of the region (with only small pocket like regions of cost = 0) is recommended in order to achieve paths that pass through the middle of the spaces. A good starting point could be to set the `inflation_layer`'s parameters as - `cost_scaling_factor:10.0`, `inflation_radius: 5.5` and then to decrease the value of `cost_scaling_factor` to achieve the said potential field. + +Providing a gentle potential field over the region allows the planner to compensate for the increase in path lengths which would allow for an increase in the distance from the obstacles, which around a corner allows for naturally smoothing the turns and thus removing the requirement for the use of an external path smoother. + +In order to achieve paths that stay in the middle of the spaces set `w_traversal_cost` at a higher value than `w_euc_cost`. To begin with, you can set the parameters to its default values and then increase the value of `w_traversal_cost`, while also decreasing `w_euc_cost` to allow for an increase in the path length thus letting the path to settle in the middle regions of the spaces with lower costs on the costmap. While tuning the planner's parameters you can also change the `inflation_layer`'s parameters to tune the behavior of the paths. + +###Path Smoothing +Because of how the cost function works, the output path has a natural tendency to form smooth curves around corners, though the smoothness of the path depends on how wide the turn is and the number of cells in that turns. + +This planner is recommended to be used with local planners like DWB or TEB (or other any planner / controllers that form a local trajectory to be traversed) as these take into account the abrupt turns which might arise due to the planner not being able to find a smoother turns owing to the aforementioned reasons. - +While smoother paths can be achieved by increasing the costmap resolution (ie using a costmap of 1cm resolution rather than a 5cm one) it is not recommended to do so as it comes at the cost of increased query times from the planner. Test the planners performance on the finer costmaps before making a switch to those costmaps. diff --git a/nav2_theta_star_planner/img/00-37.png b/nav2_theta_star_planner/img/00-37.png new file mode 100644 index 0000000000000000000000000000000000000000..fa4115bfb9032b243ba1015c94e8ae2e3543bd4e GIT binary patch literal 115135 zcmeFYRZv{fvo|`pJ0TE!@L<8+-6ett8!WiH6C}6}?h+xmJHZCG1a~KBfZ%dBi)H?2z6CCEc6%XAP@*kL0(!D1VRu3{(Mo9fhRR=7Vm+7 zkzQ}KT{Ml|sT`c_&8=+As9Zc9%&5#ftjs|mkGZ!oi&$N+P(zl}@@b4ZP@*P_BYt8D z`Z*pj9$bJ*2*l)WZd}J~$FyNhDEKQsCNvM}f*1xCLgshgGJLaUrh}@yd`;CgTPC4* zub_p9&FAi7GgDYLSeCC*dv2+}iAUq#CelWtczbiNf1kF7po+{AdDUO#1_GfYTLH_` zR#6r-wYOz6hS-~!v3b}!0P6*TgvC4@j7_c0T&PUUEUfHAz(>vPU@9w!2w00p<&}zq zl$oWKyqA;NYcJI|re4;j0uZp6D7vtRATWTfnTs)%hpmmBv!I6v_-R~0;QQlab}-db z6Bla{u(paim6W}c85K7hH`^;#84oKrPOvCCm9P`UTu@V5_CG@azeK>6E-ns&?CkFD z?riQ{Z1zqT>>L6D0_?9i**Q5`fflUJo^~$A9;|lGG>=348$;U6+0@C(!Ntnnj_Pqt zV-tH<7ZETRcu)18!5>-he@3@+{tpoVauv%nm?w?f`VF==z2Vz&=E0NrgA4z{dv_ z5)J}UffS@A-gwOIx7ynjYPoG){1kgv`ysT{Ky|!}ykXp0JAJImt9O!1Jc>%9iX1U= z5tIBCYUy8-XqzA(*mE8tR9#b8LhjVr+;1ckW&b&xw2oJ=>WKE}j9yR>N779NA|NW1 zlBi%v_9cky9MmeaGh=?r{BrPeO7HEJy0F%EtLxFtQvH40-|Z{7pczO!%e&U2*6qga z%aS@`dh&IiCmHhtA9#4{fqHb*yEVrl(X64s889w=vR%P zl24Bag`Q>$!L8!1h>DJ*lY@3wmPS7|2HwOKJrgPOe|`D*6GCE4Li#v1@RiBfJPdb@ zThgfRzs89BxbkZe#j?QUzBPgX)Io}3UxZGDP6;#)VF9z4()^o*g>Gmcw@T6{+-_C7`T_kxlhyt;A8XP_~0ayl0{ z5D&K_&;^tm#JeQWKuRE7wk`j(<4P&iv_}2`Nd#ArI04VbI3MNUw*9I|TgBtdI+iol zQmBq6gwu}p7b*+$zFgY+jrxmE9m~+o-B5Ml_p@I>cPtBn*c3qlq9?^sSRnKrIFKOY zI_4fIK%Nz+{7^JJxu!@2s_rNa<6Q7=8A1EE&MqqE-P43L&nR*lhbBf6j!79b8EM&l zo0XqFEN{kN>^0(6DPF9wT<-z1sqf=TZV@8;LT1bDp<^X!DE}T6M=|d-)!)!ln4)xy z1*$0iocnAPKo;Q9IL9N*HDdF&rsAV1=)SXH(xVI$5ttTN64))8EVPW=n6zWeS7l%Z zSSOh^$VeIo1nhjohR$X9Hp>c~Af+Y#ZSYz)t|X%yAW({*IXYat ze2+^2hSbq?>9DsDap9Fd#d{A>9n?WG72RQ#_o1|;nK93w!~9!F8J@tin#ve1kT}-x zo9A^_5nyOXB@!+Yjt(KkD*Xpbc+la#Xg9qosKI+#drkI9>HzVT{Da5`!t~z!ZArZP zT0-b^ck~ZH(jKSO@sW}WG@g>>6cYPkG2D{Lrbu5+kY^c%da{$jS%=?5k30Y4k2af) zPB6D2a+oYKBn{U>3XrKsX^ygtv@$50l9&z8qCn@!*oF6c9w`nH9VxIcEt$-msS1ye z!>=psJ4wo5d0}$OR#Z44q$SA*Onw3Pk3dAk$aaut{5{Bmj2Sl86uzgG25Tul=S?xF zmc*X80-K=0w(-gyA(>K9m)p zEa3eU_qb<;5xPSrx{A{ymF6tPGCQbXw@=~9ySmRFKy8mex!A677$guk#(kUY$IBk|B5-%3ga`3D zCcN}%a08|Ul!sDqQu?nkZeI|-aATZ}DqQP~LbXp@h+RHMXARkeq(62r@8i+{vEW2F z%N_hCt;aDBmQeLP+85Ot#`@;=5TA4-!pyThsS{|$M zMc^Ve(5ss%Nde(^4AxV)wdtEiv9w!uZKSLKJi4vjO6{zBWw{)1X zWi6aVKPaxL^M#Emd9_WjxZ<{Z0hC$Qgv26k zt8&>1kI~Ph0Es6x3J7x}{MMJkgONii;a+Ue3hI1uL}N5MG7L_h&1#ZiK+}XKMng1^ zQzdW1>3zW%^Qm`ucf)i#|-~TwKC!75;XAgMQkGZA76VU+x5$z;)_X@ zuIjx4Q?l@kDwLkDe^1Hu`y6r9?Ho(Po&kq>O+fqK0j7RU7-%KQesBu>e4^@*f)S<3 zJ0lCD1jK;R17-guzMDQbqeK$&b26MxyQGwpG~SoRh=-{2AS2}Gb0x4NvIQ)^;RH_U zbL;L&tE%6YLYLtDFW7fPV9>yWHh<{+5J%PKDq}av`%H z!PJBD6u!FM{~nx5eC`y7y;3zi&UyEefuY%p(E`Qnb?=mf0Pm|ejJeO9T@xnPTr<;R zm3DeVDX&!%5<@--mlN0^n*}p^p(2gG`c@t44EkfEN{+>)7q*4??<5zYW^-$-5%53h z$-R51qyF3yzsKO&6t_B{Vb5#an4)#M8kr2f<(t8Owk0DGnn#}l?1!lh5{g#al|b2% zSq?UJPCog}$&S#COPwR(8%02JAEhB7fK?Th8XCsSI3^-gw*R{j!SA0fNrfEq%^KS+ILaEH19i!DOmfU zifj?0jAJhQG*Da`3aO|&&v4dam@Fu~wi*>dOGh!mta{Mz1@WAUQew58+aZnpwM&(< ziThuqkEk_7U$?dg0yOVViF4!4gm)_N^gNbDzTnXBccERiFMQjd1Qb4ST+7}Z(;|t4 z#Ql=6aM1glzSM}?le&UOUC5;#Xmxmz4= zCOObKU#%}`kSL$PL5rE}hBG@)1aN+%~dl|MmZ|j3kaHlDMwaUM=JitcI-4Rfm4#@zl6DPa{OI$sNV9;mbr&XGl+BrlDp6J1gYFj?p+yk$$g1D}hhF@c~=#?2i3T zuJ8Niq)3#?jc9Q4JxG;LICjF+fRM!Ir8CaJzL3ZIs= zP^}QoT|qB>=hQcvVKPLSz5<{TU_zYeX5NiWNKFEkH=Y9hy`JSkCcQKxy?MZf_gTjn zr(iK+afsdiMT4I^0S{m&awes4c$$7#5(&7+Te(mPOCu+UjiTyN&rB`aGh2PZ!wh# z`HNCRZf``b;&w_%4@$!Ds8y})$y#_gOFn_05C$w05q=?QJ8{qBg>?Y(ii?|fsCDI0 zB4#q1E5a+z2b6F%?-Db%{{lb`KG-&F%$8XS@qQV!F0&a_X%r%v6;Uz$`#;BDnQ`jz z$0TP*5Lfo}bJUp}+MKv&nsnWgH~mz(X9df@D64l620JnO^>Z4}mkqXpw-5yjz8&fK zE?;L(osDi9cV1R(#6Ds@^SZ{;HCa;>AuyPBX1tY05QNS|Jy<|_lTC(9Z#ht4ABFnT zGGn6FmdqF>mJ3PQtv|})6W05M5Qw-C`)}_>Gm;}cDFexH09bic809E`3|<_^60oJm z(7{x#418@vOcPB1fqotVlS@GeFmA%V_1N!?<787377@yw_aPeYOd~o z=n?KXL>ZfX*4#bziw5u%@*I<`^UV?8!nJA}#+~gpT;rhX6m?-Y9=SHA6A2+~4CTQ{ zrcCb~0ko5T12fwm+28TFiQ8)hKf##4(V@}#phJ7=AMSC@f6)d?>hZ|$2|(iRN8gn0 z?g^^);cCd-p8c_e79nQj!p}5OPOr*!=u#Eqt_hIYE)9>o8`?oBywSNkRpb7y2xv= zXtxlG`xRJ)q_xgb1PmC2xG(uib17!`F|1^d7A7eVurO{zfD_>}BQIML=^s6DpF{O< zD|hL>>`ytqDkkyJn$xXzKT;ls#<(%eb+ss~YTTtL-H5!b;+*6*biAv8B3_y=X;ln5N~TR>eyVc2$v(8P4LOP4?8cn1$?0Ul`? z{Eb*~&e=ru8XP5qZl~44?JspB;F7KRKRho|rbkskMg_b~m>H9gpt7E}^ zkDtp|FoQo2_GAF7_B*@41e_Kn0ok4{B4JVkV=hJ;q=IvcH!n|iXns(Cx;di0IHAmQ zzwk%GJ}PLwsHR`}q|ObeMZj}fo$1qH^^MxmE}AK&u&PJ%RQzvPru zmE(3>qhGA```L!k$j3wfPJDyK$!#0(s_AF^qsS*|Xo?I$Ca2thA#man@s`3MUG%3} z>!$O%hT$NMF0pPLKO=m9qX%2O#&A*dhZ~PsBLskS|MPqASD}DB18e1z6AY88=yca9NDBpW{T1AO3w*v6oi?T?BXVk2mtN%?la8k;%i~#i~3g` zL?@SQPB%wzpnT{9aaQxP#O|@{*?8BSw^-nhkC)UT%Jv8FqO4{Jp|b{!N1G2$0n}lb zbS_y%g_x=TaLmV&AUB&IO?9QU)xvF5sc0CS6Hzifs4CMkerc{X+C;J!zH>c8YxzcBYKjAE4>Pb$kYwUxds{CmDLI&Z=h zD{9m|sukbGo>%`MayBk^bI9Cbkn6xtNYZ$5kL+|&Z?asjbWjCMta)EdSxz}E)1Mh? zQQ1ciS*k$67z`AD_I>8B+E$o%vpw>6E;k~zqt{m8{)^W0d%368fW3&m7Y^5Jd_tZ3 zQK@wq{&^=K!bt^qkFqkHyt;%c!3#@JcG{muSTe)+H(fdY7yh6__^Yn^Fr1c(I{J_h zq)zbEo@Mu;mWa_$*JTEzf&WcZu+p1?AS#%mgi+|;e4_+F5)Pk*c? zF>!Ie0O_O85|QXg2Dx#4({1V6$5n~da~UcJ4@%Fo`1u>oDF-{h_jvJI`#Z1g?)XLA z3q#7TZUsji5Y#{*ArCVB50Wt9QiQor_h@#+(7M+)MhGRK(Ak_ZYoA}`w$R-ECb@>C z5}{_CKh*EJkNU7F5ISb)YS=Uq&Ao?RBjW~x)5tlsSR@rPjs#{&GnV*WV`Gyl$V>+j zrNBNDUfyT76b8Rj-d={hs;Q?i3h<9uu%bzOz@tE(&^h@px`tM7XbhpCMx^srYOmu{hEUB9}C3f&>C?22`-tUnK3ZohD*`N1qX z?z14%k4RBBp8;u8-08jc@CCQi*y$}Rc4$6@}ZLtyZ`*y7Jn z(v4n6k26YJzPeg^NOr}My+A5(2o76;;Eog! z39|@LHU|8s$AAvW+6xo(5K_`^vXw)BijM8^zG={{?}p)xJD>&;ub>a^8;AGoJLLC* zUe7n}>ZCFtp8+W(TS)($HaRW$f`-nnF5nDJ1vxo{Mbruzq%(H+_BwzOP%=KS%vU+z zNC7;YM@p)LO4Q2DWJ{d2!o?Yfr8O^>W4!3IAo&a8-o)6mQ~Rv;ih8!(FrLhe<-8kD0I6*qx@M9D7AUFIIb?om3HcGG z>}-3|SeZQD=_-r_TIep&Rv)r{b@L*5EI+Y zowPh6^19Sw-1oP?jF@akAX`FVlg6RedZw;cRkxGPbJF1)8Z0icj-d(8PNUs%rk4So}2UN~w;MX5@V9Frpf zUa^vj{wAH3s$ENr{FL>NJ90n`>I*3|N@ z49y#d=E-4T>KqV8YbeB8#GWYnIV{^;GYc;j5>d|bJ+_7E`D82;*a;iQ6$@#2jba;L z^scoW9@y03L;vxg5<@wcOY3m9DiP1K=0VnJ_XWSNSGx2Acpl-F+UaJw zZ{_R=?0%_&`3iLI7s>}}{3%6{bF5OG)?R({is*8{S6V`(^@4%+rgu8^(e4wyJBFE? z+gq}@XQ;XC>PIU`1z~CD=WU^6 zZJ&Nv*T*;ieO}y+1WWOrUa69! z&yS96ijo5Z+j*NsQ(DSBqaTRGPvD}koAu&WjyUrk9XZUN#+{=+EY6CYb7pU#A#kT5 zkUec*M0pMa68$ZYgt=0Mbb*Y+<_T2*V};z*es5!yo6?w@mXfEqCv z>dBB{2fhO1{kHlv2tueMFV33$u*(N~a7C?-3Zfx`fAhMz%SbpydL4(E!7uQHuX7cq z5?GF|zLHJh0lY&^6HNFO|9a@9?nqS)n_^{vA+)6bA~ak1W?{X~VLubE@cY+;z{0sr z6_jU!dy|Vdb3MCzWBsiDH>4dX54huf>>gPQRcGggt=Ng^lv=r6MyYWSxO7@{NyK0S zWEPVB!p>nqgztlazSV=h1{e*lq1;KT936*{zsOY042`~ zu2z3NvzF_I$FLpcq=~`I)HaXSdMWY}$Z>^|4K&zX3J9k>vJUWwm0wypBe4$Bd)1_M zeC3$AX^a%#XeQqGrwnd>5Xo-6T?SwLQKM#plp8X%uf8cj9cJc|CD;dF@A}`<8M4is?#te0IIld@P4Eg~a zusfl@m)t-^*q9AMUvrNBDz!}{<6qQZ(I16iVT zBHhx<)Z+C%_bvqOPoOgy%y+IUXB-!n%fT@5y}cR@vbSDp4I1NO@J^K0!sWJK^h8c? zyN@sWhaB!s$BX%p`kt{P;^`E*N|p^5R=x8c=I<=VYixouz$T|K?Mo8nQSWmV-i#EH zn!v`InDSZHP2E(NEjKJ%t<;%TPbYljE=;?#wj2P20#zZF{m!C!?_T|Beb2V9JWhK<{A8_@F_uCIY|>x%ag}BSd*7jmh9?*nDUt5<00YOT?+1B8o@w@wzT!HH!=nXeG~FzZhSB=sb4M@gpB! zEjO;Xa1K}JE-yJdi|av!RO_!R#QqAy$XUUNkko}YYwV4w+%&LA7<$d<)OGxQS7*v~ zxs=wRYO5-vEZ24_^F=yAW5u7lC$~6)SA4mc9xmc8)pOB9e-x_xr?%O_Y0pqwdt2>+ z*Wq$F5AS>lIxgFjwi)j!B1(e*U9l;^cRrfvd_kJY@SL$NI0y(W>wRy=e%{2mN>Uab zPHwA)*^yMUd%?@SZt-A?8S>A$NzRW*M=PHDoJyrMgjSWwJHi=XEOYIu>gIMe zC|jcTx_d7YABCm33NOYvo!6dC!OCs2YPD>A2=R?3r&Mvr9V=!)X|yN zUqbEaj>>66u6m*WFJ>HK#I4;nF!YNk#*|B*wn41-U1_&6VjyDd zNdxwP=`dbG`63XF=l==BVi21pbSO%(xU9SlB}4RJ?dSEc8vdePm$V1$9B04`&dz7( z>_jVR(gLwE&Fw4=-ToxHi2wP0h~Ru+8E85$Hn?|K9h9FKQC$>yxM$z<`N^FhUb}St0h}7hBq>S!u#z>uO8G6+LW%5g;jGH&JpH#DDPDsgoulzPfu;?WZbizgRMd$aXb$TSxd&Cx zOx_0EafFWWKV#G3aNPg=E;c3ytl>R(j2>=wvhU#?JeFs4_o#H>7i&V@7Ny&5RQ9PE{s1Y5+GAoN{vUOo z{xu!Dxi;O(5L-uGAIrS2T=rht5Ij7zFMYyy##hA2@WVHk8-ZPKEoh|-V{d7d zuatrT-Pf|!&n|yx7WUA9Xp25(KOM*Jjmc=sv5s{t*F2bZkY?vSla~0tQ|w;eE8#!w z?SXI~cJ|w`s3Z^1+QlC_?cso!iX`v%yv1ML6ee$d-d^-Kz?TGi4PDRr+145dK;GCe zTS7a4T%6-%f^o%(-=`U!K9aANje~^G>O^Yy?xyYhSbUpbJeG?z$GWN|+1pka%x>s9 zpskbwmb0W+Pw1V9wZh|pVnnS+8M#uNW1BBEdQt7V)oJIoAl_kypeJYg)%Aq>ha3j0 zfTKH7H`@jO&6!v|pO0AcjeV~71vI{|o}=Nx^E9;(kfCn7QJc!JxA2Dq{s~b@!6QNV zP48(CK<0jGnmACUfPRNJIV%P-46nSg>W@sJ8HPJA1nlX$9usU;l2P6#>&{jHoJRFM zOs8$OLbEIiVO0SgbK8s%tpK*a{(%>n{lUKD&XDY?$u1`6moI0eH4S$nO=pW8S$(wt z{zkK6_nDE}q8y#%ICK~&0LaV%uf3IC2O@cNrs=#d`Fn{#?q9M{p9u{D9{Kv->Iv58 z!}{0)SJGVz1}A*rOQK=~(x%fIU!>3caQrIo zaAyE^IJ?P#L}yLS(;T%QfaJ6W<>ZeEJ@i1ki9%Z;Q2J)S z=9~?=cZER$(kb|tz}_}7C*fJumprSWY;xgq(dRTL@}TJU*Y^<<6wPz#o*AnhG}&e+t4O9Fro}qwfdz3?^m4oP#eA#tuJFt|HOM zO0}s$J0irf9ZhJU455$VJ0)iI38D^1nSKC)MXiiU?6U+R-82rOH5}rNyG`O_D}k!_ zv2Cmp$De1NEGg|ALdUL*D6gP?j;p~~%RXdUbzO&66SHJYZzt2puIk<1B-*Ir#W%7i zR0))6);fL+*z=w+=Pe>3*MT&thWS~r<)h+pi; zVM32i2BSDy54H};YNDI&0s0axYP`5i5DjO0#zHACuYn|kBsbHmx_0``2cno(o$R$G zwe?v)(oxY%3&xhS3zL-YGOA@W2o+of;)Bx~dte=aF28rPe(Ok@X6WbC&7Ka|Gx*ej zC%tgFUr^Nj`BRG4N=VArA_KLyX$C9v*C|UBdY3L5FZ$2=t*NFvkX0Lrhpz6Wt0;3D zkY&Pr*tvc^28ozh$0qfqZBcm9Og10HTdo9MDi%2-XLasHn?qEgJfWiO(BxVoQBI|z zrVBpD7DMsvV0R6VU8wR{dB)79Y0mL(J~yy9%6X!xGW_~6i6VvB45F_Mfef$x)h4!< zhPY1NkEXx2p-tiWeKE>5XpjstfEIlOgPTH&8|`i6_;`m6M~vUiS!bG|vOkIu6T&)f zpa}*xc|`-!gq58W>l+ny6QPP(0C8Mh@2f#wPg{R8ZB{Qxhtb(u@Aw{%#(VF!>u7f# zBl5M-=}kn!V^$OBcI*Or@$<>^grmt}X2q=J*K!Cz8uf>h?rdgZAN{b+i#`}A+``kR zJUM)&MmdLQidtfobC}05xoURL8zur+usqLEh*;_tkIYlDI$uTAe;-|WqvnXiOMUEmzX}Yng3*BEuB9AQL<^ ze8$0+>>7N&d05BCx=6QVGddkvgg+B3dJ8+N(M`fCeB0K+*ihe^TXG=};VEz4NU0P@ zFK?UG#CInpP%Gq}+^r46=*4 z_?>X%9qdL2;1axJ+1A^LOQ6iGgQh~${nm8(nikjkQ=8`z_2CmN)oGGJSDps$`fEK} z@MMzd81x@;9s`a;tW7wwD5Qeg4WSmDf0x)K_lx`2A}wyNZ_~tw+M^J6gRDdN9nm$mGghSyxt{vxftGzUF!c zmHh^IYfn|t#6X@*gv%NfxwgZMFjET~7aR>BJ%L&J_C3v(KzPoX_T_A z)rOa(`%+>Cc3;Zx2yA+{WdHnSAQXbo$-iLPVrCz&`%!0_z)SDYs5ILyvJ5Wckuc$7(Cpq)(^BdpeLBA49{?2jnZtas#j;4QdR`* zps_NSfrav^G+R}9On;%!MQ^Y;28q{PP_FC;Q|4dJG3+cq^lNvuXE&}7p1@2G`WmA! zC3~Wo6W5*JBx(v6)IsHzg>JbDg>5wuN4P^`_g?%-ZkXMSWRsvZDe;cyOW_&eEsM+f z=Iw1$SQ=+$4@I ztR=87n;6sn%48r!9H(@}*^(jY3_b??Q$EGfuM|cz_H!sKM7;Ri$N7pH;kuW}BCdlc zED#O7$4)osho|8Rv@xpdS$D{7tBVCV&szI)a7KBz${53{0y4@Rn?8;|Yr#4H%%EOe=n}Mr;Wh1lZTk{SF!z%D70v|=YyjVWt7M2+( zCho8BL+WBOZL9)c%XJ~*?N95pS@XCc$3hfW&;%z@c{t@Ip(W3NG8X3vvR@mzMD>*{ zaBE*jqZLWtd{tW5uN|ZdPsm&h$RkZzoL|Qm62KfX-LK-)Y9IYNbV6-BBl4~4xh!M) zP)w4e{W@wuyJ74!HX!36%2{s*-_5AN%E~|5HPlh5RTlxl9qErm%nA#aU;Gh49w%MP zs6?V{PQ=~&`I`dCW6@}t+;Tirp2*k;&?_q4cwB1QjFga(!>||Tar|xBZ*`;~1`00< zhiik>_v4c3l|1$4%rD?YIT;F6eh#6%!FVg&i3Q5ZHV;+yQzfO8ZBg$94I*}poLoj>X0N6*q=pZf0`(xH3*yMoYsCehP|nkavP$=- zGAbh7Thu06*5T}H)Om@nF_+)>sPnyn#nGRb@LuL+%#?6_0|n%pd!jxQ1PTV{OT`VL zUR#Mon~8#7|Ae@??WK~(HHXBKty%t|p{!TuNo#j5?HO2fF(0Gz(GOOV^7hvErz}tG zp)HKkD`VEt3s4(u@I8%wv?C!ygjz(_I($@%h6J4DG%o8GB1_naE@`u7Lv`_0jl$Q( zhVqdPczl{lIlnZb1JTo-ukw#j$BwduPP<;V8Ef&v1PgFfX@h(BNgnl0P&6C2x=Kcm zGuG&s4AZ;8?~7ivq-VrJTO3VMg~3&6+&OZ~nYqo%XvN01f+NWG&EntZkO?^p-QER? z6Vbp`rB6e`cR#hJ%#v77XO@7$)qb0=UV-&TcXw%yhE5ugp)sZqqX*VW!e?G8zluJy zWk5K-;u)*Ob7izX1&`Q_Ik0Pk{~}>TMtdA^A)VLFP;c{O%?lrhK@s$}qF2yNn)4v^Qq@SyYJeyviPC zE+!?>1y2gvqZKV<)ULhZNicuElYrZQ;-Vgdb(9zti!E)dMSJ6UMRZ%LLzb01(XM3S z0P3#N)a;e0GI(8fu|?B@dsAO7(Gtw*_54Tz=VPi78WvcF*h+)jPGJE{c$KsTaoQ}Q zdLGCNsu-0=YX>xl)bg^$?I{3o{LeZu3Fr@rh3F*IQE6UsN{KuZpI30D>7d~}9N}S*sV|8`$QI=c59g*HC7`@n+&v?d(v>#}{ z8KLxO_HN)CFWh!{hGby;5w_*y9cj_VlE|)$nML6fz{LL;`>mVxOTFAwk?=Lgo;8!b z^RUgp2ZJngb)d?}Sa~Pzs`1^Uw-^$KvtHdh_r}1Aiv}k{9Z*uV{mn8O zlDWU#oy$iGRD&FKa_MuGAAtliO|gJ7JhEwjg7G`UChhJ5yvoF%)de>`E+mf8JR!ET?Q^Q43jv{e5l%p`uix4D)&K=T08o%@WJJGV)qt#wvk74EPyYekWtK!waH%RCn0JmIc>8K)D<~UWi;oq>hjEefnn>O ztiBm-rh`#pPj%+7%Q_Vz={FohHo@@D$^m*~R9%ev{l*A@0pjbgw zGE3*uY9fb0C_<>&e$)i*BNC}|l!e`>rVz|GPysj(1JZv6n*$f6`))*|9_8USr))MQ z35c9H!ZNc9@$2JUNkQH}iz{$`i^yZV#rsyaI{TQ8r~+O7PBN>mx4_uBHbVmg!H%uU znP8`NBa~saalY6|6&;}RYUwkEl@IX+)y;BEa zFxKwgF4r!o8w@5O+!~XE8<$C);n4vKtIS~*&sY+A!qln~ZW90k<-y!qO5~}O z#-!87QTF$#*4Yk0!>Qw{bE18JgCFTkh0)@hUgXUYav}po4(K+HwhH`c!wB|C|16;P z*PvCrtwV~ha<7fzs*yZNg(_~dF>xQW?1AXyf&?zEhd@;H4^Y3DrZYt;Qu-z&cC=~k z@E|^iZl)}ytSb-scSB5xPuG)5nIUa%waPXYG5^EDkO~|o5>0ZfBQg}LlZ#)ei9P7p zeFBYVf89PN1zHjyMq|^uu6(W*U?T{r(NTibmK~VsLv05AIcMK9TFXv!5_1VYvTO~L zMPDbI=xZfDy+N-rDdnZAj)Cg?IRyo%VDeZ|M$AXQoPDP{+;jKPM?KzU5JnDZ z24WE#={v2JRp#L8U<|Y%Ys+4E^q|}ej*z7TK8>YI15k<9^}^MC0!uyv)B3t#E}D-2 zS%oH^s8y?oj5PgDjJiux{caZ$o>opztZa9R{h~##>fLPtYy_=`tH3M$=mQ2=8F)Ng=UiamS%k|r9B9ECbt2rkuhA!6$JLtZj!Ak z)zB|_2Q8hod=ymb&L~qiBWUMUW|Cx;X153eQ(^r#?;DEj`AdU04(T)Vn*@Dq)cO+h zsQZBBz%#=j^R$;j;W`P)P_7a!N?vWM63&xFc2cvt;MFphx}Ii)ZtvL^FzM57oSfr`nT{= zueZ%&wHgt#^StVOYm(vT{s9xKdo5pJKN4s=s-M`y>Lcmxz0o##Xv=mDGsW8e(v|3o z0yb(M%eOVkvZ_^a;tkO}2!u|_7jeWSKgVGZ2q-G0zHwj%arqc> zV3U@zSt+vsUKXMbGk(0>Hq4B_^Ybwbs)GkH9R_lmCUnP{Me}bpBQor{uxsPZUXiiA zsJ*L3)F4k9?&Bo>b9)u5etpT%_kv#u2FBM6WuCt0ZxyLKj!-A7^n}KGGj>ZSLAL$| zOb~rlT}_0nRF<}iG(C)2$f-Sw4n~Ur1CRe+q7<6`;7)3v{#)EV0-iDuO=Kee#}Stu zg9q#Zc3+h0iVVN?6;^`O-OdP|(owX=uRH_w@0z^4UCOZ<$wa#&WkI2kPyc}mU{<9R z^M?f)wD_0Cq11$lei5~oQJq)u1=kFzXzqT*F5Wf> z(4TTX6HxzX4ni6Ak2-07&^YeUkxa=d-K@33wga2g5EP&7gdaQVXl;*Qpi}Pp4+2@} zDYi(kPfAh!HlAJjwA_HhqOJm!W9VnX`zp_Q>tS z2d$2t-B;Az?OU`!)FdR<-dr2SEYl>I{ncYo0=H@=$YrX%FIk}~Crpjd^g%?}{Su3C zEaMZaCu}`oOITwp;jJVmj{y0v|^INtL>P}xj&!)Qec~OxcuHKULIzKN=e=_ zxSy@y<^ANBvLIpJ5^eh`Uf*&rca7O-B`{K?bZs01sS-Bdx)@;`+evTxi{Y804Og}B z3_w?0)S=3l@^gyY(GC)PfW)~hHxDeBYh0U`+cL~>k7`YQu_YT%3V5_b`+;X@yu%T> zNm8J$ldw2X!3A9N$Zy=%?>cWDF|;_9j);hcKH|qVRY69YIe*HvBkNI zFRB0>HgT)G7hD{#L_8GtVSQjge**o7M5@MlG@O%Yb=dSa29_BNi%k}Chh6i%f>gL9 z^sBs_*1p|X6-N)MZ+tQeH zXz?sZ8D&PeJ}4ae&!28KzL4IVbZkviI96nPXmvz72ORl6O^k<;&P8Ud1)x5J1}C@W z0)JV$YV2YOjM7v_@1JuSzha2Zy76#Q`At~`+R#rC52PE!z6DV5It*Y$KPquHcbbc= z7&*rD;$H(jrG!+ivzZJgr>B0guh-%UG5f9dnRthG%ZGmNEcy{#bxoTRZB}Q0pOpN? zZ3t8ghM&VZ(Ko-A4goG&5oVYpeTzPvq=t?%@s3FdyhHxB{WshLUmqC-B#Z67?>(cH zj?u03+dTQHOdnSgKZs*GbW!qGl8HgzYl4|QswE=k#7}BhIzV-+lz2Pb9vZx^r!dJkvoDpHOT{cpVdd@d?*<^xrv37^iYF>cn+ zQmdSp!7o}QS!vuE1pXCfuVt9s(yi>*s^Tqp*i(BI+%L{WlCYOoNacvUSoL{33B2Mo z0h1jed~N=nJx>Vi<|Cz0Yn>6E4ChqI5-m(0+a{1YMl@9i)HqtaM1P7@Kzc3sSb3%d zFiq$bobb(|6&fq0s;_7}!?6Q^nmVaJ$F51|cKJb*xlVn9;;LEB#8JdVJKh1<%HMoj zOVB9F9HGJG3~wwlH#p{OId&v+u;qrheq)l3L2*8{=7re}0nP?G+88pMx8s~7 zlsE6I!V(7ViWe8a&+>PpV&5yB?DSQMo%?72#(x-e`r!G`S-axE1=Hi-IL%t1%7tsIgqpMj)%cWp0rO#kmyC!>rK0F;{8G*`n( zIisXMiz^I9(z`uvO7*8$-I6Cl4{!`CL?C_g$ptWeB#5$0q$2L=!u-OYM_Nd za2N-!r+nWW2LGg<+q7UG7D?I3H?=vTXbupF^@hn}X7 z38ZNE1Kv;94c_eT?cwPoSlqx0+;3C>kuyS$V>xKu5F*yz*qi-(p`ex>o>~};K226e z^#2fb6DmSl9cX-_xfAwzuqq} z>)w0L*?XU-rt|}o*Mhe3N|jX1@T}takpotJ{ey4Z^77V)dr!viMW#;w#t377d*rH` zg*_5^sE%4K#_9yaw63aSLF@>Z#N4__o&|J)F5vh`(6l@>R$(vZN8-}ae+u*oG5tTK4NJKR7#1uufg7vZf6l!lb zcjja$0ISP5t^8f-3HN{27Uacc9Wfe!4-F~fy}MdaZIN4ys-wCgaJ*bj|Z9v9N-wDM@iObPa@23f*7gG zYH!W6^7@7Z{mg>uW#v03|L38imSc&r1=Q-gg<{ue}X1B4>=d$}NP%~@`_^k_QW2Z-}XJI)kRR+*$4Hepn?$=ok4 zTEqvv2G0vCh6d*CGX7d!sbTlQ0cMO7hffI~p?HZ*jyM@|>u?MQU*n(nKLf(td-c>dYz-}(IaZQzayix}K={6BqU zhYmW~c0^i|z8F$xl_4>=@9fC#p0F2dT2B%6go32Kt=6iB&Tk{E^tsrK)hcdqs@W1en?cXd;b00AWw z5+!Ysv|9sn0Z=kD`_2XaX*?WAE=P_X1z4tu)e1wsB!qIgJ!Go?B-f3aUT$w} z#ThXD7}AWXpH#l|BX|I45T0O=u_o%_gSO-uI#@9*MWe;+=p|(*B0Gje)#J7ZEn=d) zQ21-L$k_RP6ZY%FRSG;B$hQJk49n!ebcz^3(Xr4>8X7>vC?bEuwAdZeCiJgR@-4(1 zsd&FXnH*h$rf2EgHugqm1pFQEXnyDnMN)Sk)@kp5Sd%$EO_zVt`^}8K(s8{{PcWGo ziBwU|${Ay1wLz?%`XIsulu+roh9rCZ|3p-zI%5J5^9xmI z#^}!0-__c(S9w=g)LXVMj1Hu}el&|aGn~;r6S&=)dc&Pnm4_q{lH4y+mc5Ef2)uV1 z$)$-C+^}|UVyen6UWS#R@^ig-Ddxf_%$8ooV_Bi{ne8ol%`7g|1~B8!dS!Ur{>DL$ z)f|d?k{Ld2AQVQk|IoDhuUr(ODPPbpHR_UPcypxB^$PCyCmC~>rQaYgwG~5OeDyE? zUdnLz0n?XgyH&gMsodceLA@Or=}$%)sqC)@9hE^sB6z$_pgFe1{pP3>4fpH9$Ga;#I@jxDUVN zemM$!IZkuiXxoZ0i(SQk7eZN`@L!cyVoLjE-&3dR_}>Dn(bb>nVo_q*Sx1$2DQWk` zp}`lqDROO>^sU|a8O4Us$${8p-Xq%5*!sMl>$bt>2bYY6>xuPSz|B^j4eHkAd3600 z73Mi)^HFWu_1&KbuQ}@S|G1HUe7+izJEil>_fO!ce{P=?CbNj&5SVJUqh0CAeQ z`h{mfo=&I%N&WQr7iqI&(}Q{cd^X~EKc$y7!h3*c_fFBD|d;ti+_En4@?)77Ot-q)QrH?5mz%Zd(Z`0LVP~`I|bB^Qd(is$ku{= z&b8l^RYdnjvXBn0Ge9l*EH8`t)PPod(DqT-p|r?^;;_(LaYNRB0)Uk1(ePSUKIO7i zW_x8nW@xsm8@nGnKD@1~6@UYz0V7v*LR0}`&8a*-n+a>6ZEY#Z)l?OJ z#1-j2ZM|d{xcIs3O-C>J2l9Hc-bQSjYwAywfT~FFKGnlh=cM8kL0IlFy>Nd}=HIF? z_BIto{%o+y4GGZn+ucs^nT<8L|KJj!gd`DTDH@Q3@>h(ZzDYaV!0y)4O9d^p=3RqS zG$Fy;Ox2h=YWb5W!TPYYJ97RiSb-&fxP4SzY^q8e)@m17@ni>iY!nlic)xQcqN zxR=aXu_gQT3EV!Vc?<)3KW+^>psI@<-|y5)*8dS_+`(MbEcAL)ZF&z8fZp$tgF#jo zyn*ja6zHw}X#A)ou%8@zpGL6-DU3p%1nA=5HOMa>N}x5_ZfBL}l8{DO>K|xAYRwvV z1h*!ze+a!cJdh5`yQ4S~2W&5upa@*394mgr&R|P-_~>_+YQI z;?0~?vyvoW3dx(j|933n4u7NI7jC~EZ*qg&qHVZG85kUriQ5vA~cjn`)dxE%k4WkKV>kz?%NH)25q>k>hcV>){e zYtPGYe6==v{a)%-tfi9yyVjvb1Cs&!fpPAS_ zYJByd>nYlo|GEjk8Dn8-yG9r11L&h=)m%SnJApdI>ln?hc<&Csx3Dh7-hAVUqw?EE zX%sNXiQpmk@@DV(U8Z+@`*OYn$A8B_y`C?cbbhdq8D!k&iUcV|Shy&-OzN{@VD^#QMh*epgw~{C8l+1xZ9<}I zdq6Y>-Yrup-z+&nxhK6NB-qnDP24Tq8SCu%C0P(y>%nDYx0<0L(`o5G ztB(a3IIL}&Lq<9x$>HmlL1T24CVK|LYlkGs7b>VGweMX&JzpZ70ob40&F(HBAdN90 z5`<7`{}E`c>VRPJUA94v3PGXueNij=UnGqKT!w@{=0I>106ba0scfEUX|Ela2>ZA> zWtO_5)4S02k^gh$F!ir)IZ-j~soe9wr4KbBA;m+1`C{BXw7@Ct(i4xob!$Wr_O?i? z!|Z(%)9e%Cz4`l2z&N8PqO6yYzn4XHIs(&J=+wj=+Sb5oFVAQce{w@2u@|UdFmF_X zT*x*Z&y|w;uhE9`@TSh1;%VLQc=!OZ4<>RHKp+N31WjHp@w_jN+x-pOn12Z9eYgP+ z3%DHrYa`kGV)1(I?WBfpy!>9HPsov75v4LBF2|7{u;sH1rkB?shwWgB2~Nj3yC^c0 zRqB*$Uc@`iW`5D}Q->7{{FH;7M`W+pt%Vk@eL%!QO7`qiqRFR{VO)CI+r4iPm#}mL z(t$k(O>0c*gGQFG&K-v6D=D|Kj+a}xnEz&M662-x49H{>0rgRNT_i0K{i1_f9p2k& zgK5s0vYEaGcNgu;=#n`zN&H^FrL-eColP^*xtCj%Q&DgU*gh?6T}}WaOk0E&&B6aq z+F^}4RzvZ1^5>|}*}@_N^3HiJ%$_je+pOl-5377V&3ELiDlz7jZ_G(dpO#-%k6T(} z{@oa-`|E}lwk^+Jpr(!}31y^yHvkixu=*5DblxNaWIMoF`RPow=EQK&2uRQuRo-Wj z+9<_x5*sO#4b?yzvFn{wn#rt$`lH_eNpL^$OxNa zZVdM?TzEPa%E-+hQ80Sa$H^bsujI{BZ^;3HE`%K7{aD4GFkjNyhn9CG?)>54z)0}E7Q2yDbPfHaLd-cK6GQK+8~Fn}gffvoB;(sWuJvehTM`Nu{&yq_qwdBL#u zW7Wwvi!W)Qu14~d;gbg7qQkhuBtJWT#^I?X0n7tRDO$H&pTkYs#mSaE-u3ESZv7Fw zS0ZYl$(vtP7af1prbO7U>3*BGnmbtELylt!)a0fwTawCPVqIHBvx-?zT>jA@%g#3Hlt?eqXFxU!N*^1Y$~2ut_)j*jg4hK3F)-jCD%V+4Xm z2`1;gXk@8@q{gV z19j4cUMpOGUi#SL!CP<>ufytqwm*D6AEHt-aL}grPs6RZ)AYKhQuSMuCv1ZNE|JhqG<6%i+PXNVj9innns{4%2WRXL`*YFf4PQ$gvxs zerku=C2=aJXoAdP^|qpgD*}b&GhV?+_gT2q_=XzsQ2qIFi2x&(=kQQzj@SI3lr5nR zsL6jlSI08$?>8Y9*%+zW#yDPc-7;oDM>TDfe>ht;|MltTNZgNEB~+w2tILEH3mGF> zV8*>w;2yQXb8ASKqa3B{e2ZVT-~J=NJhl=qr5xN2x4_W8RGh+W754^;hK!x0hYi_C z#Y;M0m3|kM(7f+^aQE@NnlKyhrx-oT;zyd%wn(6+aoC@lEGcFi_AqNX$aUq!;KtqiR0XY!ijf43$2N02;iA-a~^5Rgd+Hq=}1ep9u}u2D?ge(g|I2cA(z)cw&V0;-eDWk zdIH-{0xl7q_shbCtCx*kD}(Gu(Zg3!S6wK$4FO36&(NBka#&`W@4%IpQ{nE~Cd1v1 z9y~t5t?Bh%M6|Ub)xtUUZ{8k$Ai5BbsoQ~op+u7cVh!v_qz!(6_<38!nm*r6Onh|o z*2Zsl2g+lOGToY}DcjaazAkGzkVTqjradT~2{zz5IHV!h_-L1l$|Pr0XDwrm8pdo+ zJN&!9uS*U?wKilwM|-@#CagTBGU)hYu*}I1*${MfRST1~CE?Ep>E_1c`>|7+>f=eR zgv|{M`K_bJ2YA(sVthN$qFvhA$&YV@uh#~-qk!Te{zW#ekD4@pWeJr2_2znMe*U(o zv(MZ%4RNW}MxogjK-$ z5{{+}ZOLa1f2WP~;WqGP*fu3p?qgrO(ala3g%tz(53((GQM_1p`iA3kGfF4Mk_?V(aDjRAT|(u)@SvdeV0*|9H* zDjEFxKYH^nMnY*$mlIl8$8dWeI#s^7UDa2wIETJXn~@-Oz{Wa;1|&2dbp$a5o%<1GA=k zwBGMj!=`WX2lunk_X{Qf}MIk z&!s}B@2-t%Vw)txhyY}pxya0c0p)v*GxDXvLIwr~!OUqXB1j~yB>H)X}}LEUA*rtrpu^z!LD59}Z? zHb3~Du27REgR7Ve*cxWKEC)()r`4mcbmnI3Ph(Hs(*V36;N@=&duy|fZfovwBnpWlfCSC9B(co_9S z`C%c$Ha$fjQjB#vbEDiszWxBrC{O-&ss>AOCProrjm^Owz%?!?0RN%6G>Y&>L-{sP zn~X~){SWZ!Wey9sx1^R7*|ci;ngkacUOv==pZC9@hC){T0b0CQ>AP-pNzRdwEcOsF zXmq&*U>{~F3x-XdVi=0br6k8p_0!(qax8T&)qlD+(WqSx$qkXceoKpj3dtf=ugW$y zUT3nM=8VzNq=22OAC2t{pQGXZ+*w21iWp?3;kd}|9a1!F(rptu*J^yGNUFxG>HSE{ zpotY1pqmMwFg+Qwzz^y4p|BH*w$DyydG=OZX*cM8Zfiyn?hWQQUrl_{Kxg;rQCjGz zeS34qJ814k9yzZ7UU?K@Iy-cvsKt5pZ|2&(eP4}Xa?*R5RH&(M*ylpZS6 z+;SY|L&SJH3)?6&Pv@;x82_GjT!;OUH4?M>c@E1mqy zPqh>!sFU=(Fdt8}M=zad8zTf%Hr1l)F%;QQig5CpfQS>_>@NcDDdh2iXuxFEBnQ+gBA}~dNXPZW3u#1_*W=Em z#}6ln?tE|tfbOeE*Uw)TGEBzd7Aq4oY?jn&n;DMZW`VAc=#Uv?1LeIVH?IF|chZR4 z_hu@tnk7MFV=QGkXxuMl4oAHMgyoY4+Hs6^(S=N4B5PwxLl@Ie*4IA7Q$~ir0&Q5* z=u+lLvcLI}AY(mUQIF5I+s1pq=>|(K+I2H{8GkBe!`OS6XYe}QsEpf?!%v6YN@{g*v zrS6wjljjhAm9p%NSZOC+3VB+U<+Nrfi(0^XImw7 zU4o2;&**7^+nzqnyZ>|)-Bj09-dzbQ9tPhtHKm3#kYJNJD_r(d?GGZG09cBwRxUE& zdj9wtruaTc_n{;5SG6)mZH80J@r^6?G5_c*fI*?F*cYzFvhFA!cs=N>v}0zC?}#ce zUB04jqC181ihuZmMwqP3wJ&+QdU)5_ManlLcx-ErIT}sTT_G;kpD@pZ>MvtzBcNA@m zjB#(-tqrx(BsCTS;(>lcX;djIZA|>^`pA}R-?4jSSu~t03ovn$d&-zm)syR#{@=iG zD%|g479U-=-Pp@t)Bk%-Pik0DI{BXUp!p-<|3L9(WH3RwMxS{*Hk!wNB0)WLiB#1t z{qK84z)b)gY+n&uI+j@P8JF_6WMo}|EH1yvE3G?vkC65vRx2JlbRFFSQYSO*%m-hJ zkBmI&bIwHf0y?0a#YWMSXzQ;dV*b{q6F|Dr=fm9m`wujsK-RScUW>1kT(y(5V!Mo4 zpbiODTa+ci55fCttJ(ahG-_DN5yFAP=V2NTr0m*fQZ?s80*reJ9SEUcFmiTyvMrik`oMksw}Fs0;*+j3L%Cb`(y*U;wv z*WVjH+_esGs^d(}_}>BXBaS(O71NivL_FDhXXJ!Uh(m*X30&eIOfRV{Em5~Ml*8l? zzvt2x07^*n&-tUMmnYM6BOd*Faldw2!%ct*r*IHjBtCx6htNZ9DEdfu^P1F@^DmID zCSZJr_;wAK*K?|I9Ngq7@vSiUe_DVC3gOx_a=4;@@=<%UccQu{MsYoPY@~l2BuamM zLnuP|>tsf|dhmODnIxB3APnGg9oyn^R>pMDa`Sx2R1h5a^4#9S;&e#kNA7ED`CZVp z;f3Hj-;Nm>ap&jrl7%EEJ*E511lKmWjO%N7 z<3j)EL;n4oh~sCRPj2_}h3(p8^QBCtBIG|^c&4I6{$LX;^6O%yx?y|$4K-Q%96oQJ zbL?aW?7JOv{dP>e*J+bgI6s5MjJH@oOf)}7|HM9lqkT5*6t!P}L{T&gbVB(k+!5?} zVIO2T;;*>;r|^C7;i^Qkc|c?5Z^uazhNnj^mwhe#bJzXDL~sPUbLOm466I#!OizL4 zPbokegNFY47!n3^2_DFDU?Jt2Ko)MdVJ_mc1E-dgmYsBV^R#6YfQ;kdgnMHTk>YH9lvapB7Heqvw)#Nv>PkOr8tv~+6=H}Dz zT7_HaV!ylnJ%2cE?*CPsC#h-TIHvs^+$OE);nsKc{VZpmT1@O$=5~|ZmGH52U9jWp ziD9mZaONFuQhd05DqWP0q2Rj{+ZTS}n|1g(`(XaJrgHqLFK|4HGjh0#V(!~02NjFl zxaX@)hlS3C>O9LgvC$@I_k(~>`tXj!qq72Ohnp+#_mbDaV(Zaxt+-t6P9~URiTa~^PR&pLvLu#+HZUH)li#a6;w{_$o(J%ylQzJTc zw@+1R&F>rIlG4%7t30+)lOry8zo{q1^}M;eQlh_Q8U*4m?PMslQO>fYzu3^~C{H{y zynUOMdL2#u5v><)I#Jdx$_Y5u%sJ%! zbM~gfN)F~2=+6UPpd`}bUy*X<`Wi2h&AalxRu~jiP5FufRojz1;(}ab_TQ3U08A5# zwl|V59gRiI^PF+@R=&DNrAlElq$t}s7P4F2dv1yKHT$?oiE zgzu3D?4~X3O8g&Zl4s+x9{|G|Nd?N^>W+p$hfiw5>Qg&3M^~{U>U-aGzZ~v**isq| zl>bt;qXnOGW8ul1FUJLqLpM2pl5^Cb$`a6-AF~E2NZcwpC=MBF%k)^aE8Dz`-=;>w z{NKF)hM|iQ$5G-So;h=RvN+Q3ed6WKfA1*!B!G{$7w_i&a)9owW&8{aWDejqua837 z=fnIY`5Hs=3eIrFjWrv_a1EUDC}ZHLy0hX7{$-A4ot_GDj%&&YsT1p11p@uKK&CF5 zsn6P1mL{n8E+)Zf5zuzhr4(bekPKF8ssgWb3ez_p?f*$Sm}-`R{#eBD}eY zEVsSoQtI?tA#KU*k88cZN-Bh|y<(vQ^CwMAzlwPDwZ5CKk_ukXkB+#5{ z`Nhi)2~WgrA+j5GhS3UE%WV1LiG$pAXi*aEUGG1!Z49Uj(l8fZbO4p8;fE;tiwzt8 zBwgE$TOFqh3`dhnumd8I2MW^(#93dmuyvEC6)c6jhz#L>F@2vy=m8$8Xw>>^#@ugN zl2>=Fgu)OiHG$5d8>dfTnvdCAfa==x^R{gqx7Ayhquc|_G&HK@9%e@MxS(?R`325o z=;y#J2|%WNxxT`c%p>sdrv?1pW$>$wpC+@F!!-0$B)3W7tTs*zCy#vEESD7a7d;@*8&0TP4k58wJ8*ag;yvWElV1la@lY{@@ngDc&7|Tm((%V zAbNgf-)x#4Q-72w_eR?Lq6)kY=STOLVHdpZl3_b=GJYk2X!US;ivaJ67-UkxxSef!C6T8bOMA*MKUgp1{B;DpOl{=v<98Ea z2gf32LSX)(&$DzaPl(C6<(~A%nO< zOn3n6aX(pqBbnmaVNekjfuJ=zMA7S1Xf%&I1M`oc7Q#T8n<^Qv2dDm&ROf|&0_4_= zp7XKtk(s0SywWE^T}ro?PjAEG2bvuQ#mh)VvvZy(Uu<4syAydxSj>?Tr#H`3V%GrW z9$Olk%FK{`M{k|vdeGzF11&oYWm2XBlm&E!8O?1DQ$b?}&5Yf~jcaY5p$H^|x4x&( zB3=5e*V>|A;k8kjGFJ5Z>|ga`SH)(itsqR}zZu{+0oziSv?}TLJ7ai4EQ~_mTnYfY zAQw71=sfgbm-3x`VZuUR?O+=!%BV7pMxEI@u*VJgn%~qn24`myzVy|&nQjfF%A)AN z&1{cQf-;on)Nbk+WK7muR3V<}MX#i==U(@1E*NP6JtAUaX2sY!(1s8mpMjp|2k$Rf zV;qM86K{#}xWjK2yc`7{-uRYqYaZM>2t(H0{eH4<(97wDJ}0@f4(+w~Z;XeaLAwX+ zGQ=|YbY`!5qOA%;IB!yk}_MKjUYOCL3}I%b|x-1tvs%{=KF~v z%MtvHD!M}Ds<=VqCG?7kq8Q15lNwKTte&ME(OO08+AAfyi_wVuku8s@Rh=Ifgd(W)zBryRd4gGR6XCX5lXAcz zk~F>oxUDh7J?mx_BuAg|#5-W<@ZFHtNNn9Imr1o8cJoXu@~~mMIWGLDZU2QdATY5R z;iubWu{%O zUEe0qD`4pxvBztxDY8pa1DNOI+ys?I41Y#Yoi+C{dtg$1vilPuDuyyW8WsD;Rh z`2{#F7&2%@fag*RK18$Z!$8>Oo=-9D?un8JCP$uIMYqxQbg@PM@A>X~^at3ys=|(V zYWb;^*vZnMY0zqIPz6^3cx)KlF!a|oQIJGt{S7hJmdDzFsm0P3BNnQ8-0PT5)o};6 z@eRi4uhB|X98}sPzwI7*)EKX{R-6)-Yr+*PWy4btl8@9m2B_;7myGDL)f<}KZwv?o z*#Gj%2RU6=sp5@Qxo}n=LI)KcIn0+Grga&(>Hj1n_=_6+rb05KOn)Oq40zp0&*^F?AYdrIM-c;d0cH+|Pe zm;b8qm+>7q+|PT4(<;~LoZ-LYVYO+8B~#3KQ!b0Uk}9K=zQIp%tfOI1O`&nkj@)cFbQlnT z7fKY(6pXv(ZZZYUrS`Az7nEA3-3~@rFWq3&5b(Sx7?6k91V`8hB=!#vmUq5Go4nj0 zvDs8UAzzR7xlO+v>*)wQ@wUS_c)cw7uy`8f3r`fOrI^OsQ|MBW@6nh2*^I0YoM3#l zG!L%~ieXAP;4QC{zmN+kC_*7d=rT*!E47xevOT^h?JmKaDypdGUXXukSob`X_a)Bq%P|1KXgG7K^Z3(ZaE=qz&qA+{>DW_F(;)~&CgbiyvF#_tJ7D5M2 zj=R(X4P*w9N9S^(M}51aK<&^5^#mqFVQ z^QDx>5$Q|Bc_w4;CERhRCFawpphulA#&nlgOA2geji&?+WLxr(n);Z1-SR`+lxYkc za1(g4CU`LYQiz3q9;MxI{p0B`rOmp#$yLA`t^+@k;;|oY6*fs}?+KVI#Vzq(GH6mw za2&pDZtiYtun5rh=9RG2M!_&T9nqtf?=fxB9W(tfd@Ii@8;rga9;=?TT27722IEuW z88hIbDZJS?=^n<;F3{N4hts$}&)(R4QntHVdP5h1JG54M| z)98Z<1CgEoc1TC!HNi*enC)+fwYKVM?>@M9m0*tqw~73%Ndc-8&ItIU_KM1~T$fGy zPvvPU+vPWu-Zl;utaBq#4WANT*3=|>#BGk1_0<=XXYL|cLv;-VVWlC3YuZ%G_k2DV zgJ54aVAb1@&0LK+ui(L73ilB(B5N;u2FV>t9(BDEy(%-5k)qs&~C zRD2?T;M_A1O~1bpo7GJBH6~M(>XZCfGa7#jO}j^D{F|_OcYRiD!JhM;==cQ2lUcN; z{o~IZTbb#BOij{DdcJr;mm)du@zkpVJ}GBTT1@i~7}k)?Is7iHRmS&g&}~e$-2L5_ zt=@z$r_gQr)1_O2l@}J?qZVo`|XXOMF;dbs|XH8Zk4D5ywwXRrWq; z2`V@u7sDO)L=mDfpzg-=l$~6v*+LIzurch54X1Ipu3NqV@*9SS#iV1q`MYFlh-E{P zrwo*>NN_G4L`wrdwDUNee6Ts+Gg)-*>sMvhq+Emg!ZfN~S(Yz9g=8zPX*iESbY3pkRUU2bJKfVuYL)o7rq~Vki-?GZo%A zkcRYq4m)(3PR~E?-$<8XQY1GdZ#c{6#o%QLD$Xw#vHoxMZ%txw$|lCIJBfwd~8_f_xBPd2^2 zYVGX}#>mGkN?KJK_Z4reLd#=t)@xZe*h*OeEDv-A89Jt2mc0qBqbxQTR8jYyK>|kp z1&e%Ph6_7oaNRzzSqWq={co5or>-CNzATG&%<*HNfhiae^;xTDtMfPAgSfpSUEy>}8|u(0f}bMv&U-9mQu`!3q58>Wf`6~y$!FS=6qR@% zywWR@Tp&prNn%`gkX8x!(iRdKXrL^zelznbD~VZgM2v8xgky|IvIl0jHUlw8aS_Rr9V zV0x^*P|r=D%`P-%R7J(KH~;;0m08^huTJZ}Z%PI9ir+Du*mKth88vFMD05}Nnj6d+ z6p)3#xpY55Q$(zmjmi3!DVw}CVEN(FMNy~qMa1ZAW8nSZ5W41w!!;9oi)`4pp@RV& zVMX}tqTBlLqlC#F&7aM8afLVWMbD;Ug8}m!LRPAbDEK*9-%1?TCeY;Z>gwoPDH)vz$Pgj>vE?4K-K?irvA)i|L|1|GW@AJel;9q!<;pY*~>x>%T~ z&w7Gr_N=LQY1|@aeb%dHn7~nZQ@)*R)!U~D{MAv5Jth2JDqRRaQQI7$*(gy>F<{9y~LHLbcNHHQPhUXsLYf|4r!H_Uw+dH8PT$nn%5M)2*%v0I-GWd#{7uEu(8~7EJ@t^>(W#GCCkgqVRR>nTicR@cW4pX zVyp)ANOpp%gt|pjQ)dHEW!sN7IRQk}#c*ENOf)w5%lnokW9`=6NKVgqPwz3~Bxdks zJ15h`2+Q{tw-Yms=N%V&WOl82dPLiYt5ySi46hJ8p4-VWh9RFL!xj$LzFAA%I-Bz> zvY@|kt@+MJNK7ShRRQt&f@=7@I|xt7jY+G~Ls9d@Sn@s#q;{3G^Gzq8mc(q@j2?=@ zkk^O#Etf_o9K{rvE82uSZuJIJM)EJ4@QJR0C7@!jAsw5_OFoPxwi)}f9X#67>|Vz~ zI&6S3@4w@M7)sl4Z@Cmx1BM@IL@17?^h(Ppw-{dbOAtzmO3M0KUE6fKEZ)IjiB z{LxKTynM@pwQa1zIGhLPx3T&&eZF>lJwK?~sJzgnos(HLh|X!7$#qm~fEnot-G&Ep zKGGbZ4RkBpn0Pa2#vI}GvhI6t;!7ON&Eqj|SNnAA$KZ$OLze-dK52y-s+fPFC7RfR z_{)w?J3aYKvsd!qtkXqcyKU>s+SnU-mWsb4M{=Cd&fA%Nvp-&Ulvw=v9Kpbwf*B7< zSvkOLD{Rh!$F#{c%1hYGY zS+bt#U){HwS9_nA-!&ow&6W)0FrD}1%U@_Srf6PUzAr2sc8mr`)ZjzJl>MZx;Z++e z;BpW_c)+W$H16*6OEM9QI3s!QLI>8kXe1-%)o(Jei>{-&14`HOaYr|Uq$MNOC~mj( zVV@ty6*a3BIqjhL&BO|a!+G?^zqtI|{;eQyUT2v?Zl{a2HSCj%Z;yP;zdpxZwmn`d zIp<8bcf!~ZjeT3st=vvj4+GT-O)Gc-xC8=LPIAO~w~(#cw+xfpjOn*Lf)eGZjPB~O zNTcPZy-Ly_tC2995r~OJ@;8T;))1fiimk+De?X4g>b$q%&y32|BrSPkwq}uZGYKFZ z`*4YCOL1y*{->{$mf>CRTKP76;LAgoL&=S0|5&=EBEaxx>*5?gJ5V@O87Hw-`O%g; zi^I{HzX3+7lvi9U6}0eYbk_wM^3vO!Zbt$-hLrT2+?e|*bQ_b|vBO7)ds%;^X`*#3 zh)(Amgil&?tT$FW`nsp}!_qAf=KF27J(;YD-WJA9OBED^aV*iDQ1hvsxXClZ|FU>E z`ujL@G_lDDNnSqvh>jD!8@2M63n)a7_k;`Z6HE0CqR(lgqk*p`O@GYmkfu9lo;tT& z?#ynzg^@FZ$kFvYD|tNYqem}=2Q(^fr!*nw|K_RXL8tdTLZt?@4X&t-+Uy4>gd2P?Z$=szcSf&d^#rc z4bVxd=&B*-druXvv7hEiyjHkL( zYnv^Tpp*%E`9)qjMx91HWZXoa?)}+Js+TBNr?Ya;3R#*JR4p79Gr&Zm1GVS}Y$-f9 z4s|#NiRj?&`Eys}HC*Fbv&i)z+MIbJ7rLu$#F%Tu@u`a_qvptA{n9 z#YLOr?V4I<#;}&HjM54}%=cOgrtUd|D>>a(1&f8Lr_uhW1#s4_ld@^R9uDG(B$qQA zZ{%mlF3A5Kul;t}b%_M%vh`GXCCgA_J-KAdNP$nV9_Trrh7>EiUPi|is>c10FZin0 zE{$5rfigW2ocK95;#=TlySh{eNVwU8#*sbRZlK)mGV*fhOo?{!6+K*_XRy=e4slsy zr}(Q+EngU)UY)3wvg(-g^;^73sN2ja0QbK#BAaJh&pUtQWCo?4#aR%^)9D}_1kvu_B?YTQ7L&Ee$0OF6MLNJHRK zW{uBGKxxPi?b&5Mpc%8bKXA1_ThQ-$AH{@SZhRw8b zO3s*6M6 z&|smjSxDAl<CWkiV z&d(h@j2jpKq8Evb(D&#g>5T1H8!3gUx8EO1TelRwmTy@=!LEwxx#X#|wjmEO*Gr}x=k0oxweIqhp;azZSPy%r?RRR7 zNk#u0^as{E6$4V4Ln@Y_^CqN6l2lyo&!Y#mlFl)iBzE7MzKx6K;J^%RLc+|54_s`% z>+qNSm}#dOlOnZPsbopD@g@^f3JC;ZPP)h_n4dGW*Y3li!XD;F#uR4I5!syzf&%&E zu@o2k(vVsN%k^GW=%}f0Pr?`7G|5X6x0aIkq-DM_+{D9<&LJ}tBBxbnowH8~_(SRz zWqnvE`hsRzGMf^enf&Yg56OMgxP4FgjJ@XyGNHoVcU`_(N%ZoQ=j$8a7$2YWY0C69 z>TrA{phB3U=^6a0{kWXfOG5ntyI{DE9$r4xdYR^xH%+6S%X0j$3R*=zl0|>a8P9RM z=h1BWBPKG@Fe8C}i-H?ZMAW_uLI1lN-CFUCR?n%jbFMm;^Lxc+4D9NxI3i_xrX^`L z9;QEd7>b6>UvlH=tpjz*PygmLygwWAPqyCOiGz;MuHEHRZJE5CEX*?!W2!Y*r!uW)qI@*j;$w`ta~)XDxo znyxY~s_yHO0)nJ;N=lc+P=bIoNDVM_cXtT`C<4+99ZGk1NyE_H-Q5C8_+Fmh`~Skn zVdmU(_FZeQwe}fOw;=zBVX2*9c0qejNrIM&E;)_5YjBx+G+U!%KQj0#Ujdg5Du3X) zQ`GK#;sByBqFcCk?pz$!>LAnjtBVI^Zu`u2vV#VeT35^*e2f z^YRO`JB}*T*MJd>*Vo*I)2=4_B+h~{Cu#fXw zv<)SUms;4`k!9sB+=@vdpE(*tes#?ZUckMt+Q-!L+TzaU=oUWkgPnLT>d3BwXoR6* z67N=<+=^jx`Mu~)7Sn0!`>EPqZF5SxL?2U(tzFnwKfG%P9|mP!{ktbtQxQdzTV8Yt zBGl_Gxo%)|DT8*@c9&er;y$2zvdCNeaV8dWzKZWKn&Ex(*51d(IX4~H0One@Pg?1E5aDLufd*(5l9IdV5In9&BzUY z=1hz#3rEqSZN~PsmxpbClv5mSc3ne8*MUJ_7G{4{B);e!~i_W>Q(^W4}Fj^RPF6b9Pere}__E)$T#Vye} z75#_u;Y*BzDZNWJT7dJ7;|UI#Gm}Rik%%q+=^7s9S?$#_6)k^mCl6mi(&nkHQsC?) z2{h8TGn}jUlIiasXTs^WN(ma|eO$_X=jE08iS^KsYS68@H$Ye^{c&E-bC!%1EM1#vgxo7;5*}({i?h*|%)934{K^{p@);=SmqJ^~ zV{I+20XAo%DPd&pdNj8VD`aR&g|3>J4Cx!_w#d<29&-M#4vwrSp7;hvfeGy||7O#g z!K3AASOw;(>RARk^9Jd+nwe8OY#D|`Jzn>(n=fQ;f_JL2i+PLxB*^IpM4UaQaiNO_ zYw~Rc&e!NX+!xNfnP#OfXHmbudxsj;{3vzY$5GD;M^tiaJxh!ipY>xrvx+Sa)P!EJ z2r4>x`9qobC<{MWkjj{2y3DvTcKKWRk|nNJBc&OX3)wVTA5qoL3bJjKRvRUl&$+wM z$oOqLx=@#I3X1-FJychmzC|M@;gLV={A%l=ALLXPt#0|5eyXQ?llwKu*9eMOH0i_- zfE8xl;S?GSzw020AI--VwBYlREM8u9isJ!X#9c*+vbX=S9*B0P@oLOqUM72OiHF$ z-edUOX&hmZ(p}!l{krANU)rk5iYx7ESDSc;06u{3_PreyZZcoKsab|>yHstCn!S2C zFImdOM57?a_Xe*Zj~MH4Jy)YxlZM^t!OuGy*-nFAg)Z>)1vrtPi*&o(us!ssN8!0q z-7+C|O9d{Wkd(p@)Q0tZbf5~gq!a8&=+{2cg=|9NQP|~Hh?k$9aE3h^7}b`D@U%O_ z7FKmXe9GK<;jkrFow3T1DQed==oI96?!H_|fMiIX$?{X=Ba4=MNV%ImTJs6xm>2hgK@(Z#J#jHENSldy-w!gUarBcAnv^Pa|SX-XPFDg7R!uOq_x*m|^X zK>e2ns;y(D00yj{5R7)Pd1|gcR;_{>R@ykTT(aN8`qXh0k;@`+=UxlKJk6YE1+2=Q%+zlqv=7zug{rE8RYO+m{$_ zzMN|FpX_mS2z4X;=aP5~T~Ug6sTc&?*Lzak*$AXIxSioRKV8qUC=U#J+|(sC zJ3^1Swm;z1yJ8MimO&ZdA#!XaC-uaOGqqkaamKneSP?=3|qZ4p4z{( zEqXNM*Kgn+&G%arPa_s;>@&#a6CRf4P1%2ozTwt$uAQ~9GYQDXl$vu+zM(lWt>NM9 z?>}WA_>=zbcUJuIcXR+9cgmTG7SIJ!%#0W?U5)gGnP-UB>b%~sE3bO{h3_3CiT(F$ zV;1Gdwt&bh$5n@EbxJegfD8|FU9r#S%ulMw#sLUp+U*_RfWetUXCuTEfynIR2Y(evhe96{;aKh}O@S!btrED|+GB?I} zGXg{H*H$_kp+qItCXs*np3DD6WMC9ai0vqw)I4#~)eFSbr_8Fgn$hR|SkRyjA;ql}gOH}z420}s**9EK}Fzm*-NqkQrm1&sb&z<6Q4o}tjK z;VtqFlG5(M)F#R0c@kl0&>=14;g6lnmB@%G+9If|OJZjEk)&!*uTQi=9p8;}Yx?*q zVbaEo87U`3fPjnD%y)9GYOae%ag$L?djGzum`)e}S5yd1y&U?5>7q&^QCYz`cY->?sIu5HvgvHd%rR{zggdgy)$Z&q!{Fupg3 zf%G3f(t0f2XMH8VT$j%6V+3;8SS+1b9{b!?h;_O@U>eweSIN9k##$yg%@+GmIn;q4Z#13Q^udnXNVp{HC%#U@ec zBHBA}DrD%BGF^}o;q@eSt6{;CKR%s?t;tqf#%-dIrOo%V>QUZ3zQCSzcF9&Bs!sxz z73>;xJZyzvM0%|X`s{dNsO$hiS}XD5o0Hsd7q(ADA%$L`K%JkKf^Y5I@ZL83`TX~q zGI5v$7Sp5}&lQ~VF-aCQQ*3~8GJQGPF#amr!Y9Lz;WGM$e=uC5L8=i z-o#%YN7@0@?O0UAiBo?4wLF>B+~TdGAI><{Gn0J_qz2-i-!)xjRfX@=*$Vc&Mt|BL zLG<~rx3!|G{<@X8JGq4Kgx3&T7UnJ9$7LM4X6KzYKLWeZxq5foXZ`H)bo5cavWH(G zNNL4kNWyIDjU(e6gDX8-Zgwx;yRgb}jq=8$m(dMdd9{x!?F3c+4+2W~jrx%oaSdJ? zLA%Wezsi)#;)iaFKQGO*Gm0&*TDr4rZ|5UFN&p@E0*OYj5kWWLf7wLUts(m4({|Vd z+@Pi)Qn&0H_L5B4N&)&cl#AHeAXl+xY)*94z05Yp=A@E8;;M=ZgTOK%FfyF?7AcQL z4BE~h_V@b9pk>~)^Pf=Sx;VX zTjnhK6^wXdSW-1t$QsTnPh$Zw!v?=&SyrMP0-o#Z`-cM!5--rHYhLs8-jYQ)g;U)+ zcg{9;(wh@65n5Y3F@nI{5+`kpbsEL>b!6|J|7te+0{ssbPJj%J+stv9UA#h|c8&Jvg{iEV9)+me1!kr#7iCbliTt zj4{1M>X4evXcycpKYjjCn5i-WW2i!Np%A&t9?b_mW0yiLs~*p z5H**SE0GbD#wZ3JqPhp&#QP_tVFOLh@1(@q9fYmQ-9F2b%LU*2-@^0PQOmU~Y_%74 zrXkwR@sxdZ%iOnU2uayEo6BAUY7IYU@LntVrTX^$UR=T54XnEu@?ovz`PE z;!!{Phe~38@A`UBA2CkEF`W zZ|5??hVK zFvkEtq`Ek+o;OZ*p#u>BXJuvOwh?%0KN<3O{F_p1LTbVxdj~PbgS^Y%Bl2?uH@DET z@@YH(SKUT%+)bmHUuHASny>-U=7f|->UMq_F6r6rY~7n7{az~3z9ob}&kIdXG2567 z5C%uc_;=dCbxb|hC0YK`F`n;;xipem-l?=&ugIjOnHo%GB3^6jfGK5l4nHVTxx<(Q zY-f@lnf=|%-T6!4Yt8ZxD0Z?m!WeqpL$5_%dQ@d-LTVl+SoO*@V^BU7v4o9HlJB^G z8Gy*c;bJR5q->xpDTXGoL?G|P*}w4f93FLvel%Q&Al*7PEz|S40p5o&G;bZuQbxzN z0zdzojc7=ffKf2>`DcbdvCG63t&sgsWSll5O|cfidQA!(+$?vB{Ff9B)Vq*nBq)EU zMVP)dFYfRrqKZ>XwXh_b9RqusK}3WG(oH37)s(aVnZ2IpsDphBka6IsZTFIpoyL<} zfGaALHR+GUEzL=VmdM8qj4Kei6AmvIr!=XsWjQQ{7=lpWSNX z8}*}F^s-t0LnQk+=H9{!7u5(s#33)pOS?RUzFjDS4iq6i18^lf-x!nxN@s4b>3zfx zY*7&ZJ9nQNX4hiMG_JA-v21C^7fw!OsA7mvbA>0=-vI4-_^zFCh3u{lb%iDdKx|ld zhW{OcY2?nmTpAS@rIZvPXGrla9(8IZ?in`{?|PaYys^K;$HWPm1r(XLTQZDRMe#PE zduaQSPFs@-0}CjeDRq@jiq!mv}&hP4!V(Ztx1Gh%*(>}B#opuRHbl8V2Sb(hFTBpC=!maO z>IGx)Gh6!>nQwXRRS8z`h)6Kkov!=2ePg)qAyfh@J2Lmm`G5hB#i~dosQX&PN8}WX z?jN7nEp9aZ7K4Y~ysY_0Vt`gPHN3>&>R0!R$Go17YV&R|Ot6xj)T+TwR-H@7 z&2CMHGy?Ws%V`o$_1a&|gr9r7C-J%7kKpmMvsWXL<5*Fdx;qfgUxOa4_ApyJeh*d( z2?w<4r)wU6CFKz9f`IFkK4-;;5MHXw(=$r=xd?k8UF~p{q2}4!DU5R5NPukSucqp= z9sNDd^k9r`t=@zV#3nNX%f2vN7S5`lp?}lbJ&kVs8U|9&jE9aP6J@&$C?GB!l!cTU zgrB3I8tgZ4nLPgMI=XGi&!n?@Wl(h^4{z@0W=NcEzD=nBB$&Qemmv-fu2%3vXx81J{S5UvqPfp{6vkBtHB~kUdOM#;A7xoyxlj+-2$nu8Pfe z+pR-M0!O>p7u&Bc2@y9zJs3HZmdyDOtBJBXHcGADu-yMpMcH%52u1&k{A|#IfAm(S zx2$wq3M4Varc!|QSl|J2e3@~nPoHu$)Eeu}OyQf)VtdbscrW*;}4;ckyc^Epz0Kg;{vw%Ox`+(66Yc{DK zS+`wI%RxuB;Iyq`t2gi$8>v-R!@bkuC;9O@>i4}abS_lSLNcz`=Wd?uMAh+>cPb8$ z7dySNBP=3t`SN`fM#=^4-$SM*QpjK?bIz;oKGz~$0uLTPxW>DqZ2Xeq`9Koy5h1jK{Kq%~l_ZA;nSkX4{nDzD_=I@iWg);f{~eDG72~azmM?C^2IsB%JgU3}UMH zP3mlEA$)*5IPN;oj>^9omqDW*tQTm(y4 z=Ny!=f~W;b=DZM?!)VspW=;v#ynomNkXS0FRX*`C2;SYy9U5%MWct%0#_* z|2V}e*uyekiv^q;@9@gmb-%xQz|OK|EWMg+rUw4K&?l;fHx$J->V7ffxt>< zhk!tC#mq!wZ>gCQw4pVA?TjJ9HMYS%3?lQSu?9?GnrV;iW9 zI0=tTy7Gm7+4SekWMR@8Y8<3Td;l<|Mx3}`xuB}hND1Rz3(WPW9HS|wue9$05^!oo zdojQ)A2n~WhF*P!ei<~nZ9Sc>N`Kr@@_?ie4$i%H=F% zrXTZZW!5%l@-=MLZ8>N8B(d8w{c_Q}7E~JbdaDg#SE!g<=IQP53xqY5@~a*-C-2%s zYgJu}5#V_Fy0Vsj@4K}MbFn-SOVgxC6Sg37HXvQ(l(W+=c5|VaEgb<8V%4L|$^v#l z()XlQS0*h-5a*mrL80Zoc*HI;j4I$fk?rv~bZ?KG*%@=1N8VI^$=YgxcNiGG`Az3K zcK8B2PIBTz^omA+{RK{7ntzQHf_w?%EY_e1OFQ3674N7o^SDYH{guwD&tb$M%naL{3RTk%)>rK;`)dZu*`kdpNX_;Dj)iv7eG`0-hue{ z?~%Pw-Nl})Gn1ubkD|1zms!tyaJ(?;D1YM*C8A}F*}0mdLp=*NbRxKf5Ac(I z@@`YUS4)Wv3|Zm?ZV_PmG6-XJM85l!HMdY@%FbWM53`-g#&;P-G}Q1$f>w9jV<|9C z(AD=UY=3*jf-%{)lQD_C$3C6lQR+O@(_p=DwO%)FZTPABte2n|)Z)iLd+W8)F2NfuVy_V8$3$Z_3^BVZJ!Zl2kBXdzwvTSXX7?XGyeCb25^3(d0SHw zT2VHP+k{z{y`rM#Ef7(>ztIdlJ=JVQFQzunM;NdT9nAg4Cs`d+9ISQw6+`yelvaCu zaC_a%+4#i)Zx80rnOBnr6$i3;+UaE^Iq84nf2VUGmkCuNjnc4hy=DS#zxt%2n!_?MD6z!OdCI;T7v~RqcY9706ise#5 z;iOrUSaB%(mn5W(%RQfEIc_K_oMuW%E~v=L{k+)UxT^u<1}WyXt$;X?=W)XE?8GnD zH+yfGMRfU-{{?vNea?GHaPloabRM^YHdc2P{#5MtbC;`KU8@@9NYJfhSwGIHqL@C6 zD@0 zp3_}ymfG^P0>D|;jNksDd*(fL#id`TwzkrgCC0kc3dCKCf2Xfs;lEdsRVjBLZRTh) z|1{>vBdDs0RNl;u2Qz*U2Mo$T;$kn-QE^&Vy40_@Ii1=j2~OE>4+o3*4-_0&)`T7D z2ZTNKHI&}DT#mVqD#}6yIs%Z#Sj~WF4iP!qZEm~3zo%eoq7fkbp`#d679P^z&QUuY@(q`ORK-ZTL7mdHZc>_N6B#=3;oIypKc1;>KC&P( zY08HOKJOXwcQZ44Mp`_|n~e&NB1e6{eTG~~bNR!;(1S0>!Ik9;YTPdv4O_w<)q`;4I9{lEo!0j$td_owtWs561R=tAP*P8)^Y+nsTCbln6%-$$?W?2;r?<% ztst+MDq#F)gYn#txar=Y9bbigS32o;wb zw_o_`-CW<(o^I5uX*w10RfTyCQosn0-ZRoWmT~A;q>L?GlAvqRIBXx&+hi&bNs9kE z8tb&eGhjdA<7_=8r76#4%<%Y+#lSoa0vF2{H=;FTQtmX<4j?cc&+EQko%{QUsi*Q$ z(=uk(NdseXPugKo-8N2QQOKRvSt?JWK`@7@G4d6~I!Q^{PL>WmjlkTLTVU4v_+ciU z8VUW5r78mScU8RxFd5kv6c3jG7vZ8Am)Iqb);YAZ$#Z;9Rp zh(j%Hps7i_9kg`NS`wPBV3ukLN&2|UsIDs@G^y}2)YdrrpC@sy)Q06k!Y%&d| z1NcijB}h+hSKoGBQA{UuqB_3@C)W%tIiLMaB1t#n4O;a&o8=*8Qk|E^;zI4Q8^(n3 zCBvS*frkWY{!qj33o?osZs$13h+=I`%L)kFOwRHLB?M%cr^4%j679K+ct)1=JOydj z8JP<-AW?|2hr-$$p^!#Vu^O6-3MvJkR^~mU!jBP+U&|0Ejb8z#6H>V$uZYt8hvO3- z!0{#B?wR>|{&4Xjb0dEV+_%s$8J=9=<3_F$ZsCg(s$GGCH~WB1fcS=53G(6EZ&FF5 zB!*joV^*lVIVD@%h~m85xyd8CGVwGFz-9Cl+s!vYr$=6@t%5#rP&!sWNWXhXo&3!& zr6ZPxTKMBwPbjiuz3Px4WlUwFRCvQREM9W6K|W59Y9r-EYedE7S8Nmnw&=l_QAv<% zLY8y#+-yW!BfHYzG}dZj`jR;UO_46lq5ui11_UG%&l67PF;L`HtHsCT3+f>&$q$vO zX%B#$mFfi9J|x+lvmn>waAxQ;)6JHEHR9VoN~jMm0#jO${k#)$bbV?;0=L z!yY%o>l%+D23Jwvc@`T_Oq6Gk$A|5%hdpHLXnJhSN_tOM%78}Oc`h=jQQ&@gXa56JcO`d5$<9o9MCmgpEmMd2js(tU-4i9~Q)fwn?3ZGU|g=bB71K_Brt%83V{27n)^!vNixsM~Jr=BwH7b*C^0 zj%jjUTI$=qZ;n%2EYR{p?L(z>LJo$*sd{7vToR@x;U1qa=@*JjLxV}-gxa_4Lp>Gi zxz%rZ8%)2$sCw%;Hd{>$xBoEEzEG5vC!nM9*6wwteL*fL`~J+70AMoU*6iBse9bBO zVy#Pm9%oR;$KuwStj#gJE1~BzS48vCq)@*8ZNEc?geB+Dhq9YUSGIhkb_W%x4E{Xa z+kXpe=Q#H)$o9~3^$!E#obn;_o@C{$f;SEy0P6a9L2>fY z1-pGAP~<7CMu>O$s`=)tis+>$d9HvU7(MmcN1fO+UD5Yeu_y{2SuiU0cNs^Z&X-{4 zWBeBxG5D88?D6eilaR|gAIN6yl(H_c+p#5a0oqEE`R)6n+);R7gSKLNqFLXbz5@g& z^wLFf)C$PLe(-baQ<63PK@kD?u?|#LsSo-1oj%e3q14O}2jW3#rb~V0a(>#cd+0=f zi3~|cb9DSuFUv2zXt|ogQy*FbBkX?5@AlMJ*^XXZ`Z~c1MnAs*wn9oAG)vh+R8S1mfJ3+78k2_xfufBT)`C+DA_torwz)+38 ze}hXxWv@G5dS9>1)~oI_j6o`UeilY@zSUHxK=9&g!*Y|HZDkC^TC2uG5I!bS(@n_t zM@I;~(TAZilBqFRrkDN!4DZ^jLX;EBsoK3lQW4?>xG)EL6!zBWPyWh(W8xW zpE#pzflfJEG37Z&y%NSW@CwwwF<2ssxZ9WdhJ9X^$bTlfh<`U(d{LMLdGVsNMlF$S zsR41-ytmlxSDGjL%lb;<7I)xzbQRTk((1>jFrJ+-52TxrmIzW=Ii}Vd59&xL=hq%Q zVKQo7>I=kL%+6s6>o~xLyC8Y&lB`byS6>ipV$nc1dAuC^aJ5@lQW9O?N0rF6BLdz7vSZ9W)m$>&+7W>7a6ag!SD(O2|F$n0=W?rO5Uf$($` zQ_}r2G$=W^uj)#JM!VYVU}IWzV+t9C^mR3e8wrI)W4npYaO+FMvVJ2S7{3ATT^m^}u3}&vGuwB2 z*%j4H+UeawBq|Xsi;D^q9($)Zt$f$WN z9fT1WRh4&Bn~`0?HZQFH#a^OZQlU+hP$sBr5y2Zfj^No5HH=<5`^qD3vJ1Qy(iqeo zJ=fi45CD*R5%AH)3vJS7{~WW8Oe0KRIfgLe?Jx?v_`F=7hIq{#h}RMK=;uqVVb|X3 zI-qvHPr4}z-NL?JQ_Y$!7R)s2)w|L5jMTMHXF%fj=_eT**;y=aMAE1hsns~BDjR7E znk)LP0EuE??y(;P?I_htGn93%vgPi1x_B%L_5ZabYQ0!Ic-|_A1Wo#U{=i*}pw}LS zSBwHHQcxv|j|cTm9_N={N(r@7+k1(}A%(8vK7pF=4TB=YOfF{6){krcaeP-c+FdWo zM3luqbx_~2VJ#7KSpNam&W)@X#F($t-x37@M5az1f$VGw zVo8=^?STD77xP1}O*>7BZiL=JAbmsfXe#%4V%*`1GSNoOr|O(w^C@I#hnop`i&uy+ zvI%qPwH3PoM-|9~F3^#cW>$>qjqRl%D_nDujBS0ai@qVO*n(rCuS>A?Qd~n+bcy`} zN)hU5fc$(QIudfqkaQ{C%zItI$A$8*p>A1C3~P39W!-S+2gs11eO(E3M_zRD@iau;zv9_-)_H|?*pIpk1H27FOE+gMp+5N+t zowS^kPn%6eaEPu2TE)-CdBV}@G|AX?)$f+de_a!uzx|$`0v<@D^ZosM1O$;k2hp}+ z(S5X|tRdl)WbgZq#%4dDxNj@`%93Vr;pGCxYItq$3Wf|=ZK})QyPX@_v?y2*=~(FC zzc~`5NN%~%pi{U#aKhiQZAC`#`kICuk+jozzxV~0aQB%v8YxPMtSfwj*ozzgKz1MZ zrQIfD(U-RwK}087_5F%jXUgPp1a!R^jnZnqv<>RTi0=lM`imfhLl&SQ?2~PcuAR8Tfn}F3Iy3Av!nlXio~S=W~m50&a>$GPjU8 zN@T!G>0kE{(%Q2=8<0y=|G8IRW`W4t)b&{voHAGHcOFeQgIQoy^h{hz>qpvJ-trK> zKTq3Pt}pFCP$I;J=s|xb`ACKcQB?cHPyl&c(3(k!BLC@a$H_a z4Tk_F$@}&`VdS=ZAJ14udt%4W9I8eo@$%6tLQwIJU?S}*7S&K~YFswlGzJXjay5nD z1^-iOSMv#ckY~(F#tJHp4zI|)QnxUc-~aF$vp4uS8xlqG_?T_HFh2> zlY`&l=(hRskvjQo4m{Cst7=<~0@Fp=@i*^}O3be3uL?!dHA;-JxK))s){or9X&#P!n343R&Ma0>K$2%QH9QM9L5Fr&ZI}#ErmVD$ElT_cka#{g|xn zaM9K$3m4Rmz|B-fNx$XQa&i@ZVSznMhMxvo;lem{C+>M=E~)9>ydqWm@1Bv{z78(u zXjdF=-D02!VTm_GFrJ&r3x(S!+&z^5d!vJW{82UWXMr8%R75@xE$HqV5jg!#f3QRL z4^o&NtW8^bJ><3AM~jq_EUkL`E?i3wRf>buKZLCro*59h_@Gvxft9>~X7_)u01Ov) zJN@qB!R=yECEa}&tDJrHx$OKgQ#k%K?DmOobZtO6X+SAd!-wWmFTh-oXJQ#K*>S22 z@XD%+xz(W7E%UrgR_Cu9IP9q%S|a#$oI>}E-FiWDEPHse-g*vEp~TUb&RnsLL3416 zi_-4}9r^G$>8KttU6fGj0vKDHw+wLiC^^~l&lXJ`{dsn^h-h@bN%tJHO4~UL1(Fo3GU2czthBH`zoWx4=C<@xtSwLv)V% zG2pLNZK!an#rT@crw^Frb9zc@`-i(EV(^!ua;t0M?N`JkR}pt!k!4gG6q)0BmMGRe zVCjJa=0-9wC--f(4spZ>qs3vuCgZKdj{tPkpB*AyOQejXzHfMWI;crF%Qws~YtlcN z#4%%JVzRx~3Mx&JVK?ZoVm-xw{*-I}7+_ct8+zKCX$)dF)ko{%{hX|$x>8Ru%*LL6pvy>rID4#U?HfSQd$4LcO!mYA zzR9piu1%-pE%#O6w%2xzLSH;(vY@u@m<08J(#n>?Ks1wN2D!_O|2hM^4amg&g?Yng zw1TY1NGJ_EPaG6sxntV)*J2-sN0q+L?!m|b*8=Xp7=nrk6luQ^fQY_rk^BAdB2ysq zV!~zk5*Qh!Hbl};=U~iPx8i-W;NIElt}Kdi0{9fsB3Go?fKZ=B*fraD z?$T<|{vHTux-?MfqW;{NM9dC0U)zmO+VTk{tb=>58WOdRZn>%aSGR2ut_d6{I`_AXp}!e z(w|!y+(1FSnpA+9`t)~dhi~+D!kW+bh^&Xb=j9tlZ@o|OpB3Y0Rgg$>PAzWSoO0tD z^T?^ipV_gz?0M-@52yXv{K(l5fPPnxZ#JTT(ay^#{02BI&nT4i&`kLY3CC9LBrC@U z9LN_%q+7-lI|f3^H}4=mGx+l3;nJxO@7{<#fWBtX*(`20cpZ5e`4dlWzoenyd|0*Qo#un5aHCVE0H6v=f z_Mvb96=}-xwhfdb^d9I$+6Ud_%b`ZxZEqJNZ%KY@dEU?YFB6f-Ej%@$y)8HP!5}63+u}}^5J*(wVkd(t7_U(JmDpw}rV<5#jX*%M{ zs&)Z{A$Qt(7YvPlPP+8Ns2oc?FfJxgEVOq(IGBwcm%jgA-tDq$LCYfSTxqyY#-UXx zH-P951C64tw05|@hI8TZR7OdxrtJY?C~sAxDW0LppniKtU~rK!nETrI(%E~Dx#8}e z0OxjFre#B`>rNed`NrF}f3%|nDH|!2iQVN=0FBK@(j68_5Plu|^K;PB ze1`->gkUL_yY&&7LQy!fZf_MxEBAid`{y#C*+uL&u9Puf01WJrR#4H+Y#;zf7v8QA z!?~R&MZb=F#Qce>KjuBB&4wwqF?g^lkdjLCmXmBwGnmYRQ~l@gyj_ew;i zCnl+?UcZ*?xrC!wU}a-#%<)X*bx*&X()ybo8%d)AHXdqQW@BnE~-1;lRKjG$JRYBp78C8Zb5RrLr1s`BGEq z9<`Av{S}1)>l~(@r|z))j~1fC#oCY``}_h*;n&<3XDuR*-NWecy;xH`@}|fYw}(=z zy^Q$EpU-SzRnjtveq6NIla96imsG1SnzaKH<=7yWYe z`vfpahcJ10tI4I)qu8;<6o^nc8$b7Vko1MHXgCGffmHosV5-AJ)bXTUj+NF}n?9S! zBTVYa1kr$I9J@-U_iOp``_7Z=tCz|w3QD98z7h2D(kpS}H{Yn1cpL@PMx$M_2?Ce0 zcZ(*!jJ8`2S`j!sKA?1oE%4h=r`>_tF%e+qaUDCKIrI-EZr`CD%$>Vz zXN3m46$!!5M3?$(3B9m)UK$WDU{tsOzFRjey^$M_lnc|l*0K$Bz;04Mmxj7IX28SQ z(_j9+ADD$xR(S4Rp*h#wC-42d<-{vKHRu)qge^X?URBs_?9O+LlY8F%jBa$mrBwed zOo!dsr{Mccb7K6bl3ImvbTXyV0r`~nGBRSYjZhXLUvndAs#Q(z=y1DQR_$#YLEu#-|Ule4B+_(3SIJuN<1 zbq3?VhlI8H{jmDmY*YrL5h}kU)FMoyaA$bCs7!4T1j#AOPK@Zc+4K*9{8z!=z}x|*i<_P(GUtr_BOd7{87D#nCY zCNTbX1{6CUN%hH(K))qECUEZN9?VKI6Nq38j4A zLDyt?E$%&B6`O)fL(4tR?g!gAz&8N?MF2G&SO;;eBxN&8T>kBD!`j>+ zpRoKxt|i!*O>o7yvhmQCWxSJSn2wH!gYx%f^gIr0Ha^KK`I`rHU{s%O*d=`%>aY9P?^^EB1(i*U+YIC6(L+KFZ1{)IP_^atI)DQV%mD}~T z+%A|>Xk(qI;Pd9fFPymO2@+a4O^ajMZ%YJUa8adrW)!|H`skW%19*l1S^u&8^T-Wm zjhl4baxUvoDCZl;kJstw6_;*5T@JhE`?Qh-JiC>hd_CA;(F}(3BrQ%*J-qin0a##t z0iXK;1WPSXaQ_&oci;Uy(yTS?F}!B|s*9(9K^VUgr&TIaX>3hfN;Y$-?c&nYIaxo( zxUHhomtyQU*TjW|_nprqVYxAUQ?s?pHr*J*QkvZl+R>V=9KKMk>Qa$7i~&(7knvxG z4O1Vw(&U;DFFAZMIKF2WVV9@afgtbPvI@%ee0txm@i#RrVkQp&{z1nfIl(|-^9NFh zMD1oyQ@t%QC4GE zwEs|ikTVW^phpNCzW$;zOpP5u!i}WGZ1KHhQ7r7Z3WDTvL`qcc_|#R&gC_lpBt?k&*7a8BYe%o zKFUjE{Q?Opw8R7_bBtEbcq9u27@qLA3a_B0gT1(v+`_~EC`mN%E*$*wwo(pZOrm)m zO97Tb`GR%kxe+tZiCFL9qdz={(i~cNuk9)hnyFtt!XTX;lyVW3Tl>Z6_Lr=bE_&_B z|BZgX-j|vvPVkpMv3dtI6A!S|f9n7E0G=JA9>kE=OnzF^SjCm{M;gScE5$RAqQX znTnUFqzc~%MDNtq>I5XcoB;+q29I2NCptq~rkLl53aLH6n(MW`5xUd|j7E0Cq@E?_ zG~;Nxtk5O{5RFH~nSg!&yoK|Ty#w%M&e)IUBm2Z*$VdA3>s0Iv}HeZ6?10R|8h?U2e`{IDie@{7~3AE z;=b>Cw9hrFEx43nL}GV{y5wZ#2w3Rad<0Vq2s2P|5Kfz;roEt2`wh%tdY*I%W#0hW zg+i*?UDhLq`zVo20w=F#(JYW-abE(JUTwnHblj~1iM57%V*{KGNS5M{BqgxU&wspe zbeRJ!n}*yeQYD(jlHwv?N(z%E;Ab|U$s*A@IJAsXZ;u8j zZk2Um6t?fNIsa`B=g*mFs=@KJhn~sLqIhkpsvNavdFZn1BEw#6$!7p#l?hyqG>QMn z&3dpG;m_0ibALX79iRd@DEk|L*e!#a#Slkjh-qo63E78LdO^INkLMjC^TTm4WP6XJ zYY7J*wH_H@Qpfa2W%Nk+xr%z75a&WyPeBOEPSlFRgHwxV1}6sa)rPBF4JqTs0bUyfBbr@xsguTbf9`aC3pjr@zG_1%y!ay{wU?K zv~$zsKZ<@ z2mD~^qFF1_s}Rv;Si1ti&)g}4oIX;5IAZV&vKA1GzR+Yh#sWIJLM=aLMtTJFyN7<} zHZ5fOyay&{gPk7Jo4@=YO;;IKRoisw4r%Fb5Tp+&CEeX!(%nc3NOv~}q#LBWrTfs` zjesEVZJ+o0{`e2CbML+HnKf(HtSJrxen_k>QYYC8Qa)Ws+D{(k(d+wNFD9etk_D3W zK23Vr+OW>b%!I?OHe4qL8<{eJ*fy}gydT_?t1BjGYF+yG72q9Gb*{(J6)o9BzxVF+ zSinjYy+V@hUHV`sta42c-94+;L@Xd%y`NO$ZI84XeMM@n)9Strw~ zCxNt=P{sZONL(HgyIdpy4mq(@DYlQXqOHt`0w&eojEpHYbOEaZm;kv^dsN@$K@Z81 zK-f({GDX5;_4l_Sp0`NUnl|k9TH$tCwSlpTadmMhR7zve{KRoYQ5iLNZvRfIh}`=f zDS^B#ena(mZ3%KcW#7QJcjl9=ZDgN(nUh!pl!V%k~ z6whM@-vi;pMr*IRdHnPex2Ls&%qu9&+8%f*)M1*uIE{EtwTb#|MAS7|YT0{Bv#idB zRIJjrK;<{gMPvUBx@f(M{9UjGE9P&HQ*As-xuBSa0(;0O7diGS+lzmrc#`rP{ifUV>YqWS}B*0W|8R(L_%3j{ia2L+X5ua@A5~MaE(lC}KeE$;@wgE;4;mm9fuLMgU z5G-u!oOjxx%3TzBx0bT3W~xovxhk>%k(fm>CqEi5B$>hPzk;PxbPn*Uu7&Y+2%6($ zZyQv|Eq(Y6*#G|x(EXsBTjN=MRVPDhMOdocBl*<5kVrQLSUVn51nR9lz6iks^E(7h zL0p7lO=zenHSf%776OqkIM?8y)&@9R7K-O>c$}Ag{^UG7mc*uC z(qeHNgPV8fhwk~Y=J2wMZ@Yq%G7JOXBeCC&LCfr9o3_@^|Kw_=kttr_#YQw8KhTJ& zlDkRBDq!@V;>XtT>A(v4_hptW&Ty|+nTeJQYKDau=#9EXWeI-P^41>d@inLjb093m zJ7kU3W&+*(L?91U#Bm?kLqpEb;XMx@k_8Q@#N?ebl0N={j5qS@^5gE7Hv-YdpVh9c~RN zSt&U_KK$crGyYU6PBM($)D3$P_w3t+l_rQp@4nY`XbZh#7lZ*e1j~KbNuKRkv?|p{Z1O$C7#oYAN#oy8lIj*mRq3EmxMT(JEP!z?k?Ztk6OLJ{F z7kU@@;hHsdt&s+5`LjzH7yHUj6YLspM^o&7@ayBdJZ=(==9Vb&oB9h{gb^b7iBSc% z8GUhMzj#fin%`#W(fLwNn6)a%mc{UP=u)tpW{Y)u67YL(;NjK5T(TU#ccwnC8_-?1 z6M2J-2}TZt9YKdPE;vZ<)t5;Qy8JI8p|vSsInJTi&VJszeX*sG83DE86x=<@P3+tIG;;4ftPlYl~wa>TD38>rV$&^G;o`@^V9L z>Xjl}yvWVCo0j^$dLIJ01?}tp`LZAZH1>+!A10lbfzjv>lR+{n#s!Gj6Is4m3iV0LTN? zUm!4-1G!l}wg0YR1;o9>I>miH82iTG>0M6>cJB7|h~lxscc;XlPx;8QdnJR4C_0Q8 zgn$3Ji>6+f3TVXe){)Z-{m1M#B07pSTY8@;*Gp7KuDRulK*vbA2n92zGyonT^}f*y zZU!Tl05XOtOcWBxj9&+It<`{uyBc!ro{%66WPi~iV=4iPO?L?<$bs_a^43KOeXa}N z6#hR1O%f=HOao;_xaCe_nKJv+U%sd6y}j;&EX^k&t7?rNuSZXGDZFhs2ZN+Mx%rvK zzgCLd@_Qu@bvg-ki&Y*j9US=`;Z?GFs{lc?cn`4Ov+usjWM##ZKs^#&szVF`36V|C zZG;lzXc;OIpKFz#n_wY6(k~c6$lL~fv(ZQ_CWunr2r-B_n?hPOzz_2b;i-KPW7IW)oT?I{Hvm$s4F<;U7aqpQJKmIPUC$lEXOMou zceHi__R66)%NKHBBWBS9OI4Tpf5-kur=l!=;zz7D+RZtG62Sz^rM?1{Xbv;@+-^2? z5&pRDz>{Xq+TsuFE->{!2sBx_8@}(}-3s%}Jq($`0@E=yLX`e2e3Wsk0|Al}1vK%u z6f%J#t)4ofH@ZtyIK)VBZFf`tG#w_V|B(lb9t=t9{WIm(WZHf`sBNuHzn;)Ee-AyK zfif&@52C`b_l(Let}hDJShQY0n1>VeoDh<4kT!ld({d84AGPKCGT-+AjQ^`H>S~RW zZDZta==EN{T|HdA_IKl=io?1h3$YEeRs`cz7T-4;*jc3^`+6+tvIw%hTI-hUwBiA` z3iyyN(JL7ofKUWjqV=>iU@g{<{R8`&<^24gI(zzhcI09{=t5Y2^V(EgrP|i_lC@k{ zQ@_V{8vny;FuNqd?Vvw-p6p;^7%?W2XH45fJ{GM+j*t2}+L^ce@b%I65ggtfZS;2w zDdIk}hhzt!m!S(*(^d>4yW=&jayxPM%)#wzVR-++DHw-*AgBZ8~<@^75+Rx6U z3DJYF>=el;X>kf3YRmUFnG<#Tb8c%tqubagrOGB?Ia2Yd+^mzClEz(o?0W)zD7pLE&8XKOR=f8 zM!Q>UKCeLYRi>jb%Gi*jrz0eiS)iu{FJCgdbhu{Q`ct||`ou{tBvXBwhWh1cgM8OY zWEdX*i)<6jYJA&^jOdH-BcP^Q;*0#qM>n~0aJ)2wE1 zyRl%1P}#4)z;8^^!Kw1m(i)tu^7<>}6Z%b_+AUyi?0N~+bQKz^^>lKw1_x^!!OQ9zHf;zTtO0^eWhG%Vb)NcS#{4PQ)JA$JM<1pHP90a|Oc@BHq zhg$8Go&N53_vCp$s=N>FS)-=somlW`UGff)VtO#W1u8GVRu}ud%h%I#8@Pf01>C*R zVboxu9qORAU-wXHI-fAM_74FI)ojYCrDQSbc<%Q8-o|&?K;`!*P9JQ(7VlagrvMlY zPJrQtqPR)WS*8sr;D4GRX-43|mB6P~rIjmEDEqvGcB1~PreQu5mp25349$)iAb<>4 zg9y+@|E8X8Y@rahG_n_GwuEv5XkS9M=|p!Obd)JyxsCL8@v=F@w$`2H@wc%I%4gxD z@P_pzl059)3+qDzEmBbmzLt|}Yxi@y8%lI87tki65a|4DP+LKo(r9UK#;f72jeL$f z4%=JH@F)2@o);deL(I;rljbd?&v2&p4_~y%R2ve_2)QT4wAxP=Jn6(qv9-^UMi=`amIwrr_sh%z&Xr0`^GX zLF+vGnuF$!;dAA$+FuK*@9De(&yEGn?ELN|c$}Q;h8zneRb<1%_=h^wr3`$sSxB`g zfrYkz?o0cJ)k7+?kZ|}3C{sx;Or`n{z*oV}hGa~r;z8qvUKvWqYVWpkvH++AFN|Lp zkIz%lDVdz%vZ;6cu!oha+Bd<<|Cdz19N~lM?<}JMWK9HhNS_QMu!;A1Ffr+n)W)5) z+(n6Pi)wwmpa-9a=HFAwK<+aGscN=kOEEyZXq{s$k{}7Jn0%WEMJj^PZUy zb(!z=#%N1v^NpmX`sETTH)cwB$cP9dC%V~OS_w8Yci)n{zuDDPD{g$XI@R9A=|-0hAK%3>gVg#QaIgK ze|cq?tR4mXCQU)yNYlH+P@`hhPj+_yRc_W$>nPyof#C#|Urm*fGlzfhk*nkCGD-S8 zuWxrKzLLaNT-=hHu#&Q2GWotu1+~=NszMCxO24Q3SsBDbIm+z3rhN@ediYA*?RMT0 z#Cy(T@I%sD^wavTY+6mPGY{LE}Ci9)=xOF z%iK-7Gv5j}xM|Hr!{+z<2YD4ZLY``&zcGfSN-S*X(U8weP0KD~`4SGgXFBA`?65^) zOQ|I|va>@xQx7`J2cP0elP#QA&Ap}ELz)QT(kR~Gm?b+mg97+@FQ4ti^gGe60nelO z!S;-{v)`7^@T0nU>(A6%Rl|=Pd2i#|v3mKO>lgv31u?JGmkf7w>qq?@eewPXGngPe zMG5=A?fwXmf?;}RUWy1;c2VL8DJ;9NgjkNaN4Ws}~~G|e^$9_77Q zWuvj^T#sV4oI|hg%f8mXXpqSi)Q_a&l~P=Dt>`nQaJKY;d15`}$#J7zy|bsrHk=Fz z4zdc*Oz|P7{dD)`FK$a>r%11?EZ4n^Lr0{6y>EcrZmBispNWmzcZD;|6qWku#1`g3 z7h_vKS$jD^+48F}!u4JFblSGTDQ9=jZ}ozD+)SW!()lEoqz)oqL+NGx>bC*J4Bjc< z3r;UquPDCZ1xE+4tT8E;ja+r8Tg~pYf##QOU?G7ktoMbG8ImA7OL0?j+IJs~M;S(w zNh0XX=79mASH55gXcj~|`hZnuo;GU#(b_`{+&p&4tRM>mP~DRUpduSBNxLwu!(ASt z}KtG;6-5oy{c z!iVP4-2S*Oy?oUN!28FQAtOH({4IYM|H$J1)583Pxbq~7Z%7JB(!7Awf8iK+v3CNV? zpaf(&d9nGM!0~c?tI++2zk<$Uv?*q>LkRu{q861 zPU7jmU0D%laNndHg5HYdrp{}=Z;g#Qf(#X;0V7g56g+g&@_Q03D8Vp>TAPwMCv1{oeYNcJ6zq62ON zRjb)mGza3rloKamfTk2Mn?Y4W#X%XzXm2uSiY-lb^sCeQz7 z^vdaXxVT**_2lNp(M`I9==jUDB|)de21nz(cSyfTRx${tasGB{s8_;JU0@7bpx>Zzny}E*4h-US)LZV4$t1*# zQ$N}=CGkIZA9Ly1x*#NE7aBHwIs=^i8>^k=sNQVhC2tIDJ|ZJW@A(}_n`KRv>$&Z{ zyxjp%@1gpIEHKT43&s^SRib5Y&vi69KFa}PN(q4uRwj_tnEDT+ri?}Rl&j_}WHDLW zCvwQ-GDX=5wSJCp$RGMz5hJH_A^~^HC0-w`?3+5a!)~OUwF`=hAK6L$1j*Xio7}X7 z1LOq9vGDj33tH~O6O$qM_A;lv?KCu{31)W~gnxZ9UgLbe?WhhRYfl|Yp_s*X^NL}{ zO94@yW-ZsK&&X!e&|_)8IIYF9Lsp;S+H8LuiyeMyDV`Ivk;}qod2jV=2kWP^J1bY` zMJFM5D<;54fDFr2&jD+&@wu8Dfu#-1qvXv0*8*(v%`jX`T!0jC*MjfhRtVcUZD26e z9d<{d3IOMN@fqG9=EvTTzPWfVPcU@#MPxfwrv!^Zt@8{W}U4z z*S)4As5ICGzc_atg&hjjNmQ-A#|QZx9Wgg^8?Y6cRtjpIt z(^i)n&d;>ZB2mSOA~k8a!1OwQoM3KW=`^=5voZ2#$Re){C{aNwgDL;VEskhzC3uZjWFKXvo_}9Y@Cae zUJFxzU0BrJKl?@hLk92?o{oSn`qwvz$bjsGpF9P7#T7)$-S*4eo+QnjUvLqQ0dV_d z?X+d{fatDa)Y-w$PU!VKBa6MBi{_o>8z!^*ul!U@?z*%@k!M^v{-QJmKtUPBLjymJ ziojHA#!e<@OV`up2yB@hI%tmRm^C+pPgPS=iWv39duItUuN$yM!$0;* z+}|Ce`;A{}IVDEy0_Z5e=;7)phven~){ZNaJS#LQD&Xez4)m84QqPC-GCVBC<$WV#s2q!6G`;dU3(*v<1I*n9eKgWMBlrlypt2x5? zY>|;N&Ol@XMKGMQ)d2oLAtjwoXh7eki=Gx}Hpxd-9nc_NO5Wnbr$M&8)3U0mA2e$- zsD)K~_hoAq#8Os!Ib~FFub4S$Ux#sMuX4584rr>wC@qX(`~Wi0B^NO6S;gNzH!IVl8T-e>Pk2A1#-(PH%k_S!XW1R5*(=%i5gDup9S#&? zvMBe2)OCc5tB26sGQe)z=hV6@q%~U{JS`TD&21W26sLsSH_`FwI_UBvQU$S#9L;Ui zV0$66H;>vcQtja`bV!I_T+@=GF1~1Rl;U{sj3}~=s;c!BIMTGXdeh50gabru;HDCO zF{yEpD&4`2Q$D%kDAgc|o3E^VYx(5)JDMJ< zKp|x)R~n__AR10G&K{J^mpYN1GZFf+&*fpCYfau?tc#$U^Il6+$F_#V8-~gG3KBP4 zdp`UKUpD|8MT(x+Ko9ev%i$_LqxHi-oI(@jCwdQEm)=vZ!nysV@?}s1|z*(nIITMZ(nRgYP_{!mi0exQvr0?6l#nu*Oo7ult^J zPw~=0=#`;WvHhQ^b%8l6csJRE8@jd=Ka%hw#o{~GD+axi!%M}co%hUWRzMPs!wma4)Cp}LO>NwH#N4epm)NUh7_I>8|z&fFA(whC`lsY0%65+*RpR(*;i&s z_vE;mh?x3myRRq(yRW*j4A3U&U5?>Nq{GR_{EdOKA)F&=s|#DGr%FDxh9R zk6I^*Psq8EwOkr|zwmyE`J{&e+WTdz;;QJBTB{WgZ8&R*l|6qJ)2~9yCmEV8yBraU z12Xzln(VyyVs>|V=QwAHN^kR4VR@mJ`6do4RfXMT!i;Qs=?UL;y!t?f>r&6D7M_ep z>V=c`-CBuYA^{moJtkEeo@d;qnu%9Ial2OKX%{8;HFd_F!oa~Nwa)iEZ|5$f23m?1 zkA85beD1!(=_R!_AcDo!1Y4LOBHLCse3dT-)hm9Vr2Wgb*BN&L2^IStnpUifS7JO{ zeh3D>z5`^*4q6vAOPkc2-2TKLvkdbFiV2fTC6%AYPe>|%A8XR`p;1St9arx-AU-L_ zOx($%R#jrnw0^dq<+VI1;?ymZyVa)WbYMNh%TXw?$lxxn>2lluG>`PSM|_33C8$Z& zQfB;s#iWMflzEx4x?<8Wo1cUuw{C|g{DO_mOs_AODMi5}Fx%E89@@9oSz2Wb6WHj3 zKvsS?j3-J=>DOm*0z4|$vz}VtQ`DM(x})KuKlVv%A--V{N|1JoM$gcI7 zW?T;dV`E*^;%L@I+Gl?_^O8>FzstY}jZuD?ty`)GVTA97Cgo`Jgkvz4^@(yX{(V<3 zyL93a@3JrFnzKpLdR*%4yMwXKzK`Oj9KDkXf?gi@vK{YPtIWsXtIb5>x>4HS;GDP> zIuAMWFDtfA_G~D3x#-E!jqywoaYp+xe)7^ z{~=vu$<3GbnaW#7Y0OxX{vg&yOx~ZpMOR{cdGFUEa>{C{Wwh!2%tKC6P6jm}Tv^2= zW~!@~^;l=@mzl$nw35_X9gN>BBMJf~yUNrY!+6UZUk^O%XiD0b80A0N=B1@&_wCNM zHP(YZW*o@y#bP>FOMJJfxedrBt;BuK6c{t$it3boD6;ElAAF%|uM27lIyi}sxeHly z){j3qO7wr6{hju`^XA>5?0&sI4B00YS-J+6j>`{wwTH>83Uj%?@b;!*en9*ilJ8j< ztxx$?7v}dkofCJ>txk9{*nTDj&G*{3hHo$+_n2jW=dW<1&|V={6h3}yN^DUU*YFy54Yxs95f6{$w2`fZ-aaSm^iyqc-+r?XG0N8SSE~IE(!~6ZZ|Z#m9SqJX z;_sWL865I}xX4Grf!3Xs#9e=Kf+(S1=kGAZt_+P7V@>^o8FiLz>TQaIJxOA5q&!aC z_*4yXIQiX)QkZVQjP-*BH$QBWvr->SAV}36U3h1>lT28oRNQxH-rh6N2MBvz`%}fS zr730C(PT!jyQx*LqR$FvLVv8h-cpQU223tTU_lD)+8@wSn(y1193COo`9^+=7@8>I zte%EtV{4>FK3^_*bXIvwIDotSEoWlSsh-n=K1u4Y zTG5-vywH%SOF2>e`{G;q?l}^$O?;L|5r;vKrtT0JJ4BHWXE=2}#umdlB$hGr9k6jA>sf(KjOpaxU zt6bIOx#(*HUu-6(eYP^7bjp{ds2Pm6-?MEagZu<ApytUN#N=+vdYL7v{qb%PahXYW<{N zGDG?>^)8HIlQOC}i1=V>GX#m^Uq&kf8o*CkVl1~ZzY&gX$DrYfX2{< zSggFoVrJI1ccGCKb(LhHMx0%GmxSrB3%+3K&S{i8s*VYY=baY9DSDKU)x{*#Ygff^ zl=WL^x7t+y_$=Z|n=E-ofDxYzxDwq+GW9r;@%CyB?$TkV#K5Xj zzY#)R`NTxKi+&CS?0X^lGQnCbCX?OXc(MajL_bV9zOdoJ&4`qQ=+jqH#PDF**yGk~ zL*_WQDLR*DK`wY+6prZ);U0bGSVv=fpb@mq)tvNndi21i-_6b4g<|xq;1)H@{h@}e zemilbIQbA=Akj42V|3g&=L}8KL6sUtBRY#sUdnDF)aT>l8C4$)KiFpW@9&#wYP0UZ zSWsoMz%bkegXauOC$A?8-zB*(@!_?@GtcYOQ)%G< z|MND*^eqfR!%y73_%Xk~8G3I#=(p*L{6B@XKS{BgxctJuo)&&o1_d2^HrwRbylCwD1mH%Jfhqcao^h|7;rez0N0v^u`=()f@_hOj_;AbD z2RF%7R~3>!bnj{KJn>MQlv997c)IyRFccGcLWW%Ni)hJM+PA$D?>vk+s@dMQ;GEej z$zx+2)dkfPpI8StOW0?&SK5HH-mk(pFB9;inuuJdyWL*ER00iA`$3=ZA_>k+Te;$_G#9%LD&D!^Et?;#5$xkZ)$9%ok1B#EEEgtc>F&I-W1YqCO_f0~HAaTLiV3qKaV>Hy#C;!9Cwg5o zZ-`0xJI+MM4@iwa*q}UqNRiIZ;xaPIWW91eKTS3SHk@oJC4U5s%;#2DdGpX7If!b#H?g)_ zX`t=zeW$-N|HxLh#<+=ODo^ zTrYfz;SwmKi^1<(ks{Ttef{Sf^i4A!aP>Zx<;&o;+UO|CudI^{j_c5d(60!Gg`!om ze1I?a${)o}DtO`iAVTlnj6n`ob`K5v?Qd2Sx&9`TQVp7MuIJ<`qAzkLR!YNg`Es9) zH$X#T(B*dj(JrRTEw-#gTY zq{FV-f$>xa$wRreBm`bVRd^Ud+muefI5?LxMR^FrR-~9T_KU9`M3}i&^x1=8JS+lw z3L^6LC1s~`)=;rei?Y(R;gL*0oAGa|x>z37IqQ_jsozyTcy~F0RO-Ek937Xq>fj7v z>74pP4_G4po4bDGmffz~9ie&xUK3RqRN`pLBCI(h!o<2Sil<{oH0Bm>om+$7J3XP# zi=I0wujrL{YW-%XQ`GAXQA{dF$uH~|sx|W~`~@yMI;dlyAzxw8Nk5I;(%)>CdNrHg z8+b{7+KS}?e|O3sIdGy~e`4JITZ z0#EF4PCc?v<^~m>R&n%E0iM`5%&IT3&MDP@y4>s4h^^HGfhS8IarBwO=iigu5Z^dAj zG4qJOai+`l_t~VeyF|Z6(Cw~rb3SCkdjq+yu9R&{^ai9_+9_LxMzNYm#mo9}?3J3} z$>|r%it{|`@$w!~s0nd+_$bS$rGi3jaL6~0L=`f4F6E$wj`54AO7A`SDXEsouH8jyAH z0rjY6##prln8nJEVBEU#6(1kMyzF&>e&_fqww*q?o&Tt_n0b5i(*z?`d1{EI(xSpI zMM?E=g{^egSj#auZQHOV@}!n<|X#x721HU5V85iJYxr}7goiZ&lf161o2P#yf* z62{jU{qrI7A9&~@Jv5B@J*o|!XIx?s$$GZ?Sau%2**8@=efrtmh(ysN7`;2JlPQ|P&ZEA$!K&LmtS)<^# z(5ee+$RNQOPL9vvN0YO5A^_QxVa(Hv8W^~GRDQFppagLVa=FE+&JJlvE6C=>q~gV6 zxRJ+D5e>;L4GDGAYQWQ3&`of7{0q((@E`*TH@YwEx>Nb97iNb;HKu<~9Jka@>-AJLJ znjj@RYHI zDfwajy+I7C0`JVL{~WF^oK}CppVJkOs`}1R16Vws*%@W=t0@2<^^uPMe5tPew+&0> zS2ENpE7=5DL&MRfbqvk#h61fx;{G@tR@9Qn4nz%t9R~(vk8U;Q6d@PwEdIz7?lVsd zst@P2U}AR#-@{9QP-#phiM2toE)Kk>zdYZuDEaFb_j@B2lF{;)B|wOjMP1c+mv$<3 zo@euPZSj`E=Hd^pY~rqvkfH}uQtN680wOyr(+QQ>xhW!+IWkfD9c6 zRTka*h z40aw?`>glqDW@X4yBQ?_FPKCtgC2KBVvHfT%uHqO=2XnkH!s>@mPfl_ECytN$(@99 zJ|tuFnaYCcC!~Wbw5Ri>nUyEFgQf0>*2M+~ofK{;lCc2SM5*T_RPieo3^t2h6g!P? z+CvfEq!iCZjMDqxo36;-KXuG4lj+Rsr4s6i8*#TD10S7*(4Enf{8Xqw$@&NI1t$Nl znk+Z>Tll5>26;Jhopk_ePDl|S?CIDEpCZEwvVA!}W~oZ4fA}CnX{jykKP;f|<={jo z&beGdpT+-c708f?f-I$e05)PiyEpanCo;K{2C34D97ip_9y#I_;-_csyJq2_*C)q< z%_r$=ewSb37OC~>K~rTEI-0Gi-a2rgi~<&=b($8Y;%H|7IJ!2O{xC3+UaXIrbWdS* zzkmmN-K*qE_`sl4`5jqn(asOl#kK_Vo)bpu(wwx6o7;QJNxWvZMPGqv*75b*4m4IR z?5qp)&C`znakcz#cs1R4+`oLqj~r{YqR6)rX~0m_H&V0--a2UhvwZoS%-!Sakt3eNLQ zWRogzFpm$;e6ids*CSKgqOycorxXkGX{z}P@#C|YPZ=a*&p)+YIJm82OQS1o;lemT z>V{ScL~`+>6C@-CzA4-OpwE8GU8&hq(g`)OAO&}>WY@54fB=&%EH7LF&hSXhwfZ&s zV37^#ls9NGe4U-<%*;L&qXiQJwWD69tIMoW*sH<0*J{eTMmaA*@EP3rJ}HatnZ}9; zJfbiT6^=tT%o1$%>S}p9LIHD5TICt36%p+cDpCGPeC*U;B#*s5$#g(`(jY~=CliyS ztSd)ZJw$4vsujKLg@1NC^1owjpw>q&6mJkwF{UYdf64xbB!qEeGygS;70OT|gL5 z%izcMX{vS5%seg?5kJc}?(){}A+3muwec?b+3`um-qL*3Vac9)H;JR7ir$=6CD>t^7<>*~iZBA#E-3DIJQ$!(*TStrKYQ&AVAY1Q58E`Hw0ii)GL zDY-fgX5Uz$zsqQs`UQMvfKCX-z?X$9lJ%w?7CkNE>me^(d3^|n`*PYX;rW6J^<@2I zJX-(*&1hlQ8pL4VX>Gd#QPYSZBcP2)sa}vI63g(UdWK50Bw8T+>%+VqW{+_ci*Ov7?*=0vG)5!-?!|+q68~%7WW|yyVNe6O#<}0s3ny*UA&#_ zCw~_?2+fQVt9`*`vTmDZDJdp=(c@DU)2&c>12CHweVdUOeDWbRNm7UFieo7`sK3=^ z4&r-E zrjw{UM2smoaj!E{?gT=-cq|jczt30V)|m>EMe|;F6f%hEkMPj_kiu|GiP$BfuPYJ- z@op$_+7(@RC>!Zh3(EX|xUSn^-x=~}1N`pKbp@fKP>4=$kSsS?!yjEjE9rdX0 zUESg5L|lq0C2703U+(4?i%vk{EYJIsFEo!JHeDz|RF&*+QN0n09ky4QgS=bB2(q`+G*feQLKJCcX301e~h$ZUKLl=#dC)H;1KWY75dF@jL7CG7T=^U;+s0xZZ~-|!N?O?tKvO_Z5dm@ z5hrYS5}Y&&(a7&LM}IhU!4O!%@XyZz)`H#y6ID~éJ?BN$!*Ur%dy7O}`%W7dJ zViIuy&FV`-GrW=+p7p#6Zp>3$i4W6}vHb~_R;rVI{F9wFSd{7bCO}_dsDG64t2ATQ zG``Uo)!~?(=k>fc?uzNA6xj)*%Ut%+NaQiZwj7N>UWFuKV7M($V=u`J>>vmO{{3t% z!LZP?AS5`Wg6_|SV0HIS7szbRch?WocHQ5Gl054g-C;X_>+yM=4?8>EO3CGkW0cd* z*dY@tM-n?Nm-ccpfOK-xStEYS4kkzP1s$juDA?DP<0ubz6fnu!Eqan$9P2W?m+ZGa zP5jdw%z81;ry6Y4_!ARkskzBj!_9UAvZaZM=m2qwIPW|xBgES@2>5MYV2r`2=j&hH+2Uu z#`m4!IQ_ViYnR+JSzOpZz6_VSNg>_x74=z`&Y?ptR3F20KI4qfKA z1faew3BRWmo^@W3U5hwV){#cJE)WGX3NbBa7Wih(R?10)2Lo_ME>nn8zHyop3qz`@ zC;iSuHP{Z~-4{}9?&*^GcQkiqWgsNABP}8A&PAZ}`vgtvar|3HeGA+Tmk#938pO97~$-Ves?&jUjrSFC8q=T0{oiVTgq z9!k^C1`2UG3RNUKdVG{vRAnX(G`-afP?@>K%(%aXM_@>1wa)6ozRU_@U$B>85Xk?z z0L5H^#LX+-Q78sCy0mE6eI^Ctj(i?t%|YseHiddbCe8?YP~{W#NzYNjp6JAbO^&8bk6i^6R4XG=e0r)@NgCK7bN98* zkBUx~>1W!QEMTQllt`K5{e+n>ayqu^;q;C@hgsZK&9NkzK$WuXHj322Wf*(C&67(O zKV6dNS*yx7y~l8?%3gu+hmTm~gYi36hh0-*!Evs*Tu3_t)K71O9zv2W;2K>&r*}W$ zAH;G9Bc8Gqy+PuNJa31CX&vrbRV75R$*Gnb$<;7wl*K#tC8T|aFgeQ zS@kMqx-Lvx-+wrABB7^B+zGr9R&!ZCy<%>hZ3225imx#IuvQ9_@>R+Q0R-dVL_?o- zLOWVyQk@3?=j~v8AXuvSjsL-Hi9|tk35$8|ww`Brc_h}XUVG4zarS*hQGM`D_re zm!6cPuM)h*qV#=?m{cE^@rPc@MEOfsR4D?6IN5a?E%sW%-prZz9SSwQmCf{&DtFkM z5Pyd__DDb4RQYQD;eACYMo#$&OMOBj4q@2_81%`XOLi(X?emPQTQosT)**WwIqa1$ z@Sd=UwBBIk^p>W-I!XKaRK?)~qm4(`EJ`@*g~@K(0=;q?i?s>d`E1Rv6(b4ZXY_%6 zC0LC&6WLqv^n|TeqwJGnKysIPVhpT{+V2~3Fjmm$C2lTQfBvk9(dfcC7HEC)g;<-p ze!M@IIeU%Gw0p$Zb+BcPLv7{FhiG-i#lV5!&jUt+ys&hxRixPW3=KPJMdVDJY#a#> z)yO!!!AZcX5riA()Xa`l=DwiOL!IIlcx&lHczuXiM#rll#Bc7lC*hn6bAZUp8`{w= zVOUY>PY)aAG+fx%k3FtzW4+K-Z(?HMeq{9lP<{j;`+|Wz)D6^!AKhnI?w4OD9(JLu zPzf3yB_d=Y%Yd=_de71ApRY6w-%i?zrbI)}UcG=}1lXuhF#UcT_K z<0RilKNCNjL~z1ASvNJlj6Ru8Mcg;)PI?HMDo=_Lia8%cv7>nsg@(Rp35}4G70G9r z+btgP(d>UmLeC5^Lk{L7N#e3JW+a~}TGpz*Clp4^if40%ca61KV&o#vQuQgg$zc@1 zZ{*u>8!o=+m%Rm(%j+6t(4tqNV^^VMtk9^6&kO>!Aom{g^2bBb5pVfDY2InJE~$qD z;0C@xb%ll8dt#PhQ41TM@Q~hHsq@fTO><5?DIYWk1z`0+dS%s#(#9OPtyo&>bi0;v z>PE3k)?B$l4OpyDq4|bRQNfROUGz$PD>4Sk8e@>X?xcdPnR#`*0)6YI%~d`Fl4wou zBo>q1#2Jph#KPr)B4M}38#J7h#_L8lsmg5^A|4OfVu4bci$A9|>9=TJKHUI&9jlYt z>eP$uvaugcB;&*7Z9CNygH;umALo5{4V0PgP(3@uZzws;Q)QiUi4x#5tP6cdV{{;1 z*0a{1nJpZFQ#9zGod`W;?~5lh2J^xN%8o+tU=H&Of6O9czgM*5^!XNREk9zZvF4D^tjb zJ&uzOBe?<^@WssYTAH3ZS)VCd=Y7VCow^Xgg_u)!XC^lJ0}2S#7`j>*4`15D!+2k5 zpR@wNj~n$Mp~2%N^;E@eXRvUN3Q27Nwv56{%<430WqQAhTEeW}(PXHZ>U#|YdUN^@ zTR}ePsg6l>s8JfMaSn<&K zX3782bQWw;ec=`dQ9vamL}G^SZWv0sySuwnx&)*fhVGJX3F+=`knZk|d;a%6_amG+ zd-mS%TEDfh0?@BL%Wzs_e2T=Y*Y{36WaX;ox>#buq=K zw)E#h<2peyYZZM7*n_!l$dNw^ z*k{0@C{!|Y#UF=7lR%)IH@b=AmCb%9DS%=2HYB}XWL!zqwrH|YvL|<7>d!7z$wt4t z$~(1Ykj$g8U;bIeHSOsx3g z!i=VQ=4E5qSpeJ)5?2jX=)I!zuj_Y4kV+*rjrgPn@E_X%JE(?Is?2$mn0JYMl3npM zrwp8#vt)O2J`;h2{8(b?tY|4IQ5Y+yNDeoEXZrw3ktu@`Cn_I`qTNpLI(9SiX8$He z7A#mRFs{4zg&bm1#B9fwR__dCS)w^PX4a!~EHzyJL#W{?N6lM1-CU3K4QTcJ9t6y% ztP`lBSCE_?-@LFqz5}1e8GiT=@rsW&bv1gcmC0PcT^0y~yA_Jebm$7kR~uiBHSsfdXCz<;THK>=r`)~d1Pf|+9! z4MI^gPLnS*_`NK7UAOr{8l-aqXCB1ky-uMlw6!`dWs&JBi%P_yNUguPxj@hSu`GtH zAbt8$_rF7}J`qX_-EjoYRPZ%7NPc#k*)Lok^SXjXFYYj4+WHFR*O7(eN{f$}`PYFl zPG6N;>V|yWeTRRW=Juwy$JisCNZ2G-frM!a4njc}uRNK&N%_fy608g?C}L;x zf!t+n-=ht=IS-S|yYJ?4tYNEqP246duqu6Y2+N` zqZE@Ua#H?b@evLQE-@HmsA`VqgBE9b3Y>4%t3;)9vjwjbe9M!!}_teicF zDf)eDu6P0ne5hCaM@5sp^OF{9eShb5`&RL?`cO~35sNgF5N`iq7G}3%lcYMV6}dDjp^VxJPT+f=d7{~C zx>0qiU!YEt!_2yBR$wyfHWPt@N%ch6DWyfJDH=0+P9=y2<>5+Fg7TVf{kRuovAK7h zXtOe3x3LV>invcOR@V9+$?FBqxOIhPt!lji{in|MSrPDW?Kb(JRfoLnYeSPCp#)y> z2;=MKl98&#G$&M%ab9f3DY(0wGm+Gjw4y)#?e4-2nk!T8E?6```{_ zeV>=`WGh~7bUaqNsNbd-7B-f$oaNtTFax+_s%u79o?@7-NWU(%o#o{~X`LwI5z$)p zpv@M7ge{~Mp|%c{(5fP6_vF*VTReu9$*^N)b-Yb5yvB|oUl)cOMKNZUj~ravftio! zuLMLVVRFGE_Z2H%lfIblLD{LU^%hJpqDtRY@3!JR|+x zp#o8G;M9eTvr2CxeX7P~Z1rBeQH0lYmb5t>j}eNNCA44DteO^;B7M8+3cZ1s7PfqN ze?4u#?(IDGyP`?M+Q9Gt zU*Mgf7*_e9%=A-fw&C~XqXK%W2GG2}nBo{vAOnL6ks+cW{?loMyce~oMd7%wS%zfu zHI3{mekkS^9a!8k%Az&a%MsyoL4bPruN@TBdJxZp9V>R7?KAmhOS^V1cIs{Mq=;{f|J?1T?ElHx44;RbRHHu2on#>zaX zGS!}C(w}0|mG29#w4x)#QuHOUXA>_4$-W}X4s5SJ9G13YIRi+5M^ragt>irI;4ZKi zb`G*Ebm6jx@ny*CmG3SDr@kp=+$=Pq4H*wn$Cc#1*PP{-{8;nf@hnP2LXk6tC{vOg zA(v5K3ssn}Q3MPyBt>9eo{(C#;!nvg@s^`fxSeQ^e2m#isRX?70<&!|#p_{W78kRP zx^9s;0!L#jc^{_67Jt@)sx*M-IPrVe$>Yv3mpUb1)-BYGAyxOcEzJ&wwu;0x_ z;)*C3QCvlz#GZ_NUA{@-Gh*N_o&|eBsBM9?*NDSTt*9Vbe+>r%v^OcM`!4pG8%v;u zFZ@id6cl9yo8eEJ>l~uAOPrCAon%yV2TY8Mjw{8BAgKg>x>xP4dNLO+8z;YStJ zt00mRDs7|aGf*>k3U;*To&dJi{u2{ln79JdomeMfAIJYbjUd@eHdHx3 zuBXUyQ0-VAP?b18*8ZA;+AHt=@w3aFZ;NbSdSu0#Yxg6_j#erkF2GT%>4`+-AM%rU z(2@IjI9Dck^lh_UiYj>eRfZ&n%IU8$yarbulbJo%yh>TRJE{~MZ@p@kY=)VDYzHxP zP`e8sYoZhVHp~SbkBaDRP0=g zy+n8;IcL%XtLPlrd3lYO)In?fKdd@KJrvK?ToDoN8>x7tbAOEyKnxGLt5s`AMAi&V zn6>q}7c(XK>h|z8pPX&34}KTf>uRTcUF`~xt7#R@nucMFZZd3V4HV)G4)B!AS#IX~ zVjq*-E#L@AS5Y-f#J|!fIJ?@)1z9Z&C_1H)7hhAG!GA+vPx-vwYKhqI%p;VUk;8^h zav|JD`T0-8zoG8kjNib%^GQ8cQ*(v~y~~re&eYE1-!i8jHvb}Ajzr8*;iSW`OcCc5 zb6hWpV2-#o0ZM&PaOiR26Ssi;kl=KigE^@n`M5h3aGW!xrhPmR$sC$BTa?r_4aX8sw+ z0dI8Jd*aoIr|s5+mgk;y^{u|tZEdt&r^W8Ax1Hy41-@DAy}=u0YDH|q>*QMI-$yLy zpNbqnnQWaC%PVk5zjr%WzK^ArlvnekPl5>?VsCo4{OL#7S?80YQ%+<}w)e<9_gQQF zQwoAQt?%fgH?_dzE?y8d%nAWSz<7^Swsn50iii0X(JWki3#^TAe*0%8iSki}Z{!&L z5AaiPi%R8ET-?!aj)BE&CUQO;_D=~@lU@LV46XU06;W`MS+tvK=$lRC{eRYDS`(Qb z)i#gv?LhF|&2@573OF6$m0Tj7 zL!ZEt%XLL--~&Qd*MXK^@AD`@=er-UKlo@@pEov}*u$>$ahzz?o&CshBaUIN_P0PD z_d={ol|~Lndy`u2Dnr+S`VuBrt$7)GAnivlSg^{Bun!dJecwBaEv%GcNqR#QO<1=<2>Fw_ z0k$8w$+mD54f5c}6CMcT)rE=+3~H;|=r+f)49q)<-$R5_#XpFB8zsi2ms)arlcnyg zPG{pH6>1fVyBD9W&=B1+Cp-{*`V8ae+TepTeAMpWJ2$Q5S`H~_pju>dr1f_WT{Q^i zbHXgJIyr-x*L{!g^=Y`_m4e6N%3I9Sau`HdRy;acf8H)vklTBvHJGc6_9Gs+W4n$q`7c1HRM)E66 z(jS5&W&QAs@M;At$3R$$@l>7SBM^RJ#Dq621-Ha!bF9JWuQ#z|0z9cAV>A59QHl5G zsrE8V!KvE*cgA%6bQ~v~x(L}K>Oyd`WCIdgk(+_wvV6f_#YRR#VuA^09AXc7;&dx2q~{ucU4CCklF?~XDMYxA9_-z22fBs-qbFu| zlr#yUb|CRYf2?TwWJ|;h3xbZ68RXSGpRxjsh7D_GOL&vk+q?fX#$aCkz?XQeM?m56 zWli|gbn{uH&ol?il4+o0(Rb3qY)?6Qs5hYv9PKe$jL5aU)pJ?P%tmtaT{0dS@D=-tp{J`G)qib@LL+$P;xbO# zFUf`9uF~lUNbzcmoU2U+8d7iD`~PaSM+DuB8y$#`=wo$CF%GJ3!~a86F+2LtX9p~! z$;QiJ^)7n*86}BM%EQHw6NX}Tv6Noh7vPOrZDOAc z%$|DT%dUvFTNolMO#Tz?PV$%17uYyqT_PfndvM#0&?~2fy#%SHG&NEoO#fiAmaF{6 zW+!2_1>fzwD|lJx_KtYRP7)LZaL zo%ScZ>Mb)_RSvH&1zS`oK(evNF{~cE6Y&I+l3{Vba8c^f-xEh}wL|W4@kn+P%wwU#9?GDM9?gXB`M1BS*`Ebz*GS- zwRb0L9h+9)Kdn6H7`47CnH_z(Aj_S0yQJq*%cQxf81TbHn^eadK7>t_HAGB}1j{>G zd1E8o^NuvM2Q!M%FYbd%{7-3$!2O5OcCJlgOTq9)fACdXt1e=vx_{^LwnM}Am$ zC(QXDkQbN1H8Ay?8VYWcMAx2yfHVKT*$Kev9d33#ANeSodORTN^b!u+~hE1!Np zJH-^{m6xNF5?Uwig$fiZeDB^>!wscEN}|B z9y5M9dwl|)W z*pAK1oEZE7n8Gb?A}||xl&eUL0Zb;${{0X>29o&k?=h1zdRb|-aGq6XT;`(`0=RygY^2}SVO9{<)ZUOH*!qE@Q<^Bb0T13 zcq;$4X1Ev^vwTCx>6KNsNf}kK_@}UA9dYx3c)B6nvAm+Dpu)8OX8~^8B0YP4bu*#Y zU@&V!Hzf{Poy_j0LpdA(kk2EL0GDFdHbIp ze;t1+R!`B=uQ;NhBADKMVX6X2r%ck^h7uchumIE>T^@##^7h#V@h8n1~=?5@bKcGGp}vxswqVbzP<9i@z4|%f;RQm@Cf58VXo{FcztrZ z^Z9hOZ_spOaC7Fg2bHvUlGJd5!^rW)6SYmn$ViSP{~5377=PKCF9iO4ZZ2tyr{4YP zhvaxFsw^FIKA>)H;Vsy`ee}=*@V^^|JYUu#F;i|MN!z6qvC4W=y^A~62{~%t9}%cj zi`M1ZGM}YS42ti4bp@OKWL3|W8$)ulB7s4btI_EjaZ_Y)gR;tG)tr6DO$Eo$5C-Xsm!8JNO>xJ!JjdJ8|l1H<3H4n3S6eFD`Y-oJ14N7c$Dj9v&2e z{(V7dTZyb>;EKzVE0Cm*q4&IiE2bYUHQp8NPHgV2_sM!cd#je4Rv@akLtj+4&1jUc zI{#nj{kCwyPN3WC*_^O@__2L#toDOjB&Ku?X05{&$j?d4^*U|8{r<~Ksjq(k#T#a8 zd6MKdg6OW<;YYV8?tLS^g*+uSUxAWouog{f93I6VM^)5vIeZs}^ocxaxrP^=>jodR zE8w~7_?Tl-Hc*@>lfU3y58vV5^bhLFx$9m@|6Cy{W>GC;-b?3I5gUBw#9W|t9A`-Y z-kKK)eK!15jJAO=?sCjfh;m^ye4RvaG+wqva(VnG;vi#lcW*|v{GpJ> z!pEB6f-ZrG*{)zofjT%c3Su`bzfkQmoV-wUEq4AJy7i!0q#=hMuf&XKF8Tt>E5y zm3-#rWXNB)l1mo=mXtCfWtCc&CL+~*ruTVvZtGyF4av7}K<9SL$dn7OI7J}9r7{Bt3YqgU;(_DwS z7i>I8&~PA3Eto<-jPF2fGAMJmcF*;AN1x-mJrTUyyPdEy^6bCn0WaeP=XvtxNj|j9 z${o8TMCO}yi9}J}0!81R8sNPX7&uiV*TIXyZEpAKU_yP4O9OH^kNajbeYuGFOt?KgKvo>nunmk!Gjnc(jw@X%85-Jg|`*H!S(x`I~Xx<5t|@?1=yA?H$a@+N0Qg=uLaH zYjt@2oSaN==0V$q%k|ldw19J8(0>|hqWB(9)(lY^G&POqs%~{q@ePZ{t{bmCyosh= zkvLK{n_sBxJH5&MZ&fa8sej^*kU!&&zo_Jn9%7_U>yb=sC;VZJIBs^&8mp0Xe>MkG)s6%zLBot`5;8>K6v^3!|cg#GH@r7mc>1ybu5 z3rTeniwZ*>GY&B3U6|U$TxP3LMqKY;>P!U%;W6)H(X<@3KO$cb89t$kOmeaHon#Pp0~Bq@1XkFjAkT3%CLe#Y`6ewB4?P) z?r!76dUf`}p?iCY-hR#?a9In*q$Rc|e*I<$Rn-pH!}KA)#dyrN{6P$HztKlelGrRH zs*7X&UQbzaM2Qf`M7M2zr5Xgy!9-QQ&ZiH?bE4tOca!og!tXz6o}Z}#flBso(>{Y= z%|qEb)DBK^j;cbUz1R=E^5;~XnWCrfAuY?V^!PR}B8qzOB3VLAv@L##%|xUw|U<+*u3Q?fJN0;^ZUnm1C{ z6aUGsuG+1@#rhJ6@qcLAuEEojh)BUl9&G4ft z7g{%}nZO|OBjHv`lFk{WE-rKesH){cq*z460h3bA_&*R+-ld(RqiTeaq?I1DGW#ir zA#?F^;FC^Bx|e14ruV%jtm0FRDRMwH)%B{)tFq*iW~@; zxu)NFWY%DohT-a!fs`{{b6)8*=V{8r7c5;T+_!u?rxM>h^nX|WiDor_ahN+-aDSch zt$*p%%Y$?l^XfCD4SR=6q~oZVNg$_{BQ2sx5#}9>|Cn9W1yXJ3zq89Sx|!;Y`(d4~ zxv!+ZZFqStD;2QFdME(<`+Wz%{||g~;;qF!!Q@OhuH2;7Xp$)iEc3l8O=; zt|udH;}b0#o4DV5d>$3sT+9xdV+pn@d~tW4==iIZ#aH(cHB;znHQ2tyrOAyqR;{#0 zCj9lX?0GIcy8311&A-W0ZX^4~fkmlk)Q^}VWt?P?tcFLc?_=|GM1o`xbq@vBeRv}4 zzsboB6Pq120Fr9G9S9c2hX^IkbM^(x z>#-AXqrY23MeiB>rv#jLi4o;JN{x6nL&zdvMW3v=i`Y-{+ApC+fl4)yDf?mP%jw1p zuanw!N{hQFpFauvf{xQvvE>Qk4PIjuGEfhwKHGWxG%;C3y#^0%PHr42PRgH0l(LT( zmv-B8*;?DN!+);)I`E;AmN8J~0#Qw4d193T!nv4{3k-Xu;AzQdV;hP1&H?$0jH;^4kXmxInIRpENLb{8a#@4Bb4FTuAXfTkaMf#5?u0bZx_zV&zWeKMV1!JPWH(t zWH@s6iM;0&n3q&6)7%G{3;CAEg~q&pVjL}I>W?F+QvK!cvs-}SJTbMAhCWc$W+#d4 zaqKf?#@koiwF`jv_8oMaMjvDiWi?BNl3M0x_I(hNjv&?mzz3XK|89?Z&wS5U-a^mT z%(uKz`TUjc=(B^t=^i%7G#uY9PQ$hCJ7>l2hbahh14N!@gTD6+1sv@gqMbc0jT@Zp z%Hd0SW^hSQb1^7AH8jQuRA*}WBCpxOW#l2Dt|4Hh)LHwpc`2h$suwQB z3+rz^%f#8m-I7Ayyj$cv61Dm757Em%KFB!}0FetCtq3QNzO5Bg_4PxveKj)$}?9>;r>VuL+#q%=euasIv` zuDvmmP_!#hnkCxgN?7O&VV27YoM-9H{JE^>w`IF$JX+m{MA#aPPq=}PVytlqiidqC zL%_<^P#A5CaT*Cgsn(Y&!PO7Jg!Ba`;Ml%KfFz75<|lqlJr?J}<^5O@LjT)6aP%uR zrQRn<0;;6)@HZ8cEf$romoHkkcy&qf*T;Vd_=}stIFN_xdda&NSb8^!WhR(d>(X>d zXyfGLhO8zfP469jH2WQ+9wI@$bYOI>*dr|;BvY)z9UA--v2LDLE6%dpqP2*fbT7v4 zF{-$x-`Vc3IsR&88Uf5Xtn)-a=v8&Hb`owv0yB5LCymC)F-=#Dki0k366NAXZL1S2 zr5fjlyr~M$WOHEu$=74BQ{o>Bfm7=Y{9UQQ=;bDbd$1$j&}+MP=+i{K<{so*Vakq= zd4EDInC_8iSZKQslvtZ#MX4ysk}bwjr<6djgh7P}zB-fr@s-NN9+YYipHu{W;b!jW1e2&E9 zj0j0WvUmOVkf`gKQJ&8fjQ=r##ZT7aT9aX_<~`6+*WCPG!%>+4j&)W#{Ew@l<0|vI zeqysmfd7NMiG`8dBE|UQ#ZB=?z5g=8ia#_~OOu^lGR)k=alRZnAd-m6`tAiKP_9XU%Cs8P&pi0gGJ~>yobbDw3>%qAOgqjaT2x z1C_X9pPPYO`0lx#e);d%sM&=p0l#W@B~?a0aS;@zm`|$32qm|k2W6A4t`Xn18T*W* zEx-~#M?F*P*epktrt;mI-r?Pd6_{oyf&Q&z>w-0Il97>LA}8*qtRKl$tL@K-?Qg~z zgHe!L5=W zBT;>lU@GJI>uk%=;Cj1fFUDX`v9QG_8I?Fc^TUDluqQjsj`AFw}r z;n>FDXd08x^`J7dDriElYKJOPvNJ+KGfbIfV!crNH6-Pi(}6Hvc3ypT0#c z8T&ha{PZHrwXFGmP-+Q8Wd+HnADHKWgl|XKZAiyiCRz*5dFbRTQ9WrZ!XI7^SlSYV z0j_J12r+8)Stf3*EI5wxtSB*bLek*DrRVV2$fJ51t49w&0WN|U|qvwB(}syc-q=l_99mk%ylwHMn)E+HBh7Y4p2oy@S9kQ1iSGL z_C7er4WXCcU|@hJLxcen0VUbUE4z)f$xca_ysVAJ>dzSZZB1HQ(SdcIXBWx(+(@$m zmrwfdEA&nv@cq`ul8GQ_{L_rH945Su26u5& zRF_(hHSv_Vtb$eIb$e;Q;}KlJzQ7{ICdoMKxb0sFcy3(c?t^oH35fm?$0Da4k2#`N zUe*MXY_&j%!bq2|N&q112OL9&F4lfWxc9eO^M z@_CeEN;fO@VN+7Xi@x8&I>~3emZv!ClCglk*n~bcf(zAFofv~ijE9s%rl29=;1k+U zi%871UZvjg(uD{V-MLJ&u)|qOcr~s|{&7O~FKTHwWOoT71A<2@idft2ct0|~F*xKo z=}hph1|PMD{*#mOk9=#3F%;E~ z=~kq!Ic013K4%@qWg|Mv{Oyw>W{H08*$|6;ygyWAi`R5UEO(I*q8}HYWv_slxw9co z7oFGMIlRzpo=R3!7c3zh1Udn5#jm5Jl*zcC$@7(2&gmzUDk9{xu?P=uKz?i*bPs%I9#9`X zVfG9yZ@ZHQd1jYQs>3I#ahyAH_umtVVW{5{UY|AX$=gE3UjN0!z{;aI_ubZpe~086 z?={j!#dL*Yeb~I&$O%XU@J{v?N4RTp`pANO8|8elk{gB86(*d(8oZ_EvBC9x{bb5~ z_M?FGm-K-WwvcE&0+D>9mmg5#7|gcy_)(?Pa`u?7oU}VR{F2uCxmZ+kP4kcfD+r5j z+4$5uj_L^8uz|q9yB}G zKubBpxp2iX1*!r!DAzC*0^@6%q1_9V<6IfY9R}+>vvD+ULa74`CMi6i9++r z#M~CnE@mc|wm5BRNc;RKV-CVtjFz9^++gIGUH`AUg^M2}#LSu2CBGS{hC3dhW`N7_ zuJ}kf;KP05ml!4*n9N!;lB)AOVJ3N}FkCT+o;J}Tth*aeqn07nWkQ}vA~H+0lP*Cg zGV>)qO%`pyKEVNi(A3)@bL|q|;yl3y3e?ik2jM zni!lhTPXK!I$fENnFIvdeKs%hB_0n;-*amAaBnrXM)lr1yc_q-lJ(yvABDmau^b#X zoqi6CIrO2ZN~WIF)RD2jbuqc4CmLBBQS0SIpq9t0|FD^XXX?El^6Y3|Gg*N>*VVa# zp9>Rmz%)_QZtOmcA8_fgnCsWNCxlrC^TeK55B#np<(^SWn6m4ytqP1`t#OenN9SK` zCqbasG$e?cB&pwK?foeIm85t&C&Al3e;xYAHI=i22PJBNoML$jHhPp^G7+BwulIx{ z7B5_R*xTZS=|Kl?AJzEF1BkA8M)ZeJ?RRM4EeK`gu84AwE}KQ!_q_3E+W%t4Vqs~sMoH#H#i!ER+im-)RhX2tDsf{;72av+iu{0x72W|FVcbx!;1i3; zhUHA{Nf{p}3_v(I&B+keIGl($k3YwHR2IPD|7|a}^jhM*mR`9w0KJMy;`wPaKn=;z zd=BJBe=_XY*I=+^I^sH`Ub#g8^^F(@oa(=o&iD#Nd!D2OW$)^&Rqfn(AOm31=3kNi zA$9`-+e4wx>%?Y!WHm!z$g28iC3nHvSe)8;qA0n%MfS+$xi}+3ZJW={QglAC zu~%!BNJO*A2d76G-2EoK=oTlDn#%j9cZz*8$(Z4VLZv1{I?a^gvimgo9oth74UfD3?QHfm!^o9e!xQfUiBw?R1Kj46m9tDX!O>{Hf(HtU+ zQg%6}9*Uiar#`8!`=cxPi>Bw_Fvm^KFEkcgV=bnoB?Bt)PEz@P7`bfLm|@ODMa0`~ zD5V|bUs-l)BNxr^KSZ;Q2J_Ejl91tcF7_QL=JEK#Ujti!{ntvFlhus&A8?*#bI}Dl zxe+WTE_E6l+Swh{vGvR>*P4%&^*)eCbIct}mz&HC=gn@oL?D6a*`9t3Vwj=hD8C^= z!eeum&g$ImQ{>u8_uIW?2_%*WECLfk;#72G87kDA_JTy6i#TcBC7igNIgpG^`!*7K zqHL`-XvZ^EKXJ<;xa*VQM0-}(Ru^Q0j5c+eFUrqfWeUp!MP;Yt;Kq0ug>esU{38*o z61j=FdDuy{@TYrSFaVamQh<2w5#V;zydo!4Now>%4bAg8DVBxKwgAn=R3p=w zJ1HtgAjiJ(O@&ytx>KjaJJeC-l0!oCb(7c@-7G{nSC5hbd9BG&>MLDPd=OUSWTsli zI3qhgp^HwO;6haw)B8;S4Qf*mGY!9I*tj%=$9(PRrjB$5GPZS+Gt~8~cV+gw*xRa7 z!J5Xn(YIl^TA6OR&Ocf%v*Kf3PM zdSlvo{2}C)gm+QOj$7US<(KOgh-F+rjS~n1A+h#EzrbF#!M>2tA#(@EJ3-v3`C&p0 zM<8l&IBr^cJW^~YtNm4y4#+mo9Tjyc4oRlmy)5Rb%YY^^|&|P#U#-f7MiXVa?XxizARlBR+z~I=3Khh|A|earG`;o1T6SFVJ?va1$dxuS{>02T@)LM8SgC%nO?W zt&#eLOtX(1o-U8XcNYa)rjBAtW^|Nh;UkyDH-~o0o!X)MvbaiZ3asA%^WVntb{dEY zF+NI#N!U$Mcgm#wNivRWLpODoZB-$~WWP|RwRNZk_rewBbzVOp#VmC7(5E5d7>5|W zf|r@{1L%qtFD@ur((tJ|4@uk2mKLh<4!8Cq14J;$IyX`b3I@sB!{Sc!8L9IW0MZ5$13#q%r)N<+sp zJRW3wA=cBl?4MHp0iH^kNCm%0+#m#b7o;dK9zZr73^Ih=ZR^AzpGTzD@5&l+|LyP5 zv}^KjoWYSxKQFv4aQJ9sMKH+R0FZyjm=veQLc@cv5hPU}aUs$1U?87w^p0!Bv6d%C zs4LRgO`qcXF)bOz0jrgXHIUaOcE40>!DvX?x$nx~v}U2Ca2&(Y!tR zvCxB^R)xpUp4?k>7Aj-nE4-t*#tTb6*>xv0I-D=^q#vK_mm-yMk)z)E0*Cm_hQ!ze zj|b3MR?uvzTQq_!Uo7A>q^z)geFSU`6;dTIgS4ovKzaM{0-L9_*A^;%pz1V_KR2eL zuIdnT{PZ?9Lvz@H6u3_Urw|C9{rGh(R$=W*uQ-#Izh!YFeJ;-Cx@(g4wfw@sqomHU zI>yj6gwcFWYuH$)D=pEc&ZROWU)>v5l{8E5FJ|0;w=LDA1)n%m(KYi-Ox*J|z5#!Nvk0A3&8 zSbMZhhA}Sw_F}8PxQH3Lwt~)h$dQAOPNx;ewGwwnp+3f@!@}rQiPTUlcS9(e`iIX`I3KTed4@kywRNbAH$r4H6H>jhVGYpiDc z-8J041j4tN0>kP2+G7yf@!14gIrB&>Cv9_?e$$kum22x5!v9)2nP9)`Yj@HS0JV%p z>C~@TJtTlpX8L2mV)shEF$_nf)dZpyHkHVeTw0=A(3N_pcnH@aAdwNmY@n$p_xo55 z-Fp}UU6X)@oL|Nz9PI?agX0B4wiPgW63`?Z>}iWsZ>`O}adT?O7LNoy`EQ$h5QWUw zFMxu_udwfG_GY3Jn_Y3`R(m^Jv!dd2ap4;^FE7*7MG!s*(XGEk=b$&FvrzA(6d{Gp z4W*u~Gpkl%B;2_IMfgCJi0nT*TnUwgIlbkWpGLwWTMs)Q1*=f;s|X&mS%yW49556^HdK*X$F$i35LnEVjF?=F61YCSw7g`Tp{c<$aQM$6)(WdPyBllsCtl3Kq*mZ_q~*Fv z)U|&a={!Q5dRbOFG^?)T){dG!w+HY}R^~x=m)^KR{|y05v9Rvgz^)?{O=VnOpj!q^E^Z(`!ZpV0>xijFY`3}1w}I}Ve}jY#-W3x!$MuV5G*pXW>!#(E}a!AQ2U;%R3Au}xUXETlzKHT z6-s$O*(q3;&rC6ltu%s!?2S^aiZ)qm$_YWAoBr~`mBaQeo<8X)FW^H<5GvNV_U9fI zjqn^6c2g~--TH~gt=bXF@Ofdi(R``P;im?myMBXn!zQeaR&!A*IX+?&Z) z`inMhd$LJbg(eR{x9%5y%acch5S`q}FDjZR2*@62M$y&0Cmc9{dKNj{l;;N0nLw4` zyxFi7*eHc=!#*Qjj7txy&$XHw&NIAuf8Z8b{70@j+otJm$ENTTXu?jY%-Cj`-{6cj zrXV{sEIyDgMCkBKryP&67qdtO`IK4@F_-Y9lGgIfBuxm<`Je|LV^ASrtwfe>68n>6hE@#6qyBPedr z(9f9XWl~=Ujf=+$Asb#g7rt_6A7Zj_4ijUKG`ae{#Iu|QAIvPs+~!(8LiY2>xh`>= z&7nh1PLkvKtqJJUJcpTE6H%JBsmB>f&oco{pWi^OX<4|-Yw?#0nCTlZR$&eU zF{unh*>eJS zF=|m`Ru>i@LLhjDNtZci9*%{x+;>3y*q5O$qc9Z46USo0HuPU!W)&m)uo>(KqeoM^iWE zgI~Y$)29}W^OU6JIU)4!JK89%WN0>gQz3RwW-S-u?e@v*Fh?%lOyEJSoi{w)_^Mu`FjScjaWWO3F_t zAW_Ey{_R|3zR4!8i{jiv;UoH)4m-4;^3yMkdtID|2;Xe7>w)r1AhGwGC1Z6kW0aR5T=*w&xEt4f6|!B@4?IXQ-Mcm3VQiz5apdiKXo* zyB2iYwR>!-n-2f)ULD#Q=2LE18&X1R)UZt$+t&EB_9SVOudvEp z9ubAHsBOZ2N@(?zHhjCs)sT+8OhB%z&8i2iw-bFxd_T)xYik&SU;PU6BwdvWTdxcV%LdUK_b)B^@+Ju zS=gefuOdPU%qc$hk!CDuSHC-JNJAp6uFZ@VVzQ`X8FEDlDq53nK`Kl!AgZ zA|c%^ASwz2gD`X>-92=OD2ND144q0h3=G{dba!_R-OYdCd;S|PxR`U!UiGfn@A{Zb z@hUdpTEr()bxp$LFm0T`OYh>wb95^2;+g8l;3>u%IB;|7E~N--ey3ytC(vfGUcgW? zenu&@^<{B_gUK^Os=Ub=7IIb^S@45LEM#C2z#Vv7rs6-NZbfg!!dhegu}VanO1?gn z)F|$c%kw~lsG?I=d7Do`o{g7Ei{IW-uY$R8&dUuLtW`J2pHhkIv8H(nx-7U9nI55j z^HdPW(I7Eh4B)d@cn)_*pig55P{JJe3S0M+&!pEdb6PJ9Y;we5DB3yLyct}6!LaaB znL)f5!bG>7TGo9^z#RRm@q-Fl2 zjCNj1XF>*WJ=QlosA4zXrfDrQYGFXPOLnZn4xKY|u?c5rP$^A?*{(5hJg;2l5oY=y z4`JC~xVPyWlt@K#Y;Z$kIy$e)|UyKwHFCcQC@NN2Z8<`mV5ydc@gKf&f((7+A(QG73}|x zOK%%>J)o}As}@i}fgkVmYcP1k=dg?gclrYuhDlTjQ{-XRclME@o^O6Mj%a*=efW7& z@$|+=Jqt0<(u5uyL-_{Sy%~Q<=8v&KX^AEhvOLGF8t9#7`2T9!(|B62yBkBFH;cuQ+ASH9$KaN* z$VP*mb_C5c;MIy!cZ__``v+)lo2o?xYV^HkaD*Ln8{HD*6LY-#%RFq3PG6(PY@87V z8v$tQR5#+bV1M~V=57S4P`6er1v=sKnxWG-9Z8I`EM7zcZ$9o+CcbFRqq_^;YvZq& zP;?C99j`NA%su0O^@)0)5ot%9%YaPy>sLc5l_QouJWZ4NAQfP%o8)sq8d2J6B8B15 z<-^`Ue@lb{T#ux22pnL z?&S$T6?8T+hg(oDJ#*Bq+Pq2na4X-*#w<(>4803Ykg@P}-Gs)+nVbE^bP#IwhL!Y^wRcn)X-P{!Gkb*%nhv&Zm?d0Fyo34z=qnQ-SZ{Nw+i;5}VeY(arYL`qQdjCsXqzxNV zy9iSA-022FYiiB%Svbb%+xj%44C950C9*i^Kj8$Di3o`nx|>0HDYekjr)igg{^D?|E5Z0gKBU9OYOar8PTn54UV^jHbu;=;uJMjn1GO zn=k@GmTcNYG3F+W%PFxO-ZiZ#FA3`V3^J!Ne#X|MKhZ^r{#nt+vLMJm$2c%#Gp648 zAEcguDTNs&i8b(=tLXO(gwd2ZH{F3>7Vq0=8+DXLAD7BL6R=*%E=u{etjve;I}HZd za;fsz;uvZ>sU&+Ql1;uvT(}MOo3>D;kJEmldq-$bP!5nyZ8GC*se`PV|AzWcijuPG zy!tC3w-I+-%ikC~?56N_rtn|b=bfy~pRE16=XrG0^y+Ujm5IuOhg!w9^;2C5N&R9N z70xFC?yZ#!w+vf=AgpY`cM1lSD|=BM%>^ViTz3)G5cU*hfC?T36P~f zE{>&;oM$k2ffB6K^_v2!=jFe@$sTQDh0cP~=0Z zXkk(0^JP5#eond(O){-t4G;7%Qmsf`7EuI`R1)(efE8^Y=#XTvUZ};12!H=uqpz&6 z5eCjgOQp?s>uw6FbW%5>3}0z?ar!QkhFZsu0C@ z6v`ETum?~&w+7h_m5E!fUnPhq0|So;I30`K6_)kiPk4m$eER53?L}fNM!ud4a%uR2 z70iRhI7zI-7ZV&wIs{lspQP0NjT|k@7JY@*wnox!+Cgn#KbAJMtGxiM?YKG8k z&ETe&pp7p3-&o6hGCWAx$#)YR^R=@tbwy-S9yFIaEB(L`@})sqB|bA2niM6pOFh!r zpw{$q;Cb-{hg~{wBT`N|@dJ`5>LrtQ0X8*d{{=K}4WOsQ4E%Rc(|?#n=lYb@KB6UH zl#3J78MrsdBpTM-q|{FENqm1HM*|q^Oc-ITvLMqU?P+?zgj9@wN8=E}Z>I5gpJ;$Bt#o@vrpCaVFuL?H=QC5VRvLJMIN+*JWJ$bb?Z*6~KH z(V~EKh(MUT0i<(kGctY^yjQbpJkEx`TUi+zwCS>K^D1=2o55ak?DAdL+I7|TnDbDy38v&hVkt(X~DXi{}#PXm_}YWwMjlMCDMq6y}N``=GFhUL0LHUw676|$5RXLFMJP3a+Egy zBWvJcHv}|ImZZ5F2m)Tw(6netITj0OM?@EQQ8X{XTJ7+DlrpW|ooKhc3&S7w$Wcp;<-6i7yGh09&Zd9)<4nQP!y+)= z$w}$q=XVeN2swbwmtm>x%rwhA#|2OL;{{%i;ABF<8rgm))|7d3?+T9xQt-)Ik5w5w zM{fq65y(Jv;Eh+csa`Lp3P0?0_{%tacIJkn<^K0_a*2LVcIq1i8x03|;R*x+l=Th%*Xu?vGl8$l($O`vOuo43jk~VQc>1pi0PZlO zrhjHWOq01w(tc$Kw#B+%U{c0Adji&!1pn!_1`RoOPA4dI$jqc$a^o$fK$uBp{s9(3 z-%Q6dE!jiXko^=5(jT8DXZ;0d642pdw(JhD0fqgnw7h*d!|M}i^x23kU*MGW!f|*0zOCm*=t(^^hYfRw1JGp3V-Dz z-}9t>kXYX0tp8nb``#l@E5@Gs=DrKMM6?RFs`Q4oF#_Yw3jGgM6O42jQST=kzcK4^ zBzo4hId)uUb&WX}{}!#tBXSYHObiowR3ID8!X`Dn>$0XVq5%N|^-Lb={)#`tR}3cENb!Nh zBzuA}2(3RUl7N{>KfCdv`;JQcb=Fqs7HqMk>&>4gY#o6@FOo~P+$Fo@rhCi!xBPho&jlCf(2 z3pH@DpU0tQ>_>^Tlg{K}_{o9!bcpw-{3|(cRTy>5@J)rbL;cU|!HaZSB)Py{8U4;Z z$q7OTYF+j|*zXN3AT!%!v71>^Uz!?dhZlvMHc)Zp#wNdI_;r*3)q_X6;ZK3yi(H8!xKp_4+29CuofCwf7SGm>x{rkjyqg%Jm$$J-M@(;fZcHVqioyz8 z{~9E1XT+C)v!NNQZ@_drmlr#M^*+v}_MJZ)qW7rG6=gZ$BG&lyp3c~4XvG+4wNHbR zs@VEUOGE{AZ*3e#<$EEndaN%Se#>evLQ1Wzv3RkoLpgQgjmiF^t7uhDW~QyYsyX)0DE4UVNCxF7Sm)I-olIoEi+ypDn2v?kLeS9Ne!YH(#uHULD5a{)AOrKCx*$sDy-J(fbpHWES#)N3=sa<680AC6lwxVRB#Nb@GS_^vcD5 zZkmg%JBN1%!u_1dG?LKzCh}*zK%bL zv4n_Dc8O$l+Z5BxV^XQPJGYT5lf%83Kg(;MrL-&(Jk0i^>Rd*cK z&2`-3>%n7q3Y#YYW7a4QPoMJ;!$gL_;_U2{20-VT}&kh`xg<43-&v5UJp6l+VmAold3@{c%Lwh2M){hJ4VcQdXLNa~vyGPy6 z=sao$x!XNyl{ly`N;8RY4Qs2)oPhb~wJ8ACI% zEL$GxyHZHxD;1XZ5MkNs4cSXW?^{!HiK~HFF`)7GT!u-^0&1*deVG{No_@ji)2#Ew z1(>y3JN!S%X)BcfvzC~(ul#7GqCH{kRT<U;h`IpazA69GY(D*5*a81Q42Y)*uXQF&8thqiJTJE-Go~?%f9fliEJ1jE>PI?j4HwBa{L^XIntUFqi2NJ!ZQ(;iwXRHP z-0cqd7%Ky`=+F$B(oS~JhnJo0zC(Mvg_hvqIbWQb*hv|a;1NyAW{CeTHpJG1$|CZa zD44;XWV(gH8ooZ~YvcOfPlTylJ8(+v7}b_i)U{*3Bc9%vjCq?5hxQKIxI^r1ldBV+ z!;$`-2rseGSBn&vE~QUf%J2!m#~b7JpN*lQI${Vi#U`S;er-0(P{pGxHe?!)3JuV8 zv;^VGnc0fMcJ#jK>+3nx1@-C!iZ*lKn}Sh4=|H&J4RjT&qfOohBiJ$kR$y7aeinK1 z)9LADahA{{2jg?){_PD5!9;g_uL*GZQ?#`(ZB^4*!N|DPqcaH4C7ZiFGi<8TMQm={ zvHR!U;~FX%00Av;eb~l6D7IOTOHx6Xw9T*1sRado+EO)<*tC~PAe`Mquc+&Qow+JT z@F;REcEezYBQSIgL&WaqSZI%j_GHQ5>pfle&DS-__RhWas&mOkk~h-LcD1{1#mHlg zLZ4nh8m()~)?U*MeP^wkbZXC&`xtTnm)Z2&`1c+qJ|J5pc3weMcDiubQlX zw8PiO*+e7LE>;oHvOdNO2a(-}rpW0Km)_pm9ntg_T`x)Gm*!^iDKxN0rZ zt7dK)sQl+$-;1WMnJP_XuJ%iBgbUHwou^omT#MQXj5kb5Q5mnrj=QOh!MU;(5{wWb zF8YTNtI<%d0bmp61Q_Y-tu*&bLVPx%+1=8U*&t+?5$PM%eCo@o1?L$ZwwDn5j14p~lqqp|+ud%$FSL>IyE zepzqQjlPuUh85ea0J%B>M^i;xbBig%wj__{iHeJ)5 zslLpcq^O_iM5xKCtpvgtt4~v=#x<_RCBOk!RJk!ayM3a#=sT+4e<_Z! zb-0oEC>m76Kyd|vygzaeo0skoopQ#G@%CnP)^K~yf%1-g*z&fd!G%UZTN zTk?L%$v1YyXN2##N_wYQXfZ#PX(_dgFHxCdk$t8Mi(Sl56_|`js1g z{<61In5$c=QDFy-Qr2g07e(j}PDT7BtTwNom6=+lBmG#+^*duCFIfc?qohJZ9t)TARY^{r5Og#HY<-g>?&%$++8nSoirKP%3`X{n}p%ItvuY(Fwwes?wl{q?P9C3T)L)tw77IZ0!4S<)9QquAlhj9aP= z^TqsDr}2j5S)F}Nf%X>Lc1HCPA((Dmwnx~30LrlZP(D$;N{+w^-Zo1p8mjc_p+l@L zX#CEO9Kjqyy_o&XS^$DLQWvC8?Cvi{p{B z%LK59mT7pOl#*T?DZTf)N%A72H+wSY4v1zZX5&(?#4T*obl-W1S=QOM{psn(^5~a7 zv3^&&!=S3)scF6cJyu9?7;`c^d!Fm^l!>%GZMNDgf8DV5Df)|7v(UN>)2dWtcqbSK z^qSeXqsRH6aWO=|0{(`ibmcdmC)}0NY>gawZR3)NF$EZA- za*ZbGg}moR=;%3m%&XTr7=SO;madtXF{bm3*df*-l+@Jz7`Zt%K9oaHIz3(!k2~7S z(?wj3JbwPLzWUggej|WzR_0e+71gwm*@+jocze0d^mG*?js*y$4`Ry~EYo52WIJ5I z{6)%%5ezp#;*$I9tXvp!CC9KQ*Pg4ZWhRVd^O6^z3P(5Q5+4?eeqqx^VKI$~xya1v zP5n+a4&D-$)#kr5R=b@nODE1)Eh0jSGMZ6W(^?ygin&S~C2C&Qg`rm8o^KJ;^Yv9| zi^R22+g^W{t?SG##gV&nneP0Ls(@vzv3&8o;Kjb}HJY8x%};{i8i_J`pQd(ya$p!L zt<+X)5{j8X{UT5T@)^d0+foS{6{o^XICl!U_VFz8Dkg+)SSjBe{8>1Lm+?8D#E2$? z8A4~;m`cvN!JOko7rsb0X3!0#tg+h`|0paym{;$z4x2Gab2n6th1UpwX>i$#z+U1< z-^0=l=TXdkD0*s-T&{TM7^ua+Z?hXNjd2&mv<;Fhww1}kFlFn2(YB`!Hdd89Y1#^7 z{OhdvEt~7Sk8C`Wt5PQRI3N_0IMo$iN^0ms^l6(sLB)IAg5^Zmp&PNH&tVe`%xaT`r(Vy5>Yy*$nP zLw<6ajcOoFuLYf9j48~q_E#~@M&#E`qHaFScf$KarcV+&ozg(k4!cr`&52aJ9yjAs zn;Fy$V03-wEpz^#2o3@(9rR1>l*Csz(<=fL7q4GkZ}lJn>>7r+y@@xynO*)+%-NiO zNr_yk1y58)Ud9`@PdyF`|D6ge0z+(Y(SA~W#qBCQ9s&I^k;qUx)(FFh!c-GD^(A96 z^%Im_57MSpC*p&hgt`fqi(m^%thEFMBrIu?R&NZ72ZVne2W_^IKNBumdH6A~Kadya zMgTfdn6O^Fw#DM^eMveOMeVKiue?r!ZSdj=4;t_=v>1^n%zZ~Dh-CCN$|XMYdBw1C z;KieFC=>FT*($YXuj5;tw(LxiOg3)|96|9$)DbkAF%)cCi?7F|&SK8Oa%#n_*AkXH z$I>Ro!O58mRZ8c+b;pP{LLWXg%Y*dtS@2+QaU{Rb)G4CHH=hGFIO=N+S^7`fyid_7C0nLNRb&CU%jO+H3SjLT*kh^PCWEn z3`JI8;X^c`TRvl_MXM0b>Ta(P2qOWM0v}xK$n2nz=n96eI5VVyzAW{5^T(B(ueQH2>99E8`VoQhpq$BGu`#^X&edgn>xLkC7Xz9FMLl9xRR*_ znyKtG&HP;Sw8CV8MpBiPI^~{53MsZH)KPx1p^8&$-+UY#{rGXcv+dsnKNKca07@yj z+Lc7HW`IkEadUs}HeI*!I3uahEAp9ec6UHM*v7DVG`fqx!#;x8{lx3&om!KKXY!P9 z6zgG2U5?)2k9(P{qcFt8`5*F>JEHtDDDC?rEa^^eIW~%j-i!(hj9@lqf1P{yp`&-d zgy0=oQddE~r3*{Q7yAgW->~J{oX_y4Oe3nwsTI&PW2~b=^pd~5=;)!oU?LIzTG+yM za<9(0rkeODMMqNcw1;Rm25=?WY|R7oA(Qfzz5ajhuL>{YNHUJTHskZVCy6MGz+Ky# zziTjZR|(!$?09DEG~hcgIRISb@(TlpU9lC5N~F=&KPq zwXqNR_It}QmAJGZYt@`(lora&<_7{D`)j=vzh2CKc-fm08S{MH8lsUnyf$jfxZK&l z*{OnZFN0|C?f!JhO7A=!QOC$P(-WP{!uwnhe;!GmuqCKp^(d@rR=D1|t1Z?rt+EDe zIZAQ|-&VyKX6y18cToGj z;e>Yj&M|>a{?6~BOr*FS3H<)>sg8z2WO62M=PyazH#Y@*Y4i> zbUr!-%j-2xvyoSe&y&@rHlHKb9dG5Zg_4=wdDm_#9T2d0FLbqFv^jLu-}x8%X2#5R zWVL#NRJgqJs@Dlplvbk>S*OYZm$4EYadu)~XPtP?F%x)vx?8f8Y>B9^g>p`W9>x}^ zl8@ojWR*Tu)A=E_b5)|jt;;tl?3l9hM0+aki@4i+8bR-!O}3h;nCEY9F6-rh@`h=* zz-u`pcZ%cGj~)15&n~o?^KLya`)cR*ehR)@#48c|JdH70>K4vg2(b2)GyiC{AqPT_ zojK-j!h(41e7WCe!kmbpL`$whEx4GT4fTfzyMu4-xKwlH;V8~MNhz6>$W3s={`Qr( z{G?n_9}w+YJn1sB-D)f~)O%x6=u;5lh4Jo@SCo@$1-ZF8iTKcjk$qGdUchmIG*fI= zBu>DtVNhWotZ7yJ6seiT9#ppbef*#<4Vw#E$KPZ74oF-LusQO-`f7NY|7QJ$?2Ns# zaU46yP)92;lZbTNHv0$JTy_a%KZ8R^t8JC$6CHS2H((u!h4vazq(2c#by@$Obocy{ z!E%Ezp{17CYq8-=g9y<C_yZya~)c^6C6iSg{CGMQ0Rr&k%%rzS()8OnNJBHE?a z@7|MkZ*`j9j6&N4`-m&ne8NEg5q{=Q%k@;raput7{!LA}V!J zL?Xgw;y2_!>n~T(~av=!HSo?DW#=s7<=Q__|M8{BvxC5b;9d@)d29Om8rx zc-sS=?Xv7&FlZxmm-UxR4Y>4tFxA?cWV@|g3@IyxnSjqwE0S#$pq#8|$-Bl1=y0dO z_9boa0UsHe>wX*@-PTZW=ESoQHPm!=S4*{9b(%1?B%(=QT;3T?>QIM-)+E{BYuvEZV-9CKkv+8Kc z$?x8N^JvTn3R$sdnCLpYq~*Zju=mYl4eG&39-K`&Q-_wt+j!_sxJ@t9|73n}m!}nA z%9B0~ch`-p=?e)xZp)TAwx57udS=hJf7=gt%(Z^I)H*?|Q~1yap&G9g9>=`sQhr)S zgx*GFP4AIVg{7?{^@D1N1Ai&I&uVIQpTl{`{#+A8e>QWK`ti9SNA*o?fy%{jx6N$a zg%WS_WWh}I-1P}(omY+;GJ?p%XyLvDS!>v6K>CtDD6ugEjknw|9``1=;^|bUtnPp# z`$kcFNqL94&F9?4r<=sJ&N)6!qDAKpMFpdc5syvu7?--xy&`J!`%ZtJ(T?*x(DfWP zeqMF7TN?Yt5S{6H+89is@w-F=l?OELq6U&&pWqAXaPhz0DLNH{tuXWR6yf7mPa~}o zGY)5xmvpR#?sIHXr;fJBtG!*zbQOtKjcD;X1IL=T60`m>O%k<9B3!R;mpr#X7#i{@eAs6(s1wxR!WMf=igP}Rd7Y+Ya&WJxTlE4SRKn^m7|izpKg z!_eBWN(?8H1n>CnyH)KDx3nm6LzG?ls${9^d;ELA*U^GcACfG?kOU$TyjmWc8@ zGLv4B>7uQE1LKZ+W$Pq651mXinBYW27NxIS812vco%&Hsg$S^WqWX?O4p~{8$lPXA z=mkr)p*-H*`Jz3>*vMcD#rc}?TTR;pX2DT_%kqibAMN;;t}<5Og?J*um=nquh)r{4 zYnBP=B#ll^zmQ;iYx9a4|6XZH$R|BYn4lU)kDec=<~o6%%?<}FISv53EQ=@qxl=Y(Q~GMqmI6WU=F;5cD8(OcBmU66S>l1of%8U*ow3Z28e)Eq#%FAZO#zJukLBTb+t z+!BxPn^J$CdV%q4>hO#pjq4cOfNotHek6Bxew6D`TlsNoO6#;K#L0XwnPtS1*l8`q zct<@tinP_AIGgB|2TdBhIG%%_L5T1$s=(XneII9si_{p~XEkiZa5rNzv!!J`!QBbT zJ{^DW_I8MoSW%F;4SNh%gaDm!jAp6zU2^qo%|lt)69t_svbFM8Kziz+ZV<8 zJx}=7O7!bUTee284Wkau1DFi1WPM6?J*s88c0l5`rizQIbe>mhxjX!brvrGDcdaaK zce9T_G{QCoS;)RkI)!AQPatZak=U(XFf+yiyK>*?8VKrqE2lg4B9|4(!*ehDdKO!d zNZxL~8#c2VX0sVs;Nl#5NS56<>zS8JGh^mS6%jQ9yRxSKL3poatKoy)VB*!^rKyQU z9yX1|z{2ojLj0wYmYYM1rQ;b;I$A{I9*cjzTbBty zwTJH`W=aVywhzJySUF4>qY5`PDzJ*~~h(896jy$vT|NW@D%#$XXFeYKH>H`qCH z#VyDrBIEuSwJ<$3_Uo>jW@?T9c+v!#LS}0cPOea$^{+n@RC^p98!#&-KV)tGyR_`fK}L8d5IY=m~QKTa2b&}T+M1L`L0 zohL@uWxL}_M7>O)SmdN+&;&IQSZH%A!!bR`Tld)=3PUrIXHh^yhsYs$qz`e12&~KF?rKC)QY7e*=D_VMUpe{ znFR${bu8QqAsB`(Wd*)%3)*a{)~eVUp@B2d?GqlS7k4EvG^u5fy6#A3s$4f^@uL3C z$*9=%S)+@|m9ZO7HG|5j9vvQAe@++Iu$XQy))a1YpOeaSNFXI}bVk-g1mW4gJ--iwIpRolCbN{O(Zh{x_K`D8#BT2Wn7_`xZ%GwzgkM|fNI`ayj^_meel-PBE z$8Rw+HDm(zHZzSr=lhtW#>D8|JuY7LAx#5qTc>WaK&8_Z8pk6t7-)?U4xG3IdiOx} zs6zi65;VMHGlOINdTKJk-Oza^#7AcL)6V{8+_jQVkd)L^;tpvQ5eKpf>F!AajojW2 zTq}P|rUOi0oek!3tm)0gKQNV?##qIywf+iJ8S*bn?+wy!wWXQ;i?3Q^6%mqV)6(${)E& zx){xR;YH@ChSk0Rf%KsN+AALWyp|?U-@nZ>gb+T2Op6+Pe&Su&Ni zayQxUF#ne^-WirKD-CnioNTpB)Z1YqYy^=nC2n%Xku|m1RP<_SD(AUu&(Oc^MyN|U66&*YGY-UMaO?(@_N}i>+a% zU%rTXcG&p|;iV?dwdiN0+F{H8X91kV9wh_jSGH>@g{?_&yE~@KG|bquDaZ4qY@=3Y z#($pX-gr!@)5mfze3F0=-1u5GG~gy($SQPWn5L~A7|_wcgU0NE?a+g-EwpUX(Tr}q76{mafU zk5xW98qK4TI5;6P%V$8O29WLDb{oSWO?BCv+dyH4E!miDTRUdO1?%2*a6}euzWQlJ z@f8MefUOt+)o4pS>3A!p>>v%bDnQULi%H~mEyUjRI@UT^WM%awv^w={M*c)7@8UE8 zEpxy?NnkErxW5+5e1-9yWVRbauPkHYX@Kia1f_G7uQz17 zVukZ81rvtujYcS1Asn4~K?%~f?6>oLJ7CH~knhrmS8dOL$K2-|#9uZ2`4%-TUc)24ND`h4dpk zKoxSdcSOcG%d;}k$^D~3yh9!YSytsH0(itHNw$NK%IR%K^vs>weKGwgx5Hg2T(;)}l>PqLwCBYp7sA6vQsp|B1}HB|h6X>wfy zMp}*Zs;khN%8mGUSAu>Y#InFiwQ+@i`g2>n2GDdHuSdU$sy7N4eC`$Jt7B6d9Lkom?TC=iDKvmU z&>N@l`WK$I0euLas!-e>cPZBQt?9(+v>W^rMt4mrTSuUI-t zDWN^rn(Z}o&g^jqIalQ`cVd65Orol`sy^|W0((3drGK$Vgh-fiVtWE<+<}S>fW<(+ z1VsIvx7cF5bIOXu^&Yt|<25?Ms_Z9U@-CQc`hmBR4Bl@VCjLg&zR_{ftvOl%TTk9}y$Ts`p-=C`8 zFCnEbw-SSU!}DNVeaFQjzNz6zstuMWi##t!o=oqW^B-qq;1S3ViN7S^;o^k%HZGYEm^nFVR ze=dFWn1e?Uivtr}ben*3nGC3p`iH0R0qw&I7={)po7M>Z_v#Yhe8lH@U*}G;Itb=o z{$A-#&ge@tj(3P{(ulgcE6$*yaWLzwUPpG1cEl2Q6KVN`^wi(H-}pySrjeN8n*~ho zj0GMuF4PyoKod!%8mc6S)I=*pUPyQu(y|ITzR02#o*yQHI+_PANrL?MUQmHj+Ks88 z;)5kMMyk-;GJTt@Fx?@hOdA?PB;S5m*MT%sx@O(yS3i>T4MLd`5iB88<7_`U*uQiI zQrdP|Mc1|>u2FDJ=$a%;yX}+`pNuL}7pXA`2JjE?x*T5{_UVu%Pn>lMn2cv#zSigH zF}W09Q^rzR^F2Jc0KcZIq@APAq|fE8ICe%=iewWBup%|RCt!3vy*47IZ1yV2$0Z@!=jw@%I*$q6 zB7Q#KHWb9aS8HW`rtGW-KDriHOAY<#q7ud8OgjQ)GchY+@}Nae^B7}mV4h~$Mhyki z7c*AzN?EbfBNnly9FM!ulJ}0RGn1yVNHeBV23@k5o$eNMHcpL!+9}@*9Lw+6>$pRI zhx=wxObV;ijhHmBnNJXk3vqqcv$^`k}O=wMh_3$~@5scS2L7W0+o zK^W@%TuzO(u|ALrZW2`&lW90s4rHpF-A%sMqvMyrKwhB9Rf~`841%MJ_*C)y&^-Kx zv2*;tkOw(yX&m+-oc;XxYv}REM{yu2%x)}OP*Ga*uaUi+lXo9mf&@5Cyy_L%R%^=a z13k29YDHB2$Bbz$qQzr|y&Nz^t8HE%6$?L^0wdFZ=*ZqYxfcQ2>qjJgEM`3RB+(Qw zxm4TEgYnx-oE{R&*&e;I?0;CT_ zz6Mhz2xZPbw$3jQ-cNbJE#@Mn@-pbicFmfuOM&lP5ZxTc(vLBqH<$h~8&V!q@jbt^ zzO~@+n*Dwz>aM4oR&RhYFIim=3((Z{wh`PRG*V)kIe8c$$ zEVLnmu=i+a(UNB^4{pF>kI7)4IBz+79h~gjzPX)!5zUE=X#K|0Mmb((l0o96&HM#^ zTRuySJF7R6IWb?$`K7oJroU_T>5C-tWtS0IM)>auzx<9d7Z+3rqAc4d)8{E@e5)SF zIznQNkJxh6sy?E9Wo+L;u45=2Hg zq7Dytk*7qXs%``)S(`bAQ%4r_%xfti=0ZbwYEmwWEb8I;{`}B0p`t`8Z$xi6L6Co9 zxHaY-Bzl`VUxYSUEyZ71%8&Dtu~90yG2!|V$;syMgsI1ual|P-)TZn?{G-jEbrmmx z2s|YVa9x^wPSxVXK2NIkL?(1wKxA}Z82uhzbz_d=660dAnjZMchL)uO4WSYyiQBc4 z(b{E&0U+?(kP0>+-z$hkjl{M{^)7jQPUFEwi1R$Jl>F+J;EfA?wdguXB-m;B(e1hcA*K)-r`X3mRezxf}<#sTwAB`k=7 zyBtWo-$DH{HL8Te3YhhNisI)+8L=WQraTq{;Xf<4zI575hWPmug0SP{|9$UCmi4w7 z!qqVsbApm{*xxI71>ZMX*i2>iC-=1Y3w2v*k%{#3nf(N}#y6M-43=&#D8Il<-QVGL z(LkzK4k0lKwQL3gq5<`PF2_57RR`_ISx`tBP@ zGS_VQW&kQS#=3B;}wYF6Q;p%MIC7@ewb3bV0}}`Nb05 z{F&DtyQCP`_CJ@Q!^J}5HN7)4JCPS<4Tr0;x!+LkL1^rao^DYbd+`ive=? z0fazVrpmUtis)p&5`ck5-}&pOeR)C%Ww5fY*uQXzSTQ^~1JDOt-Krt$+VooJ=5`6y zM!v2fD>4ju-FW!U)DC4GJbPhcS-nvKbl6tl>U%b~8oM$&dr5Hi1 zCJAd<7=EJ2T&y zXY%@p5YuiW(HIopp4?--eo-jgxr$)hSm&cGz~v(o1#+-ee4 z15=aWEj+$ag@1o%i8-}F7cK^25C6mKx>BEW(t1AlePOV!D`hwSEJo;yN>VM$g~}Y0 z45_xMGSOB6HRoVs=v~CpJ99)BrEKthSKh2?`ai7QkbwbyAz$FCcd6WAyR~X?#bkX%}B(p`H zN7o?nJ)~t%a025SWw}V#cxNG<)EeQ}-g;v~PBL628BKKe5{nI@e*6QX?1&?l_Pq?; z0bsu~)~?-qiAu)Rv@8p~zS$|unbp z)inD2Y6mpwT%a$QaiH}hd#Q6=E@FbxP1z3coeA~$0r-#S-*J!`=Y0!y4eT|)-b=H>Mr zpEc>~p0B)k11Ld9z5S=!_}kYn|BIJgZ0~*-Ev#5L^Om*!)^zLJ{J)|st8d#0iBOSW z23aSfIv885tP`&9&#qOU}r>qfK1Dp4o)DtBe|n>zlN z$(@~^-BVG2xHUQW${$$f~lz2L$_&}=Zl zl^?%@21VCWg!y=H;<;qBsH8J{Urg%IPylu6}k7P%N_BL73S+>c zA(qU4NRx>*q1imKzP5Rzi}t06xRd{Lm=}x3YSK-o34XUH*Axh^;Q^RbF%o&1StC7^Ps<54r9~=nuF#qT+miI^sa8w|;QiB(4>~y{~bz zPo-j5EkaIRPCC~3#uVlItDK+F0VxI(%JLkp87C1CM^n_=+Ts+#JFf`$jicsLQVAgC zBCGWB27xi@sfUD0jT$4hqCy#mQD;2!V8K-LPT_E*6;HV^sqX`>^92QFh!bhtVf4mmK(k5#|3MSA)w^_Y8Xq@k5ZT1Lx+WjpSMff zxIGi&`BB@Nv+E;^L1|ehn8%`EDKOQ-Lw(g1QOIq{TKraVNfch2M65elqt*+udqoE9 z3=&-o@0c3yWIZG!T85;vGv%~#25^^GF{)f^rn~GAW)*_vN7n*vA^f`|(Y(jC44dmt zX^R~TOQzTu*8zvvSy@Ki+McjHkLtETmm71t+?YXjs4d0#xZ>$TdCs>Vg;nA-o5ahh z6pwJPDvf0FO$1kv+!SLiury}7i_pKdPChqbN&U^TG0;wkzuBs0C~^jbT_+DzN=BsV z8Bi?@rbUn@KJ_*=1LQ09tR(yc*;!31Lb2c`!Ez@|!Y)#Wuyqi@4%_8+ebdCBQ% z_QSfJ1ryC=mgS!A#9w zJhHwDYG@Af6y}&%IA{WQFwsv@Wz01Oc1HtR00UVxJyO#PiVlDD5Z8x(ASC+PT?T3L zLb%~!+9a$lGR13*Hfufm>qk3Tg70nRmDrC`$k9hHwTS=fWQv?6`&48_4f$5D>@3~k zf-_8V@`uRwPsRydAuI#VYip|HPqH}k1|W$86!w~GDy`XtpR9nvBj=H`_--NG`5cg4 zq9ZWb&DhIMY~fM)?kVnrM8UORl9VF5vl&aN7&0h3!7T`?ej}4m+`*J|+K7xBV zj*tP>AA?-5KKkVUMJs^eJm|q1A0c0p`)6eZ!;;q&^|8~1&#Sc#@_WCg13jVIqUrlV z$@iMqRe;@j@xU5v=a^=glR4AZmX+W$juFZU@TbvfgIgvh0Bz%Tdqd<{rlacRMymOD zzaKPW^}OA9TP$L#VGy4b1=|5F><_^~~|6g|-x?I7?4$ zx)%e*u z>8N+zK%y{gi!E%yyDop|lhVERXP5}Nob_-Hz&lK;bkf@~Ppk$(YJ}|}Z9H!f{N+q0 z9}Qj*A=>@m^N<*#^$Al|2WqQF1Zr%QkxB}L>pNA>A)g56`AeM)@<=NL_GOj5U71ra z-OE@y1`PxJTO3arDhipt8h7=UyJcC;?Et~A!Snm`{LijGusmf>zrqVRSN5%9RawG{ zJB1n$aP+c_qtpDc#PIV?-ZwzQDPn7LCW2J54Sej|bqC7j zHIsHj6~Yk-!r_kEtVD1YGR|yurLd*1bpb|_%d}lDS zuZ(*-45TP7=Ub(NRX$A4Mr<^XEqs0g5|1Dd)ReZJOy{SHnlz-s(|0FuXJ3DlOZ$ue z!yU0pa-yrGbBQLf6y#SV_i)}W)DH^)#|`ecURS_zs}w+m+nrtWtVvjeuo~6Qv(Z23kkXmOI>BJb`6!v zxJ_+@10o_{sUyFB;&xl8K_&6qB;9tQ3`*eFk2sd7)QgSMPRh8w?osWLL?M3Q&A!I0 zGyVGovp^oH_(e^}O?8+GV;rd%%k~>(>1CQT>i1iQ=oIiD7N6!7>~YsDXa59 zpp_alkzHSdajd`?ac)SeksaEM3FU=KEJd**ntS%Ej`%f?Q-jH0SrUBT+Zpf`d&Iu_)zc54l$-N>dPW0$(6CRUBW>T5EfCwL>6=AYR-(hs@OZvc z$c2|EoFO?uMkTgImLGid;Fe{vaHV*%Z)-9;;&X&}91vEeDJRPy&)T)wbrjZJ=a6~( zvuWV{VN%Gkp#;!mCf1C4YsJhk9_HO&hEi(F)jL3xCQzE$G)^}^&P)z~_uCrJTlQ-T zv&@mFGHqMaq_TvJ4F18p)Di&Y&%g;zfDu4C=)h}PB&V6NR zCI-C>uE{!2@=Qg%mXEy5EGpMFoB0)c{Bb7ObJ-{o14ezjdyR^B>6z-YN*S+GDv5IX zb6)#YS249-_!xJO5r=9(tVu5UUo?-g)0*6<;ZsO3i!VjI(^zsuM0rJXcfj+c4IUtP zcWHHODD)3@w4A*khupFr(78fpvv=S9+$5(v)eA@9k;QV^66q>)lE)S;%2pzx`MT>4 zXV2GP4=lSgtlo8592Nmk;0+tj_085L4`(?fiLP~oJbN+2gwHyVR)Tx;*D_=&S{ID%sI ziWYSfOn*ddtj*UC%NX#DMVzjWf;}cdVfJFHL8N`3&f#hcO}?hwISa;X)?35)38d(WF5pLf()bMgx%*ca3F!r^w=_zhZDi6qdr6=}7c!W`OFH z-NfCW7Fh2?dL9R1VHtS;a3}?paLFDS`p_y3fk$O=|3akCVW)UwG>pIRYYaU9Lw?_O z_l2_%rwbBot@nw)borVUTQrr0Mx_sXEvN`hiQa*ul}`B|W~)pO<^{m0-kyg3P!aNQ zQ>%&qi<=EYIM&Da@u{~3#MM#v4qFV2O4u%{S-G>W(z6Y{B-GQj&;~G>KGgaDU9qrH zkHz%&QL`5bf6e~bY;Gjh{EXjk9(6pOi-Yj(0z+^9u0|nObCD4^)<9UpW7L%x2=yp; z-Jb8h;;|b%%1xT`v?##!*%8m{B?vbWt(Huc5V9|=Ug7wxhqn_YKgxWr(=nu-YK3k5 z!Ji`vSp_|Y)x3vd%Bq%$-Y1eO*oJ95%*9y;`ObW$#C=V*w&Ck$O`11kuY$Ns zETsIRf;ms+r3E8g?{6^{7Wq@?b1BvNd|u)HRcrNjyJ2DW5RZ5-0}o$!gqLI77B77-rT$LGQf33#tA6>0nOG4j4+T< z|3UlEa&2qZWG3v9FU`Hzf3f5@3<} zavQ~uFa8z!287Gg-3CPEH{g~EXQ8yZ%_F?+nIvo&#QU=+_G9#3ltz5a_21-ur?uhF zIIatn`J#q$9Y5~Ia_sW;RR{|91_xP+zRD$xfu2L^5nCQ9xw;=R;jPpDf6mTw)8`q;wh zutXQ@KAGvKms~X_ybC!hR$NV}QJ>amG+T2BOJr-?Er;&+XRK$P8;n3gorS(!zbxGE z(9+M=#J(l2lfGtoIKecgba~UraOD%ncvH zN5h9K63!Qv(Mdzr$&gGdXX2W^RYR;VsMvO0eKP^rSiK3&y$Qu-T=J{LD?XAOk&0x&|V;ZNr-lX^XgWn>!+KZP%gZ?|&DHxqTk`3uU*I!wgOQkSL zkfR=jlsJC(gpD`2YCpL)!^GRqO+6El!i@aDk>;w7lCo=Vu%)gqGOshl zo%LVV==;+gVB!=5&tagk%=z6Jy?z}hKwPMHpGnvqmAV~B5kU&8j3`zm1Qudx!ikGc zWNS$G6PE)GTeZnl8NvrxI`=RYOtL-!tzJsObC+CH{>BKfp@xVR1lHcxhwMwM!{KQ4 z{w4_#6Sdh~M}x?Fa-(3aS?|6i5uGTij&4Jv3l$Cdr<@g8@F6o3;O`q@6H#p7;F3$s z`U@|Up@wE)G$DkfcfoWguQ#NWhDQ&@Y$qO@sF35M z_+SLNjn}J%(mEE-cOf*R^dIh)0n3v`pK6a7FRVl({;jIv z(fhtDQANiRD7CnQp!e)L{#~$XwN5=6s#Wr_74HUM{*d>IAnL#L5`Dau=$CmVwl~(6 z?#KT!FODla^~o2IUL6N@EDE7{tfe%Y@?q{u*q)^xfV|Fk6i2=vo_N5d+pC8+z`$U{ z^7PTrVbkb7$k`8{dJ2d6`!s;Bn_xDqB&X$6Vr09CgjMsyr0|&uDIw11rbQW=q46iZ z!jiA{TR-s;m-XO$_RqO*?)e>jGkd2JP9r{Gw%J~*CcOj$On$1f=s!t8#OYiB=3!#^ z1dGT#@x(>h6pN8jK_~OtrcRMu$4#4jUkn1nw<6ItaWtrc->J9+eOOp+{pYEutvURb z@}k6oC(w~S1=RS>2B3E*mC2r~7Rf39a6V`pYBpdeK;fECIWBeGv8G}4_R6x=XZs4@ zcr72zu9Cz7I;g9kTY0YI2Y(i(O^GvF#{jp-Nt9imbbraEQ=~MI04$zfWmXT1Nw!ay zW=Kel^2P@M&_$;fua$Kwiis$qerYOrbXs{_M7=NBzjg^&(nxc={AZ%emrglrc*yxi zf^Vb>L8B|d$}Bxx?rioX87>?SA-=KJ?9gL4j+?`EliHlRSd`%}07;mx%Uq_T-Aeao z&3&9^jjU$;pVJ#%lTDr$Lx9|%a$JP7P!vDmSUbAlayr}Ur3#bk@x>U?#<&GjrwG&XEV zQ^W(GlgKaD>9*h0*B9?N5a*>XAMze(xn;=E%$yWCRU8P&4398m;X3W=XTgAT<#MIW z_k<`?PV4#_o6t**)kzw_oUHZY0oyVZsmds7xowh(H*hYnmu(}5DC5E}E ziFDW*?+XPYOr@s+aDV0pbaym+O8)IER4Y}tC*~y$pn(OTymBXabq7+^z^PPyWMC1b zGzI6Vap0xHNi4`6^o3Dns%JV6z)Z>ZANkFAYO4U!aipXXo6H7eTT?etu%~gBUiJI+sz(~O zgE3*6Y3I`F^iV*KM15-MM(1_~3k5Z}K!1ZkY30xi!Y4kW-V*$J21m=wLMEOJKnW0hZnLSZ&p3K;^7NR#Z;? zOgXOK#TXYGH2m${)KOo%*m~}d!d_6}O)OOHAUF*h@dt)!!GR{XB}w?aS5c6`c~F|F z56Yt>sxU>u))y8lf2x;7mLV`En%Kv?;)abSNQ=Su?zaUi)4+xVIBwma+0DKbg7!Y2 z;Qe6>NFhbM>jS297hL}>t?3v;N}fQYAEoQ|CiYKc6~D5NVd8IUBjLM#weMH9C=gbE zT${hYgjBw;r?&tbV@4@DSBILcYglu{hPfyto$UG1tTvWP_YT_ zpcb~J+d8JIKPE&qX}iFy4~pE79-?jCc+uLPuDi+fG9(*Y zu(OqdBD_zAE8x`xmI0bLgLFA86h2_t5E+ESAVn>JES=Y|afa4S-*?BiN<9@2IoOU=x$Ojd#8BQI=lCQJdSv zxf#{e$$$aBuD~z8Hu_ukNOJL7{ng;YpoO1B(X$UnO3i}qf@8oixH6OM#de{`_pn6( zuM9k@3%U)0t^HbKQ{(G}TrKw4llR6(7kyiyh=tC%TebUjvLrs?)W+^=&lKM|rvu_1 zQ+_D?nl?9AX0_2jTZ_)0N0jWfmzQoOqBYuVy%rR39A?gDvhPTKPeQ5GQ1O_Fj2jRz zUE!^hTRVi`>{Kc}E~Vx)tZ;7iJt6OIa}&tLU_Er#{(D z+CGnK7-_uGbwj^NE^q+G`C3Mq4E1nM2!+y0GYw^f$!0q@#aI4s52^rY0IS2KQJTzb z@{J>v^5D+NYX8LO@6aJlA@9)@1i7El#P9REf#0?lq%^f%^X${0B8r_k(wwi;T5W67 zt{&l|c_slrb9s-;feSu6KByxmbS2243ofOA9&NP3$wg?W&+bsA#zKj^;+g{Ck`pHg zjZ2=Em;tFg?Xgvh84^Kn%8!20 zz{|&io37&5f$s{S#XkQN)ZX4WTO{9cy=ph;Xb+f%oKDXm<;BA z zM6yNX;`S0j%)8=JjO%jMe|zjZm@isj9|wn?P~*>J8x>ik*)#8gV7gouarNn4*$aDM zBa%9DW&eiA-JpZy*O~1XpkL!xrJvKDQE;_%?{2`3Se4=6D@ zowCwXlzM5lB5dTL`aj;u{e=A;o0sUOP0U?!m-4>>2Hx6<<3sL0av-n+X5emUfXB8$@#AU=Tg9*$?>PGjp#UIn7hX2^oKIJHb z0?|610bXc7I1qv4-sS_W`eTE+8J$ys(Jru&8iDUf4Q-cqnz(%e{9sA;P&lSg+^>r7 z+@S`$upgkQhji6_^eR&!TyEII_EGhX=u z^l$MI4=vv4|F&g-g3gG$i2!6gH`-c%iV+w^?1uMP;PVkhjBiLo=$C zh)3EOU<81>JjTadi+gN($ppeyib!6rCK_Oj!R@H zHNumls@AsDGKy}ymihN2X{$XrRd~XZ&Bz0Y1jQ~o56tr!Xz-+-4l=VihnC#$m^diC z`>@G6O%az;U_WPNbg6mr`vD`l{M8C8;Co;!olVyxBQbjYzd5s^d`qsenF!Pv(0wp4 zr~U((DM?v^{3NIh-nVrV$ANd1;Vn&fD^*g*pSWb8+xTI>Z|j}i)=USX1q^1zb5yph?^4E%i zB}wo1?&|}{AhX*HBxXQyQ&Be<)U(U-#2*RHarMHrxUVo`XX zT8sGkLvo;WSKb1wEc&SVhsih`_`XQsY+fDDIA_z9nW|Nz>I4+oWgW8nYH`=N(oJfq zdHBWasi~^SF7R|Olm13rMnC~Prst;Et%i%d6F!k=*_~AhJQ7YmhhC7(MR;yRMb)u{ z&|&$YQJd#?^$?oLe-&Qkt}GyP-i2B*7#b=Am%QG*H&tiC87Q~^r7)2*Smsg`vB>`Z(lTDST=O&1n9}3tMXMl(piD|urpITp0;PHT zcBe(F>g~&{_97wZl+K+$q1sb3>%${kM>~V!@qED>ZUw-B*@0 zBjYh`PWJvthte;BCk3^Sp<07qLQ9yTS|c{UB?pcWOm`@AFOqd$g~62@P20ZQgChBHA)fZh0rTwH-;MvLPj&?m9z zp#DR>=%48i{(7y}&6{ImrKh#d<>jHYXu)aX)ke0xmUlWp4 z`yn)g&d#~C(5$NU0jPq@>g%4HZ2IwIk>?X^=z~exA0b;**VMV{twt*ZZSfPack3wa zoPM5N{8T4lfTp4xH>{u$dkRl 0 - * @param raw_path is used to return the path obtained on exectuing the algorithm + * @param raw_path is used to return the path obtained on executing the algorithm * @return true if a path is found, false if no path is found between the start and goal pose */ bool generatePath(std::vector & raw_path); + /** + * @brief This function checks whether the cost of a point(cx, cy) on the costmap is less than the LETHAL_COST + * @return the result of the check + */ bool isSafe(const int & cx, const int & cy) const { return costmap_->getCost(cx, cy) < LETHAL_COST; } protected: - /// for the coordinates (x,y), we store at node_position_[size_x_ * y + x], + /// for the coordinates (x,y), it stores at node_position_[size_x_ * y + x], /// the index at which the data of the node is present in nodes_data_ + /// it is initialised with size_x_ * size_y_ elements + /// and its number of elements would increase to accommodate for a change in map size std::vector node_position_; /// the vector nodes_data_ stores the coordinates, costs and index of the parent node, /// and whether or not the node is present in queue_ + /// it is initialised with no elements and its size increases depending on the number of nodes searched std::vector nodes_data_; /// this is the priority queue (open_list) to select the next node for expansion @@ -121,10 +129,11 @@ class ThetaStar {1, 1}, {-1, -1}}; - tree_node * curr_node = new tree_node; + tree_node * curr_node; + /** @brief it does a line of sight (los) check between the current node and the parent of its parent node - * if an los is found and the new costs calculated are lesser then the cost and parent node of the current node - * is updated + * if an los is found and the new costs calculated are lesser then the cost and parent node + * of the current node is updated * @param data of the current node */ void resetParent(tree_node & curr_data); @@ -137,7 +146,7 @@ class ThetaStar void setNeighbors(const tree_node & curr_data, const int & curr_int); /** - * @brief it returns the path by backtracing from the goal to the start, by using their parent nodes + * @brief it returns the path by backtracking from the goal to the start, by using their parent nodes * @param raw_points used to return the path thus found * @param curr_id sends in the index of the goal coordinate, as stored in nodes_position */ @@ -150,19 +159,22 @@ class ThetaStar * @param sl_cost is used to return the cost thus incurred * @return true if a line of sight exists between the points */ - bool losCheck( - const int & x0, const int & y0, const int & x1, const int & y1, - double & sl_cost); - - void initializePosn(int size_inc = 0); - - void setContainers(); + bool losCheck(const int & x0, const int & y0, const int & x1, const int & y1, double & sl_cost); + /** + * @brief calculates the euclidean distance between the points (ax, ay) and (bx, by) + * @return the distance thus calculated + */ double dist(const int & ax, const int & ay, const int & bx, const int & by) { return std::hypot(ax - bx, ay - by); } + /** + * @brief calculates the piecewise straight line euclidean distances by + * * + * @return the distance thus calculated + */ double getEuclideanCost(const int & ax, const int & ay, const int & bx, const int & by) { return w_euc_cost_ * dist(ax, ay, bx, by); @@ -170,11 +182,12 @@ class ThetaStar double getCellCost(const int & cx, const int & cy) const { - return /*50 + 0.8 * */ costmap_->getCost(cx, cy); + return 50 + 0.8 * costmap_->getCost(cx, cy); } /** - * @brief for the point(cx, cy) its traversal cost is calculated by *()^2/()^2 + * @brief for the point(cx, cy), its traversal cost is calculated by + * *()^2/()^2 * @return the traversal cost thus calculated */ double getTraversalCost(const int & cx, const int & cy) @@ -183,16 +196,20 @@ class ThetaStar return w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST; } - double getHCost(const int & mx, const int & my) + /** + * @brief for the point(cx, cy), its heuristic cost is calculated by + * * + * @return the heuristic cost + */ + double getHCost(const int & cx, const int & cy) { - return w_heuristic_cost_ * dist(mx, my, dst_.x, dst_.y); + return w_heuristic_cost_ * dist(cx, cy, dst_.x, dst_.y); } /** * @brief it is an overloaded function to ease in cost calculations while performing the LOS check - * @param cx - * @param cy - * @param cost denotes the total straight line traversal cost, adds the traversal cost for the node (cx, cy) at every instance + * @param cost denotes the total straight line traversal cost, adds the traversal cost for the node (cx, cy) at every instance, it is also being returned + * @return false if the traversal cost is greater than / equal to the LETHAL_COST and true otherwise */ bool isSafe(const int & cx, const int & cy, double & cost) const { @@ -205,35 +222,68 @@ class ThetaStar } } - void addIndex(const int & cx, const int & cy, const int & id_this) + /** + * @brief checks if the given coordinates(cx, cy) lies within the map + * @return the result of the check + */ + bool withinLimits(const int & cx, const int & cy) const { - node_position_[size_x_ * cy + cx] = id_this; + return cx >= 0 && cx < size_x_ && cy >= 0 && cy < size_y_; } - - void getIndex(const int & cx, const int & cy, int & id_this) + /** + * @brief checks if the coordinates of a node is the goal or not + * @return the result of the check + */ + bool isGoal(const tree_node & this_node) const { - id_this = node_position_[size_x_ * cy + cx]; + return this_node.x == dst_.x && this_node.y == dst_.y; } - bool withinLimits(const int & cx, const int & cy) const + /** + * @brief initialises the node_position_ vector by storing -1 as index for all points(x, y) within the limits of the map + * @param size_inc is used to increase the the elements of node_position_ in case the size of the map increases + */ + void initializePosn(int size_inc = 0); + + /** + * @brief it stores id_this in node_position_ at the index ( *cy + cx ) + * @param id_this the index at which the data of the point(cx, cy) is stored in nodes_data_ + */ + void addIndex(const int & cx, const int & cy, const int & id_this) { - return cx >= 0 && cx < size_x_ && cy >= 0 && cy < size_y_; + node_position_[size_x_ * cy + cx] = id_this; } - bool isGoal(const tree_node & this_node) const + /** + * @brief retrieves the index at which the data of the point(cx, cy) is stored in nodes_data + * @return id_this + */ + void getIndex(const int & cx, const int & cy, int & id_this) { - return this_node.x == dst_.x && this_node.y == dst_.y; + id_this = node_position_[size_x_ * cy + cx]; } + /** + * @brief this function depending on the size of the nodes_data_ vector allots space to store the data for a node(x, y) + * @param id_this is the index at which the data is stored for that node + */ void addToNodesData(const int & id_this) { - if (nodes_data_.size() <= static_cast(id_this)) { + if (static_cast(nodes_data_.size()) <= id_this) { nodes_data_.push_back({}); } else { nodes_data_[id_this] = {}; } } + /** + * @brief initialises the values of global variables at beginning of the execution of the generatePath function + */ + void resetContainers(); + + /** + * @brief clears the priority queue after each execution of the generatePath function + */ void clearQueue() { queue_ = std::priority_queue, comp>(); diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp index 23e011eff6..4372116373 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp @@ -62,15 +62,22 @@ class ThetaStarPlanner : public nav2_core::GlobalPlanner std::unique_ptr planner_; + /** + * @brief the function responsible for calling the planner and retrieving a path from it + * @return global_path is the planned path to be taken + */ void getPlan(nav_msgs::msg::Path & global_path); + /** + * @brief initialises the values of the start and goal points in the object of the planner + */ void setStartAndGoal( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal); /** - * @brief linearily interpolates between the adjacent waypoints of the path - * @param raw_path is used to send in the path recieved from the planner + * @brief interpolates points between the adjacent waypoints of the path + * @param raw_path is used to send in the path received from the planner * @param dist_bw_points is used to send in the interpolation_resolution * @return the final path with waypoints at a distance of the value of interpolation_resolution of each other */ @@ -78,10 +85,15 @@ class ThetaStarPlanner : public nav2_core::GlobalPlanner const std::vector & raw_path, const double & dist_bw_points); + /** + * @brief checks whether the start and goal points have costmap costs lower than LETHAL_COST + * @return the result of the check + */ bool isSafeToPlan() const { - return !(planner_->isSafe(planner_->src_.x, - planner_->src_.y)) || !(planner_->isSafe(planner_->dst_.x, planner_->dst_.y)); + return !(planner_->isSafe( + planner_->src_.x, + planner_->src_.y)) || !(planner_->isSafe(planner_->dst_.x, planner_->dst_.y)); } }; } // namespace nav2_theta_star_planner diff --git a/nav2_theta_star_planner/src/theta_star.cpp b/nav2_theta_star_planner/src/theta_star.cpp index 28e9c3f79d..af2217d412 100644 --- a/nav2_theta_star_planner/src/theta_star.cpp +++ b/nav2_theta_star_planner/src/theta_star.cpp @@ -20,21 +20,24 @@ namespace theta_star ThetaStar::ThetaStar() : w_traversal_cost_(4.0), - w_euc_cost_(6.25), + w_euc_cost_(7.0), w_heuristic_cost_(1.0), how_many_corners_(8), size_x_(0), size_y_(0), - index_generated_(0) {} + index_generated_(0) +{ + curr_node = new tree_node; +} bool ThetaStar::generatePath(std::vector & raw_path) { - setContainers(); + resetContainers(); int curr_id = index_generated_; addToNodesData(index_generated_); double src_g_cost = getTraversalCost(src_.x, src_.y), src_h_cost = getHCost(src_.x, src_.y); - nodes_data_[curr_id] = {src_.x, src_.y, src_g_cost, src_h_cost, - curr_id, true, src_g_cost + src_h_cost}; + nodes_data_[curr_id] = + {src_.x, src_.y, src_g_cost, src_h_cost, curr_id, true, src_g_cost + src_h_cost}; queue_.push({curr_id, (nodes_data_[curr_id].f)}); addIndex(nodes_data_[curr_id].x, nodes_data_[curr_id].y, index_generated_); index_generated_++; @@ -76,8 +79,7 @@ void ThetaStar::resetParent(tree_node & curr_data) if (losCheck(curr_data.x, curr_data.y, maybe_par.x, maybe_par.y, los_cost)) { g_cost = maybe_par.g + - getEuclideanCost(curr_data.x, curr_data.y, maybe_par.x, maybe_par.y) + - los_cost; + getEuclideanCost(curr_data.x, curr_data.y, maybe_par.x, maybe_par.y) + los_cost; if (g_cost < curr_data.g) { curr_data.parent_id = curr_par.parent_id; @@ -106,8 +108,9 @@ void ThetaStar::setNeighbors(const tree_node & curr_data, const int & curr_id) } m_par = curr_id; - g_cost = curr_data.g + getEuclideanCost(curr_data.x, curr_data.y, mx, my) + getTraversalCost(mx, - my); + g_cost = curr_data.g + getEuclideanCost(curr_data.x, curr_data.y, mx, my) + getTraversalCost( + mx, + my); getIndex(mx, my, m_id); @@ -159,10 +162,7 @@ void ThetaStar::backtrace(std::vector & raw_points, int curr_id) } bool ThetaStar::losCheck( - const int & x0, - const int & y0, - const int & x1, - const int & y1, + const int & x0, const int & y0, const int & x1, const int & y1, double & sl_cost) { sl_cost = 0; @@ -221,14 +221,16 @@ bool ThetaStar::losCheck( return true; } -void ThetaStar::setContainers() +void ThetaStar::resetContainers() { index_generated_ = 0; int last_size_x = size_x_; int last_size_y = size_y_; int curr_size_x = static_cast(costmap_->getSizeInCellsX()); int curr_size_y = static_cast(costmap_->getSizeInCellsY()); - if (last_size_x != curr_size_x || last_size_y != curr_size_y) { + if (((last_size_x != curr_size_x) || (last_size_y != curr_size_y)) && + static_cast(node_position_.size()) < (curr_size_x * curr_size_y)) + { initializePosn(curr_size_y * curr_size_x - last_size_y * last_size_x); nodes_data_.reserve(curr_size_x * curr_size_y); } else { diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index 4b29b8e472..3389aded5a 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -35,15 +35,13 @@ void ThetaStarPlanner::configure( global_frame_ = costmap_ros->getGlobalFrameID(); nav2_util::declare_parameter_if_not_declared( - node, name_ + ".how_many_corners", rclcpp::ParameterValue( - 8)); + node, name_ + ".how_many_corners", rclcpp::ParameterValue(8)); node->get_parameter(name_ + ".how_many_corners", planner_->how_many_corners_); if (planner_->how_many_corners_ != 8 && planner_->how_many_corners_ != 4) { planner_->how_many_corners_ = 8; - RCLCPP_WARN(logger_, - "Your value for - .how_many_corners was overridden, and is now set to 8"); + RCLCPP_WARN(logger_, "Your value for - .how_many_corners was overridden, and is now set to 8"); } nav2_util::declare_parameter_if_not_declared( @@ -51,7 +49,7 @@ void ThetaStarPlanner::configure( node->get_parameter(name_ + ".w_euc_cost", planner_->w_euc_cost_); nav2_util::declare_parameter_if_not_declared( - node, name_ + ".w_traversal_cost", rclcpp::ParameterValue(6.25)); + node, name_ + ".w_traversal_cost", rclcpp::ParameterValue(7.0)); node->get_parameter(name_ + ".w_traversal_cost", planner_->w_traversal_cost_); nav2_util::declare_parameter_if_not_declared( @@ -81,12 +79,13 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( nav_msgs::msg::Path global_path; auto start_time = std::chrono::steady_clock::now(); setStartAndGoal(start, goal); - RCLCPP_DEBUG(logger_, "Got the src_ and dst_... (%i, %i) && (%i, %i)", + RCLCPP_DEBUG( + logger_, "Got the src and dst... (%i, %i) && (%i, %i)", planner_->src_.x, planner_->src_.y, planner_->dst_.x, planner_->dst_.y); getPlan(global_path); auto stop_time = std::chrono::steady_clock::now(); auto dur = std::chrono::duration_cast(stop_time - start_time); - RCLCPP_DEBUG(logger_, "the time taken is : %i", dur.count()); + RCLCPP_INFO(logger_, "the time taken is : %i", static_cast(dur.count())); return global_path; } diff --git a/nav2_theta_star_planner/theta_star_planner.xml b/nav2_theta_star_planner/theta_star_planner.xml index bdedae4a22..1890ab14c3 100644 --- a/nav2_theta_star_planner/theta_star_planner.xml +++ b/nav2_theta_star_planner/theta_star_planner.xml @@ -1,4 +1,4 @@ - + An implementation of the Theta* Algorithm From c2baaa086ffa33f53d72638b2858cd396deb708d Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Tue, 30 Mar 2021 20:27:28 +0530 Subject: [PATCH 38/81] Update README.md --- nav2_theta_star_planner/README.md | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index fecd798320..5faab9f897 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -1,21 +1,21 @@ -#Nav2 Theta Star Planner +# Nav2 Theta Star Planner The Theta Star Planner is a global planning plugin meant to be used with the Nav2 Planner Server. The `nav2_theta_star_planner` implements a highly optimized version of the Theta Star Planner (specifically the [Lazy Theta\* P variant](http://idm-lab.org/bib/abstracts/papers/aaai10b.pdf)) meant to plan any-angle paths using A\*. The planner supports differential-drive and omni-directional robots. -##Features +## Features - Uses the costs from the costmap to penalise high cost regions - Is well suited for smaller robots of the omni-directional and differential drive kind - As it considers the costmap traversal cost during execution it tends to smoothen the paths automatically, thus mitigating the need to smoothen the path. (The presence of sharp turns depends on the resolution of the map, and it decreases as the map resolution increases.) - Allows to control the path behavior to either be any angle directed or to be in the middle of the spaces - The planner uses A\* search along with line of sight checks to form any-angle paths thus avoiding zig-zag paths that may be present in the usual implementation of A\* -##Metrics +## Metrics For the below example the planner took ~44ms to compute the path of 87.5m - ![example.png](/img/Screenshot%20from%202021-03-27%2000-37-47.png) The parameters were set to - `w_euc_cost: 1.0`, `w_traversal_cost: 5.0`, `w_heuristic_cost: 1.0` and the `global_costmap`'s `inflation_layer` parameters are set as - `cost_scaling_factor:5.0`, `inflation_radius: 5.5` -##Cost Function Details -###Symbols and their meanings +## Cost Function Details +### Symbols and their meanings **g(a)** - cost function cost for the node 'a' **h(a)** - heuristic function cost for the node 'a' @@ -45,7 +45,7 @@ Because of how the program works when the 'neigh' node is to be expanded, depend on the result of the LOS check, (if the LOS check returns true) the value of g(neigh) might change to `g(par) + w1*euc_cost(par, neigh) + w2*(costmap(par,neigh)/LETHAL_COST)^2` -##Parameters +## Parameters The parameters of the planner are : - ` .how_many_corners ` : to choose between 4-connected and 8-connected graph expansions, the accepted values are 4 and 8 - ` .w_euc_cost ` : weight applied on the length of the path. @@ -67,9 +67,9 @@ planner_server: ``` Do note that the `global_costmap`'s `inflation_layer` values recommended for the above values of the parameter are - `cost_scaling_factor:1.0`, `inflation_radius: 5.5` -##Usage Notes +## Usage Notes -###Tuning the Parameters +### Tuning the Parameters Before starting off, do note that the costmap_cost(curr,neigh) component after being operated (before being multiplied to its parameter and being substituted in g(node)) varies from 0 to 1. Keep this in mind while tuning. This planner uses the costs associated with each cell from the `global_costmap` as a measure of the point's proximity to the obstacles. Providing a gentle potential field that covers the entirety of the region (with only small pocket like regions of cost = 0) is recommended in order to achieve paths that pass through the middle of the spaces. A good starting point could be to set the `inflation_layer`'s parameters as - `cost_scaling_factor:10.0`, `inflation_radius: 5.5` and then to decrease the value of `cost_scaling_factor` to achieve the said potential field. @@ -78,7 +78,7 @@ Providing a gentle potential field over the region allows the planner to compens In order to achieve paths that stay in the middle of the spaces set `w_traversal_cost` at a higher value than `w_euc_cost`. To begin with, you can set the parameters to its default values and then increase the value of `w_traversal_cost`, while also decreasing `w_euc_cost` to allow for an increase in the path length thus letting the path to settle in the middle regions of the spaces with lower costs on the costmap. While tuning the planner's parameters you can also change the `inflation_layer`'s parameters to tune the behavior of the paths. -###Path Smoothing +### Path Smoothing Because of how the cost function works, the output path has a natural tendency to form smooth curves around corners, though the smoothness of the path depends on how wide the turn is and the number of cells in that turns. This planner is recommended to be used with local planners like DWB or TEB (or other any planner / controllers that form a local trajectory to be traversed) as these take into account the abrupt turns which might arise due to the planner not being able to find a smoother turns owing to the aforementioned reasons. From 653f9201cbed4f14e7ee351a879be37e223a4a82 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Tue, 30 Mar 2021 20:28:02 +0530 Subject: [PATCH 39/81] Update README.md --- nav2_theta_star_planner/README.md | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index 5faab9f897..ba129cc357 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -10,7 +10,7 @@ The Theta Star Planner is a global planning plugin meant to be used with the Nav ## Metrics For the below example the planner took ~44ms to compute the path of 87.5m - -![example.png](/img/Screenshot%20from%202021-03-27%2000-37-47.png) +![example.png](/img/00-37.png) The parameters were set to - `w_euc_cost: 1.0`, `w_traversal_cost: 5.0`, `w_heuristic_cost: 1.0` and the `global_costmap`'s `inflation_layer` parameters are set as - `cost_scaling_factor:5.0`, `inflation_radius: 5.5` @@ -84,8 +84,3 @@ Because of how the cost function works, the output path has a natural tendency t This planner is recommended to be used with local planners like DWB or TEB (or other any planner / controllers that form a local trajectory to be traversed) as these take into account the abrupt turns which might arise due to the planner not being able to find a smoother turns owing to the aforementioned reasons. While smoother paths can be achieved by increasing the costmap resolution (ie using a costmap of 1cm resolution rather than a 5cm one) it is not recommended to do so as it comes at the cost of increased query times from the planner. Test the planners performance on the finer costmaps before making a switch to those costmaps. - - - - - From 0a34ff3722084bc1253db8dfb86c3a85f8a0c21d Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Tue, 30 Mar 2021 20:28:43 +0530 Subject: [PATCH 40/81] Update README.md --- nav2_theta_star_planner/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index ba129cc357..e41b05f7ff 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -10,7 +10,7 @@ The Theta Star Planner is a global planning plugin meant to be used with the Nav ## Metrics For the below example the planner took ~44ms to compute the path of 87.5m - -![example.png](/img/00-37.png) +![alt text](/img/00-37.png) The parameters were set to - `w_euc_cost: 1.0`, `w_traversal_cost: 5.0`, `w_heuristic_cost: 1.0` and the `global_costmap`'s `inflation_layer` parameters are set as - `cost_scaling_factor:5.0`, `inflation_radius: 5.5` From 6f07371f827fdac83957273084f02ef72f58e02d Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Tue, 30 Mar 2021 20:29:25 +0530 Subject: [PATCH 41/81] Update README.md --- nav2_theta_star_planner/README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index e41b05f7ff..dd11c6e17e 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -10,7 +10,8 @@ The Theta Star Planner is a global planning plugin meant to be used with the Nav ## Metrics For the below example the planner took ~44ms to compute the path of 87.5m - -![alt text](/img/00-37.png) + +![example](img/00-37.png) The parameters were set to - `w_euc_cost: 1.0`, `w_traversal_cost: 5.0`, `w_heuristic_cost: 1.0` and the `global_costmap`'s `inflation_layer` parameters are set as - `cost_scaling_factor:5.0`, `inflation_radius: 5.5` From d4ca17c8750cf4f5498b67bd6828509ca37e9a59 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Wed, 31 Mar 2021 01:22:38 +0530 Subject: [PATCH 42/81] Delete global_planner_plugin.xml --- nav2_theta_star_planner/global_planner_plugin.xml | 5 ----- 1 file changed, 5 deletions(-) delete mode 100644 nav2_theta_star_planner/global_planner_plugin.xml diff --git a/nav2_theta_star_planner/global_planner_plugin.xml b/nav2_theta_star_planner/global_planner_plugin.xml deleted file mode 100644 index b33665c0c7..0000000000 --- a/nav2_theta_star_planner/global_planner_plugin.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - An implementation of the Theta* Algorithm - - From fbbb8c147aabb741f040aeee7c04af76e81bc3d4 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Wed, 31 Mar 2021 02:12:00 +0530 Subject: [PATCH 43/81] fix the linting issues --- nav2_theta_star_planner/CMakeLists.txt | 20 +++++++++---------- .../nav2_theta_star_planner/theta_star.hpp | 3 ++- 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/nav2_theta_star_planner/CMakeLists.txt b/nav2_theta_star_planner/CMakeLists.txt index 738faced51..27bc8ad3c1 100644 --- a/nav2_theta_star_planner/CMakeLists.txt +++ b/nav2_theta_star_planner/CMakeLists.txt @@ -15,17 +15,15 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(tf2_ros REQUIRED) find_package(ament_cmake_gtest REQUIRED) - nav2_package() include_directories( include ) -set(library_name ${PROJECT_NAME}_plugin) +set(library_name ${PROJECT_NAME}) -set(dependencies - ament_cmake +set(dependencies ament_cmake builtin_interfaces nav2_common nav2_core @@ -37,13 +35,13 @@ set(dependencies rclcpp_action rclcpp_lifecycle tf2_ros - ) +) add_library(${library_name} SHARED src/theta_star.cpp src/theta_star_planner.cpp - ) +) ament_target_dependencies(${library_name} ${dependencies}) @@ -55,21 +53,21 @@ install(TARGETS ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME} - ) +) install(DIRECTORY include/ DESTINATION include/ - ) +) install(FILES theta_star_planner.xml DESTINATION share/${PROJECT_NAME} - ) +) -if (BUILD_TESTING) +if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() -endif () +endif() ament_export_include_directories(include) diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp index f9583c18c8..03e4868db9 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp @@ -109,7 +109,8 @@ class ThetaStar /// the vector nodes_data_ stores the coordinates, costs and index of the parent node, /// and whether or not the node is present in queue_ - /// it is initialised with no elements and its size increases depending on the number of nodes searched + /// it is initialised with no elements + /// and its size increases depending on the number of nodes searched std::vector nodes_data_; /// this is the priority queue (open_list) to select the next node for expansion From 5c42a87bf70e18dc7f5cf834319759739775df5b Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Wed, 31 Mar 2021 07:45:09 +0530 Subject: [PATCH 44/81] remove the space on line 7 --- nav2_theta_star_planner/src/theta_star.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_theta_star_planner/src/theta_star.cpp b/nav2_theta_star_planner/src/theta_star.cpp index af2217d412..5ad263077f 100644 --- a/nav2_theta_star_planner/src/theta_star.cpp +++ b/nav2_theta_star_planner/src/theta_star.cpp @@ -4,7 +4,7 @@ // 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 +// 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, From 2137bd57099ebaf40da93a80d6ce0d6aff736d2b Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Wed, 31 Mar 2021 07:46:28 +0530 Subject: [PATCH 45/81] change RCLCPP_INFO to RCLCPP_DEBUG --- nav2_theta_star_planner/src/theta_star_planner.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index 3389aded5a..3fcfaad1fd 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -85,7 +85,7 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( getPlan(global_path); auto stop_time = std::chrono::steady_clock::now(); auto dur = std::chrono::duration_cast(stop_time - start_time); - RCLCPP_INFO(logger_, "the time taken is : %i", static_cast(dur.count())); + RCLCPP_DEBUG(logger_, "the time taken is : %i", static_cast(dur.count())); return global_path; } From 6586a8219f32ddb434dcbb6144796ff77377ec48 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Wed, 31 Mar 2021 08:27:49 +0530 Subject: [PATCH 46/81] Update README.md --- nav2_theta_star_planner/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index dd11c6e17e..ec76549810 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -39,7 +39,7 @@ respect to a function, value = 253 ### Cost function ``` g(neigh) = g(curr) + w_euc_cost*euc_cost(curr, neigh) + w_traversal_cost*(costmap_cost(curr,neigh)/LETHAL_COST)^2 -h(neigh) = w_heuristic_cost * euc_cost(neigh, par) +h(neigh) = w_heuristic_cost * euc_cost(neigh, goal) f(neigh) = g(neigh) + h(neigh) ``` Because of how the program works when the 'neigh' node is to be expanded, depending From 3c1bb0d32653a20bee356d119beffced5386bbfd Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Fri, 16 Apr 2021 11:28:55 +0530 Subject: [PATCH 47/81] Add test coverage - added tests to check the algorithm itself and its helper functions - added smoke test to detect plugin level issues - inlined some functions - shifted the functions `setStartAndGoal()` and `isSafeToPlan()` to the ThetaStar class - removed the functions `dist()` and `getCellCost()` from the ThetaStar class --- nav2_theta_star_planner/CMakeLists.txt | 7 +- nav2_theta_star_planner/README.md | 48 +++--- .../nav2_theta_star_planner/theta_star.hpp | 111 ++++++------- .../theta_star_planner.hpp | 40 ++--- nav2_theta_star_planner/src/theta_star.cpp | 21 ++- .../src/theta_star_planner.cpp | 14 +- .../test/test_theta_star.cpp | 153 ++++++++++++++++++ 7 files changed, 270 insertions(+), 124 deletions(-) create mode 100644 nav2_theta_star_planner/test/test_theta_star.cpp diff --git a/nav2_theta_star_planner/CMakeLists.txt b/nav2_theta_star_planner/CMakeLists.txt index 27bc8ad3c1..6eabb292c2 100644 --- a/nav2_theta_star_planner/CMakeLists.txt +++ b/nav2_theta_star_planner/CMakeLists.txt @@ -15,7 +15,8 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(tf2_ros REQUIRED) find_package(ament_cmake_gtest REQUIRED) -nav2_package() +nav2_package() #Calls the nav2_package.cmake file +add_compile_options(-O3) include_directories( include @@ -66,7 +67,11 @@ install(FILES theta_star_planner.xml if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) + set(gtest_disable_pthreads OFF) ament_lint_auto_find_test_dependencies() + ament_add_gtest(test_theta_star test/test_theta_star.cpp) + ament_target_dependencies(test_theta_star ${dependencies}) + target_link_libraries(test_theta_star ${library_name}) endif() diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index ec76549810..f6447618ee 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -1,52 +1,51 @@ -# Nav2 Theta Star Planner +#Nav2 Theta Star Planner The Theta Star Planner is a global planning plugin meant to be used with the Nav2 Planner Server. The `nav2_theta_star_planner` implements a highly optimized version of the Theta Star Planner (specifically the [Lazy Theta\* P variant](http://idm-lab.org/bib/abstracts/papers/aaai10b.pdf)) meant to plan any-angle paths using A\*. The planner supports differential-drive and omni-directional robots. -## Features +##Features - Uses the costs from the costmap to penalise high cost regions - Is well suited for smaller robots of the omni-directional and differential drive kind - As it considers the costmap traversal cost during execution it tends to smoothen the paths automatically, thus mitigating the need to smoothen the path. (The presence of sharp turns depends on the resolution of the map, and it decreases as the map resolution increases.) - Allows to control the path behavior to either be any angle directed or to be in the middle of the spaces - The planner uses A\* search along with line of sight checks to form any-angle paths thus avoiding zig-zag paths that may be present in the usual implementation of A\* -## Metrics -For the below example the planner took ~44ms to compute the path of 87.5m - - -![example](img/00-37.png) +##Metrics +For the below example the planner took ~46ms (averaged value) to compute the path of 87.5m - +![example.png](/img/00-37.png) The parameters were set to - `w_euc_cost: 1.0`, `w_traversal_cost: 5.0`, `w_heuristic_cost: 1.0` and the `global_costmap`'s `inflation_layer` parameters are set as - `cost_scaling_factor:5.0`, `inflation_radius: 5.5` -## Cost Function Details -### Symbols and their meanings -**g(a)** - cost function cost for the node 'a' +##Cost Function Details +###Symbols and their meanings +**g(a)** - cost function cost for the init_rclcpp 'a' -**h(a)** - heuristic function cost for the node 'a' +**h(a)** - heuristic function cost for the init_rclcpp 'a' -**f(a)** - total cost (g(a) + h(a)) for the node 'a' +**f(a)** - total cost (g(a) + h(a)) for the init_rclcpp 'a' **LETHAL_COST** - a value of the costmap traversal cost that inscribes an obstacle with respect to a function, value = 253 -**curr** - represents the node whose neighbours are being added to the list +**curr** - represents the init_rclcpp whose neighbours are being added to the list **neigh** - one of the neighboring nodes of curr -**par** - parent node of curr +**par** - parent init_rclcpp of curr -**euc_cost(a,b)** - euclidean distance between the node type 'a' and type 'b' +**euc_cost(a,b)** - euclidean distance between the init_rclcpp type 'a' and type 'b' -**costmap_cost(a,b)** - the costmap traversal cost (ranges from 0 - 252, 254 = unknown value) between the node type 'a' and type 'b' +**costmap_cost(a,b)** - the costmap traversal cost (ranges from 0 - 252, 254 = unknown value) between the init_rclcpp type 'a' and type 'b' ### Cost function ``` g(neigh) = g(curr) + w_euc_cost*euc_cost(curr, neigh) + w_traversal_cost*(costmap_cost(curr,neigh)/LETHAL_COST)^2 -h(neigh) = w_heuristic_cost * euc_cost(neigh, goal) +h(neigh) = w_heuristic_cost * euc_cost(neigh, par) f(neigh) = g(neigh) + h(neigh) ``` -Because of how the program works when the 'neigh' node is to be expanded, depending +Because of how the program works when the 'neigh' init_rclcpp is to be expanded, depending on the result of the LOS check, (if the LOS check returns true) the value of g(neigh) might change to `g(par) + w1*euc_cost(par, neigh) + w2*(costmap(par,neigh)/LETHAL_COST)^2` -## Parameters +##Parameters The parameters of the planner are : - ` .how_many_corners ` : to choose between 4-connected and 8-connected graph expansions, the accepted values are 4 and 8 - ` .w_euc_cost ` : weight applied on the length of the path. @@ -68,10 +67,10 @@ planner_server: ``` Do note that the `global_costmap`'s `inflation_layer` values recommended for the above values of the parameter are - `cost_scaling_factor:1.0`, `inflation_radius: 5.5` -## Usage Notes +##Usage Notes -### Tuning the Parameters -Before starting off, do note that the costmap_cost(curr,neigh) component after being operated (before being multiplied to its parameter and being substituted in g(node)) varies from 0 to 1. Keep this in mind while tuning. +###Tuning the Parameters +Before starting off, do note that the costmap_cost(curr,neigh) component after being operated (before being multiplied to its parameter and being substituted in g(init_rclcpp)) varies from 0 to 1. Keep this in mind while tuning. This planner uses the costs associated with each cell from the `global_costmap` as a measure of the point's proximity to the obstacles. Providing a gentle potential field that covers the entirety of the region (with only small pocket like regions of cost = 0) is recommended in order to achieve paths that pass through the middle of the spaces. A good starting point could be to set the `inflation_layer`'s parameters as - `cost_scaling_factor:10.0`, `inflation_radius: 5.5` and then to decrease the value of `cost_scaling_factor` to achieve the said potential field. @@ -79,9 +78,14 @@ Providing a gentle potential field over the region allows the planner to compens In order to achieve paths that stay in the middle of the spaces set `w_traversal_cost` at a higher value than `w_euc_cost`. To begin with, you can set the parameters to its default values and then increase the value of `w_traversal_cost`, while also decreasing `w_euc_cost` to allow for an increase in the path length thus letting the path to settle in the middle regions of the spaces with lower costs on the costmap. While tuning the planner's parameters you can also change the `inflation_layer`'s parameters to tune the behavior of the paths. -### Path Smoothing +###Path Smoothing Because of how the cost function works, the output path has a natural tendency to form smooth curves around corners, though the smoothness of the path depends on how wide the turn is and the number of cells in that turns. This planner is recommended to be used with local planners like DWB or TEB (or other any planner / controllers that form a local trajectory to be traversed) as these take into account the abrupt turns which might arise due to the planner not being able to find a smoother turns owing to the aforementioned reasons. While smoother paths can be achieved by increasing the costmap resolution (ie using a costmap of 1cm resolution rather than a 5cm one) it is not recommended to do so as it comes at the cost of increased query times from the planner. Test the planners performance on the finer costmaps before making a switch to those costmaps. + + + + + diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp index 03e4868db9..eb9f6e7fed 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp @@ -68,11 +68,11 @@ class ThetaStar coordsM src_{}, dst_{}; nav2_costmap_2d::Costmap2D * costmap_{}; - /// weight for the costmap traversal cost + /// weight on the costmap traversal cost double w_traversal_cost_; - /// weight for the euclidean distance cost (as of now used for calculations of g_cost) + /// weight on the euclidean distance cost (as of now used for calculations of g_cost) double w_euc_cost_; - /// weight for the heuristic cost (for h_cost calculations) + /// weight on the heuristic cost (for h_cost calculations) double w_heuristic_cost_; /// parameter to set the number of adjacent nodes to be searched on int how_many_corners_; @@ -81,34 +81,50 @@ class ThetaStar ThetaStar(); -// ~ThetaStar() = default; + ~ThetaStar() = default; /** - * @brief the function that iteratively searches upon the nodes in the queue (open list) until the - * current node is the goal pose or if size of queue > 0 - * @param raw_path is used to return the path obtained on executing the algorithm - * @return true if a path is found, false if no path is found between the start and goal pose + * @brief it iteratively searches upon the nodes in the queue (open list) until the + * current node is the goal pose or the size of queue becomes 0 + * @param raw_path is used to return the path obtained by executing the algorithm + * @return true if a path is found, false if no path is found in between the start and goal pose */ bool generatePath(std::vector & raw_path); /** - * @brief This function checks whether the cost of a point(cx, cy) on the costmap is less than the LETHAL_COST + * @brief this function checks whether the cost of a point(cx, cy) on the costmap is less than the LETHAL_COST * @return the result of the check */ - bool isSafe(const int & cx, const int & cy) const + inline bool isSafe(const int & cx, const int & cy) const { return costmap_->getCost(cx, cy) < LETHAL_COST; } + /** + * @brief initialises the values of the start and goal points +*/ + void setStartAndGoal( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal); + + /** + * @brief checks whether the start and goal points have costmap costs lower than LETHAL_COST + * @return the opposite result of the check + */ + bool isSafeToPlan() const + { + return !(isSafe(src_.x, src_.y)) || !(isSafe(dst_.x, dst_.y)); + } + protected: /// for the coordinates (x,y), it stores at node_position_[size_x_ * y + x], /// the index at which the data of the node is present in nodes_data_ /// it is initialised with size_x_ * size_y_ elements - /// and its number of elements would increase to accommodate for a change in map size + /// and its number of elements increases to account for a change in map size std::vector node_position_; /// the vector nodes_data_ stores the coordinates, costs and index of the parent node, - /// and whether or not the node is present in queue_ + /// and whether or not the node is present in queue_, for all the nodes searched /// it is initialised with no elements /// and its size increases depending on the number of nodes searched std::vector nodes_data_; @@ -132,7 +148,7 @@ class ThetaStar tree_node * curr_node; - /** @brief it does a line of sight (los) check between the current node and the parent of its parent node + /** @brief it performs a line of sight (los) check between the current node and the parent node of its parent node * if an los is found and the new costs calculated are lesser then the cost and parent node * of the current node is updated * @param data of the current node @@ -146,13 +162,6 @@ class ThetaStar */ void setNeighbors(const tree_node & curr_data, const int & curr_int); - /** - * @brief it returns the path by backtracking from the goal to the start, by using their parent nodes - * @param raw_points used to return the path thus found - * @param curr_id sends in the index of the goal coordinate, as stored in nodes_position - */ - void backtrace(std::vector & raw_points, int curr_id); - /** * @brief performs the line of sight check using Bresenham's Algorithm, * and has been modified to calculate the traversal cost incurred in a straight line path between @@ -163,27 +172,36 @@ class ThetaStar bool losCheck(const int & x0, const int & y0, const int & x1, const int & y1, double & sl_cost); /** - * @brief calculates the euclidean distance between the points (ax, ay) and (bx, by) - * @return the distance thus calculated - */ - double dist(const int & ax, const int & ay, const int & bx, const int & by) +* @brief it is an overloaded function to ease the cost calculations while performing the LOS check +* @param cost denotes the total straight line traversal cost, adds the traversal cost for the node (cx, cy) at every instance, it is also being returned +* @return false if the traversal cost is greater than / equal to the LETHAL_COST and true otherwise +*/ + bool isSafe(const int & cx, const int & cy, double & cost) const { - return std::hypot(ax - bx, ay - by); + double curr_cost = costmap_->getCost(cx, cy); + if (curr_cost < LETHAL_COST) { + cost += w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST; + return true; + } else { + return false; + } } + /** + * @brief it returns the path by backtracking from the goal to the start, by using their parent nodes + * @param raw_points used to return the path thus found + * @param curr_id sends in the index of the goal coordinate, as stored in nodes_position + */ + void backtrace(std::vector & raw_points, int curr_id); + /** * @brief calculates the piecewise straight line euclidean distances by * * * @return the distance thus calculated */ - double getEuclideanCost(const int & ax, const int & ay, const int & bx, const int & by) - { - return w_euc_cost_ * dist(ax, ay, bx, by); - } - - double getCellCost(const int & cx, const int & cy) const + inline double getEuclideanCost(const int & ax, const int & ay, const int & bx, const int & by) { - return 50 + 0.8 * costmap_->getCost(cx, cy); + return w_euc_cost_ * std::hypot(ax - bx, ay - by); } /** @@ -191,9 +209,9 @@ class ThetaStar * *()^2/()^2 * @return the traversal cost thus calculated */ - double getTraversalCost(const int & cx, const int & cy) + inline double getTraversalCost(const int & cx, const int & cy) { - double curr_cost = getCellCost(cx, cy); + double curr_cost = costmap_->getCost(cx, cy); return w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST; } @@ -202,40 +220,25 @@ class ThetaStar * * * @return the heuristic cost */ - double getHCost(const int & cx, const int & cy) + inline double getHCost(const int & cx, const int & cy) { - return w_heuristic_cost_ * dist(cx, cy, dst_.x, dst_.y); - } - - /** - * @brief it is an overloaded function to ease in cost calculations while performing the LOS check - * @param cost denotes the total straight line traversal cost, adds the traversal cost for the node (cx, cy) at every instance, it is also being returned - * @return false if the traversal cost is greater than / equal to the LETHAL_COST and true otherwise - */ - bool isSafe(const int & cx, const int & cy, double & cost) const - { - double curr_cost = getCellCost(cx, cy); - if (curr_cost < LETHAL_COST) { - cost += w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST; - return true; - } else { - return false; - } + return w_heuristic_cost_ * std::hypot(cx - dst_.x, cy - dst_.y); } /** * @brief checks if the given coordinates(cx, cy) lies within the map * @return the result of the check */ - bool withinLimits(const int & cx, const int & cy) const + inline bool withinLimits(const int & cx, const int & cy) const { return cx >= 0 && cx < size_x_ && cy >= 0 && cy < size_y_; } + /** * @brief checks if the coordinates of a node is the goal or not * @return the result of the check */ - bool isGoal(const tree_node & this_node) const + inline bool isGoal(const tree_node & this_node) const { return this_node.x == dst_.x && this_node.y == dst_.y; } diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp index 4372116373..efc7eb1b13 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp @@ -54,27 +54,6 @@ class ThetaStarPlanner : public nav2_core::GlobalPlanner const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) override; -protected: - std::shared_ptr tf_; - rclcpp::Clock::SharedPtr clock_; - rclcpp::Logger logger_{rclcpp::get_logger("ThetaStarPlanner")}; - std::string global_frame_, name_; - - std::unique_ptr planner_; - - /** - * @brief the function responsible for calling the planner and retrieving a path from it - * @return global_path is the planned path to be taken - */ - void getPlan(nav_msgs::msg::Path & global_path); - - /** - * @brief initialises the values of the start and goal points in the object of the planner - */ - void setStartAndGoal( - const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal); - /** * @brief interpolates points between the adjacent waypoints of the path * @param raw_path is used to send in the path received from the planner @@ -85,16 +64,19 @@ class ThetaStarPlanner : public nav2_core::GlobalPlanner const std::vector & raw_path, const double & dist_bw_points); +protected: + std::shared_ptr tf_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_{rclcpp::get_logger("ThetaStarPlanner")}; + std::string global_frame_, name_; + + std::unique_ptr planner_; + /** - * @brief checks whether the start and goal points have costmap costs lower than LETHAL_COST - * @return the result of the check + * @brief the function responsible for calling the algorithm and retrieving a path from it + * @return global_path is the planned path to be taken */ - bool isSafeToPlan() const - { - return !(planner_->isSafe( - planner_->src_.x, - planner_->src_.y)) || !(planner_->isSafe(planner_->dst_.x, planner_->dst_.y)); - } + void getPlan(nav_msgs::msg::Path & global_path); }; } // namespace nav2_theta_star_planner diff --git a/nav2_theta_star_planner/src/theta_star.cpp b/nav2_theta_star_planner/src/theta_star.cpp index 5ad263077f..d3f6cac1df 100644 --- a/nav2_theta_star_planner/src/theta_star.cpp +++ b/nav2_theta_star_planner/src/theta_star.cpp @@ -19,7 +19,7 @@ namespace theta_star { ThetaStar::ThetaStar() -: w_traversal_cost_(4.0), +: w_traversal_cost_(2.0), w_euc_cost_(7.0), w_heuristic_cost_(1.0), how_many_corners_(8), @@ -30,6 +30,18 @@ ThetaStar::ThetaStar() curr_node = new tree_node; } +void ThetaStar::setStartAndGoal( + const geometry_msgs::msg::PoseStamped & start, + const geometry_msgs::msg::PoseStamped & goal) +{ + unsigned int s[2], d[2]; + costmap_->worldToMap(start.pose.position.x, start.pose.position.y, s[0], s[1]); + costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, d[0], d[1]); + + src_ = {static_cast(s[0]), static_cast(s[1])}; + dst_ = {static_cast(d[0]), static_cast(d[1])}; +} + bool ThetaStar::generatePath(std::vector & raw_path) { resetContainers(); @@ -108,9 +120,8 @@ void ThetaStar::setNeighbors(const tree_node & curr_data, const int & curr_id) } m_par = curr_id; - g_cost = curr_data.g + getEuclideanCost(curr_data.x, curr_data.y, mx, my) + getTraversalCost( - mx, - my); + g_cost = curr_data.g + getEuclideanCost(curr_data.x, curr_data.y, mx, my) + + getTraversalCost(mx, my); getIndex(mx, my, m_id); @@ -125,7 +136,6 @@ void ThetaStar::setNeighbors(const tree_node & curr_data, const int & curr_id) h_cost = getHCost(mx, my); cal_cost = g_cost + h_cost; - if (curr_node->f > cal_cost) { curr_node->g = g_cost; curr_node->h = h_cost; @@ -254,5 +264,4 @@ void ThetaStar::initializePosn(int size_inc) node_position_.emplace_back(-1); } } - } // namespace theta_star diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index 3fcfaad1fd..ea3b65602c 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -78,7 +78,7 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( { nav_msgs::msg::Path global_path; auto start_time = std::chrono::steady_clock::now(); - setStartAndGoal(start, goal); + planner_->setStartAndGoal(start, goal); RCLCPP_DEBUG( logger_, "Got the src and dst... (%i, %i) && (%i, %i)", planner_->src_.x, planner_->src_.y, planner_->dst_.x, planner_->dst_.y); @@ -93,7 +93,7 @@ void ThetaStarPlanner::getPlan(nav_msgs::msg::Path & global_path) { std::vector path; - if (isSafeToPlan()) { + if (planner_->isSafeToPlan()) { RCLCPP_ERROR(logger_, "Either of the start or goal pose are an obstacle! "); coordsW world{}; planner_->costmap_->mapToWorld(planner_->src_.x, planner_->src_.y, world.x, world.y); @@ -140,17 +140,7 @@ nav_msgs::msg::Path ThetaStarPlanner::linearInterpolation( } return pa; } -void ThetaStarPlanner::setStartAndGoal( - const geometry_msgs::msg::PoseStamped & start, - const geometry_msgs::msg::PoseStamped & goal) -{ - unsigned int src_[2], dst_[2]; - planner_->costmap_->worldToMap(start.pose.position.x, start.pose.position.y, src_[0], src_[1]); - planner_->costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, dst_[0], dst_[1]); - planner_->src_ = {static_cast(src_[0]), static_cast(src_[1])}; - planner_->dst_ = {static_cast(dst_[0]), static_cast(dst_[1])}; -} } // namespace nav2_theta_star_planner diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp new file mode 100644 index 0000000000..42a6c6b512 --- /dev/null +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -0,0 +1,153 @@ +// Copyright 2020 Anshumaan Singh +// +// 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. + +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "nav2_theta_star_planner/theta_star.hpp" +#include "nav2_theta_star_planner/theta_star_planner.hpp" + +class init_rclcpp +{ +public: + init_rclcpp() {rclcpp::init(0, nullptr);} + ~init_rclcpp() {rclcpp::shutdown();} +}; + +/// class created to access the protected members of the ThetaStar class +/// u is used as shorthand for use +class test_theta_star : public theta_star::ThetaStar +{ +public: + int getSizeOfNodePosition() + { + return static_cast(node_position_.size()); + } + + bool ulosCheck(const int & x0, const int & y0, const int & x1, const int & y1, double & sl_cost) + { + return losCheck(x0, y0, x1, y1, sl_cost); + } + + bool uwithinLimits(const int & cx, const int & cy) {return withinLimits(cx, cy);} + + bool uisGoal(const tree_node & this_node) {return isGoal(this_node);} + + void uinitializePosn(int size_inc = 0) {initializePosn(size_inc);} + + void uaddIndex(const int & cx, const int & cy, const int & id_this) {addIndex(cx, cy, id_this);} + + void ugetIndex(const int & cx, const int & cy, int & id_this) {getIndex(cx, cy, id_this);} + + bool runAlgo(std::vector & path) + { + if (!isSafeToPlan()) { + return generatePath(path); + } + return false; + } +}; + +init_rclcpp node; +auto planner_ = std::make_unique(); + +// Tests meant to test the algorithm itself and its helper functions +TEST(ThetaStarTest, test_theta_star) +{ + planner_->costmap_ = new nav2_costmap_2d::Costmap2D(50, 50, 1.0, 0.0, 0.0, 0); + for (int i = 20; i <= 30; i++) { + for (int j = 20; j <= 30; j++) { + planner_->costmap_->setCost(i, j, 253); + } + } + + planner_->size_x_ = 50; + planner_->size_y_ = 50; + planner_->src_ = {10, 10}; + planner_->dst_ = {40, 35}; + + /// Check if the initializePosn function works properly + planner_->uinitializePosn(50 * 50); + EXPECT_EQ(planner_->getSizeOfNodePosition(), (50 * 50)) << "passed this 1"; + + /// Check if the withinLimits function works properly + EXPECT_TRUE(planner_->uwithinLimits(20, 40)); + EXPECT_FALSE(planner_->uwithinLimits(120, 140)); + + tree_node n = {40, 35, 120, 0, 1, false, 20}; + /// Check if the isGoal function works properly + EXPECT_TRUE(planner_->uisGoal(n)); // both (x,y) are the goal coordinates + n.x = 25; + EXPECT_FALSE(planner_->uisGoal(n)); // only y coordinate matches with that of goal + n.x = 40; + n.y = 20; + EXPECT_FALSE(planner_->uisGoal(n)); // only x coordinate matches with that of goal + n.x = 30; + EXPECT_FALSE(planner_->uisGoal(n)); // both (x, y) are different from the goal coordinate + + /// Check if the isSafe functions work properly + EXPECT_TRUE(planner_->isSafe(10, 30)); // cost at this point is 0 + EXPECT_FALSE(planner_->isSafe(25, 25)); // cost at this point is 253 (>LETHAL_COST) + + int c_id; + /// Check if the addIndex & getIndex work properly + coordsM c = {20, 30}; + planner_->uaddIndex(c.x, c.y, 0); + planner_->ugetIndex(c.x, c.y, c_id); + EXPECT_EQ(c_id, 0); + + double sl_cost = 0.0; + /// Checking for the case where the losCheck should return the value as true + EXPECT_TRUE(planner_->ulosCheck(10, 10, 40, 10, sl_cost)); + /// and as false + EXPECT_FALSE(planner_->ulosCheck(10, 25, 30, 25, sl_cost)); + + std::vector path; + /// Check if the planner returns a path for the case where a path exists + EXPECT_TRUE(planner_->runAlgo(path)); + /// and where it doesn't exist + planner_->src_ = {25, 25}; + EXPECT_FALSE(planner_->runAlgo(path)); +} + +// Smoke tests meant to detect issues arising from the plugin part rather than the algorithm +TEST(ThetaStarPlanner, test_theta_star_planner) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr life_node = + std::make_shared("ThetaStarPlannerTest"); + + std::shared_ptr costmap_ros = + std::make_shared("global_costmap"); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + geometry_msgs::msg::PoseStamped start, goal; + start.pose.position.x = 0.0; + start.pose.position.y = 0.0; + start.pose.orientation.w = 1.0; + goal = start; + auto planner_2d = std::make_unique(); + planner_2d->configure(life_node, "test", nullptr, costmap_ros); + planner_2d->activate(); + + nav_msgs::msg::Path path = planner_2d->createPlan(start, goal); + + planner_2d->deactivate(); + planner_2d->cleanup(); + + planner_2d.reset(); + costmap_ros->on_cleanup(rclcpp_lifecycle::State()); + life_node.reset(); + costmap_ros.reset(); +} From e15ce175c021cebc485fb26b1db59fa9da47e84c Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Fri, 16 Apr 2021 11:49:49 +0530 Subject: [PATCH 48/81] update the test file --- nav2_theta_star_planner/test/test_theta_star.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp index 42a6c6b512..b08052cd84 100644 --- a/nav2_theta_star_planner/test/test_theta_star.cpp +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -75,8 +75,17 @@ TEST(ThetaStarTest, test_theta_star) planner_->size_x_ = 50; planner_->size_y_ = 50; - planner_->src_ = {10, 10}; - planner_->dst_ = {40, 35}; + geometry_msgs::msg::PoseStamped start, goal; + start.pose.position.x = 10; + start.pose.position.y = 10; + start.pose.orientation.w = 1.0; + goal.pose.position.x = 40; + goal.pose.position.y = 35; + goal.pose.orientation.w = 1.0; + /// Check if the setStartAndGoal function works properly + planner_->setStartAndGoal(start, goal); + EXPECT_TRUE(planner_->src_.x == 10 && planner_->src_.y == 10); + EXPECT_TRUE(planner_->dst_.x == 40 && planner_->dst_.y == 35); /// Check if the initializePosn function works properly planner_->uinitializePosn(50 * 50); From 2f9d63fa86f20de6d5a1c1027d88b392444d6100 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Fri, 16 Apr 2021 16:25:16 +0530 Subject: [PATCH 49/81] update the test file --- nav2_theta_star_planner/test/test_theta_star.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp index b08052cd84..1ad6bbaf27 100644 --- a/nav2_theta_star_planner/test/test_theta_star.cpp +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -151,6 +151,7 @@ TEST(ThetaStarPlanner, test_theta_star_planner) planner_2d->activate(); nav_msgs::msg::Path path = planner_2d->createPlan(start, goal); + EXPECT_GT(static_cast(path.poses.size()), 0); planner_2d->deactivate(); planner_2d->cleanup(); From 81bd2abc704de928a7481ddf0de5c6040e98ddb6 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Sat, 17 Apr 2021 00:01:44 +0530 Subject: [PATCH 50/81] Update README.md --- nav2_theta_star_planner/README.md | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index f6447618ee..992d81b151 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -1,14 +1,14 @@ -#Nav2 Theta Star Planner +# Nav2 Theta Star Planner The Theta Star Planner is a global planning plugin meant to be used with the Nav2 Planner Server. The `nav2_theta_star_planner` implements a highly optimized version of the Theta Star Planner (specifically the [Lazy Theta\* P variant](http://idm-lab.org/bib/abstracts/papers/aaai10b.pdf)) meant to plan any-angle paths using A\*. The planner supports differential-drive and omni-directional robots. -##Features +## Features - Uses the costs from the costmap to penalise high cost regions - Is well suited for smaller robots of the omni-directional and differential drive kind - As it considers the costmap traversal cost during execution it tends to smoothen the paths automatically, thus mitigating the need to smoothen the path. (The presence of sharp turns depends on the resolution of the map, and it decreases as the map resolution increases.) - Allows to control the path behavior to either be any angle directed or to be in the middle of the spaces - The planner uses A\* search along with line of sight checks to form any-angle paths thus avoiding zig-zag paths that may be present in the usual implementation of A\* -##Metrics +## Metrics For the below example the planner took ~46ms (averaged value) to compute the path of 87.5m - ![example.png](/img/00-37.png) @@ -38,14 +38,14 @@ respect to a function, value = 253 ### Cost function ``` g(neigh) = g(curr) + w_euc_cost*euc_cost(curr, neigh) + w_traversal_cost*(costmap_cost(curr,neigh)/LETHAL_COST)^2 -h(neigh) = w_heuristic_cost * euc_cost(neigh, par) +h(neigh) = w_heuristic_cost * euc_cost(neigh, goal) f(neigh) = g(neigh) + h(neigh) ``` Because of how the program works when the 'neigh' init_rclcpp is to be expanded, depending on the result of the LOS check, (if the LOS check returns true) the value of g(neigh) might change to `g(par) + w1*euc_cost(par, neigh) + w2*(costmap(par,neigh)/LETHAL_COST)^2` -##Parameters +## Parameters The parameters of the planner are : - ` .how_many_corners ` : to choose between 4-connected and 8-connected graph expansions, the accepted values are 4 and 8 - ` .w_euc_cost ` : weight applied on the length of the path. @@ -67,9 +67,9 @@ planner_server: ``` Do note that the `global_costmap`'s `inflation_layer` values recommended for the above values of the parameter are - `cost_scaling_factor:1.0`, `inflation_radius: 5.5` -##Usage Notes +## Usage Notes -###Tuning the Parameters +### Tuning the Parameters Before starting off, do note that the costmap_cost(curr,neigh) component after being operated (before being multiplied to its parameter and being substituted in g(init_rclcpp)) varies from 0 to 1. Keep this in mind while tuning. This planner uses the costs associated with each cell from the `global_costmap` as a measure of the point's proximity to the obstacles. Providing a gentle potential field that covers the entirety of the region (with only small pocket like regions of cost = 0) is recommended in order to achieve paths that pass through the middle of the spaces. A good starting point could be to set the `inflation_layer`'s parameters as - `cost_scaling_factor:10.0`, `inflation_radius: 5.5` and then to decrease the value of `cost_scaling_factor` to achieve the said potential field. @@ -78,7 +78,7 @@ Providing a gentle potential field over the region allows the planner to compens In order to achieve paths that stay in the middle of the spaces set `w_traversal_cost` at a higher value than `w_euc_cost`. To begin with, you can set the parameters to its default values and then increase the value of `w_traversal_cost`, while also decreasing `w_euc_cost` to allow for an increase in the path length thus letting the path to settle in the middle regions of the spaces with lower costs on the costmap. While tuning the planner's parameters you can also change the `inflation_layer`'s parameters to tune the behavior of the paths. -###Path Smoothing +### Path Smoothing Because of how the cost function works, the output path has a natural tendency to form smooth curves around corners, though the smoothness of the path depends on how wide the turn is and the number of cells in that turns. This planner is recommended to be used with local planners like DWB or TEB (or other any planner / controllers that form a local trajectory to be traversed) as these take into account the abrupt turns which might arise due to the planner not being able to find a smoother turns owing to the aforementioned reasons. From 727e60eee3a78a94a8b4736c15e7732390a9147e Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Sat, 17 Apr 2021 00:03:13 +0530 Subject: [PATCH 51/81] Update README.md --- nav2_theta_star_planner/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index 992d81b151..c3bf2fb02f 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -10,7 +10,7 @@ The Theta Star Planner is a global planning plugin meant to be used with the Nav ## Metrics For the below example the planner took ~46ms (averaged value) to compute the path of 87.5m - -![example.png](/img/00-37.png) +![example.png](img/00-37.png) The parameters were set to - `w_euc_cost: 1.0`, `w_traversal_cost: 5.0`, `w_heuristic_cost: 1.0` and the `global_costmap`'s `inflation_layer` parameters are set as - `cost_scaling_factor:5.0`, `inflation_radius: 5.5` From 2830e544ce6556ca2a7b5870b960642e6e11e0d1 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Sat, 17 Apr 2021 00:04:41 +0530 Subject: [PATCH 52/81] Update README.md --- nav2_theta_star_planner/README.md | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index c3bf2fb02f..c674957fd3 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -16,24 +16,24 @@ The parameters were set to - `w_euc_cost: 1.0`, `w_traversal_cost: 5.0`, `w_heur ##Cost Function Details ###Symbols and their meanings -**g(a)** - cost function cost for the init_rclcpp 'a' +**g(a)** - cost function cost for the node 'a' -**h(a)** - heuristic function cost for the init_rclcpp 'a' +**h(a)** - heuristic function cost for the node 'a' -**f(a)** - total cost (g(a) + h(a)) for the init_rclcpp 'a' +**f(a)** - total cost (g(a) + h(a)) for the node 'a' **LETHAL_COST** - a value of the costmap traversal cost that inscribes an obstacle with respect to a function, value = 253 -**curr** - represents the init_rclcpp whose neighbours are being added to the list +**curr** - represents the node whose neighbours are being added to the list **neigh** - one of the neighboring nodes of curr -**par** - parent init_rclcpp of curr +**par** - parent node of curr -**euc_cost(a,b)** - euclidean distance between the init_rclcpp type 'a' and type 'b' +**euc_cost(a,b)** - euclidean distance between the node type 'a' and type 'b' -**costmap_cost(a,b)** - the costmap traversal cost (ranges from 0 - 252, 254 = unknown value) between the init_rclcpp type 'a' and type 'b' +**costmap_cost(a,b)** - the costmap traversal cost (ranges from 0 - 252, 254 = unknown value) between the node type 'a' and type 'b' ### Cost function ``` From e72f401219fc52d362e1c9dc5a2439b714a454d4 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Sat, 17 Apr 2021 00:12:39 +0530 Subject: [PATCH 53/81] add test on the size of the output path --- nav2_theta_star_planner/test/test_theta_star.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp index 1ad6bbaf27..c41e97722b 100644 --- a/nav2_theta_star_planner/test/test_theta_star.cpp +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -126,9 +126,12 @@ TEST(ThetaStarTest, test_theta_star) std::vector path; /// Check if the planner returns a path for the case where a path exists EXPECT_TRUE(planner_->runAlgo(path)); + EXPECT_GT(static_cast(path.size()), 0); /// and where it doesn't exist + path.clear(); planner_->src_ = {25, 25}; EXPECT_FALSE(planner_->runAlgo(path)); + EXPECT_EQ(static_cast(path.size()), 0); } // Smoke tests meant to detect issues arising from the plugin part rather than the algorithm From d6eb872b0008c88e6fbd6acd9016b44d35a2bec8 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Sat, 17 Apr 2021 00:17:42 +0530 Subject: [PATCH 54/81] Update README.md --- nav2_theta_star_planner/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index c674957fd3..45ea444171 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -14,8 +14,8 @@ For the below example the planner took ~46ms (averaged value) to compute the pat The parameters were set to - `w_euc_cost: 1.0`, `w_traversal_cost: 5.0`, `w_heuristic_cost: 1.0` and the `global_costmap`'s `inflation_layer` parameters are set as - `cost_scaling_factor:5.0`, `inflation_radius: 5.5` -##Cost Function Details -###Symbols and their meanings +## Cost Function Details +### Symbols and their meanings **g(a)** - cost function cost for the node 'a' **h(a)** - heuristic function cost for the node 'a' From ae430d6c37156d60df239a1547e229625fa01376 Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Sat, 17 Apr 2021 00:39:01 +0530 Subject: [PATCH 55/81] Update README.md --- nav2_theta_star_planner/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index 45ea444171..685429c3a8 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -41,7 +41,7 @@ g(neigh) = g(curr) + w_euc_cost*euc_cost(curr, neigh) + w_traversal_cost*(costma h(neigh) = w_heuristic_cost * euc_cost(neigh, goal) f(neigh) = g(neigh) + h(neigh) ``` -Because of how the program works when the 'neigh' init_rclcpp is to be expanded, depending +Because of how the program works when the 'neigh' node is to be expanded, depending on the result of the LOS check, (if the LOS check returns true) the value of g(neigh) might change to `g(par) + w1*euc_cost(par, neigh) + w2*(costmap(par,neigh)/LETHAL_COST)^2` @@ -70,7 +70,7 @@ Do note that the `global_costmap`'s `inflation_layer` values recommended for the ## Usage Notes ### Tuning the Parameters -Before starting off, do note that the costmap_cost(curr,neigh) component after being operated (before being multiplied to its parameter and being substituted in g(init_rclcpp)) varies from 0 to 1. Keep this in mind while tuning. +Before starting off, do note that the costmap_cost(curr,neigh) component after being operated (before being multiplied to its parameter and being substituted in g(node)) varies from 0 to 1. Keep this in mind while tuning. This planner uses the costs associated with each cell from the `global_costmap` as a measure of the point's proximity to the obstacles. Providing a gentle potential field that covers the entirety of the region (with only small pocket like regions of cost = 0) is recommended in order to achieve paths that pass through the middle of the spaces. A good starting point could be to set the `inflation_layer`'s parameters as - `cost_scaling_factor:10.0`, `inflation_radius: 5.5` and then to decrease the value of `cost_scaling_factor` to achieve the said potential field. From 4ea8976fe2aafa68ba72a720dca0c494ac1f806b Mon Sep 17 00:00:00 2001 From: Anshu-man567 <56119116+Anshu-man567@users.noreply.github.com> Date: Sat, 17 Apr 2021 00:57:46 +0530 Subject: [PATCH 56/81] change the function name from `isSafeToPlan()` to `isUnsafeToPlan()` --- .../include/nav2_theta_star_planner/theta_star.hpp | 2 +- nav2_theta_star_planner/src/theta_star_planner.cpp | 8 +------- nav2_theta_star_planner/test/test_theta_star.cpp | 2 +- 3 files changed, 3 insertions(+), 9 deletions(-) diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp index eb9f6e7fed..2c796acc7d 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp @@ -111,7 +111,7 @@ class ThetaStar * @brief checks whether the start and goal points have costmap costs lower than LETHAL_COST * @return the opposite result of the check */ - bool isSafeToPlan() const + bool isUnsafeToPlan() const { return !(isSafe(src_.x, src_.y)) || !(isSafe(dst_.x, dst_.y)); } diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index ea3b65602c..65cf3a6b0b 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -93,14 +93,8 @@ void ThetaStarPlanner::getPlan(nav_msgs::msg::Path & global_path) { std::vector path; - if (planner_->isSafeToPlan()) { + if (planner_->isUnsafeToPlan()) { RCLCPP_ERROR(logger_, "Either of the start or goal pose are an obstacle! "); - coordsW world{}; - planner_->costmap_->mapToWorld(planner_->src_.x, planner_->src_.y, world.x, world.y); - path.push_back({world.x, world.y}); - planner_->costmap_->mapToWorld(planner_->dst_.x, planner_->dst_.y, world.x, world.y); - path.push_back({world.x, world.y}); - global_path = linearInterpolation(path, planner_->costmap_->getResolution()); global_path.poses.clear(); } else if (planner_->generatePath(path)) { global_path = linearInterpolation(path, planner_->costmap_->getResolution()); diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp index c41e97722b..f421b206fd 100644 --- a/nav2_theta_star_planner/test/test_theta_star.cpp +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -53,7 +53,7 @@ class test_theta_star : public theta_star::ThetaStar bool runAlgo(std::vector & path) { - if (!isSafeToPlan()) { + if (!isUnsafeToPlan()) { return generatePath(path); } return false; From 478a84341b67e754db8ddb9e87a0e0186770a062 Mon Sep 17 00:00:00 2001 From: Anshumaan Singh Date: Sun, 25 Apr 2021 12:55:30 +0530 Subject: [PATCH 57/81] update the files - inlined the functions `getIndex` and `addIndex` - removed typos from the comments --- nav2_theta_star_planner/README.md | 12 +++---- .../nav2_theta_star_planner/theta_star.hpp | 34 +++++++++---------- .../theta_star_planner.hpp | 20 +++++------ nav2_theta_star_planner/src/theta_star.cpp | 8 ++--- .../src/theta_star_planner.cpp | 11 +++--- 5 files changed, 40 insertions(+), 45 deletions(-) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index 685429c3a8..f20d0bdc54 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -10,6 +10,7 @@ The Theta Star Planner is a global planning plugin meant to be used with the Nav ## Metrics For the below example the planner took ~46ms (averaged value) to compute the path of 87.5m - + ![example.png](img/00-37.png) The parameters were set to - `w_euc_cost: 1.0`, `w_traversal_cost: 5.0`, `w_heuristic_cost: 1.0` and the `global_costmap`'s `inflation_layer` parameters are set as - `cost_scaling_factor:5.0`, `inflation_radius: 5.5` @@ -41,7 +42,7 @@ g(neigh) = g(curr) + w_euc_cost*euc_cost(curr, neigh) + w_traversal_cost*(costma h(neigh) = w_heuristic_cost * euc_cost(neigh, goal) f(neigh) = g(neigh) + h(neigh) ``` -Because of how the program works when the 'neigh' node is to be expanded, depending +Because of how the program works when the 'neigh' init_rclcpp is to be expanded, depending on the result of the LOS check, (if the LOS check returns true) the value of g(neigh) might change to `g(par) + w1*euc_cost(par, neigh) + w2*(costmap(par,neigh)/LETHAL_COST)^2` @@ -70,7 +71,7 @@ Do note that the `global_costmap`'s `inflation_layer` values recommended for the ## Usage Notes ### Tuning the Parameters -Before starting off, do note that the costmap_cost(curr,neigh) component after being operated (before being multiplied to its parameter and being substituted in g(node)) varies from 0 to 1. Keep this in mind while tuning. +Before starting off, do note that the costmap_cost(curr,neigh) component after being operated (before being multiplied to its parameter and being substituted in g(init_rclcpp)) varies from 0 to 1. Keep this in mind while tuning. This planner uses the costs associated with each cell from the `global_costmap` as a measure of the point's proximity to the obstacles. Providing a gentle potential field that covers the entirety of the region (with only small pocket like regions of cost = 0) is recommended in order to achieve paths that pass through the middle of the spaces. A good starting point could be to set the `inflation_layer`'s parameters as - `cost_scaling_factor:10.0`, `inflation_radius: 5.5` and then to decrease the value of `cost_scaling_factor` to achieve the said potential field. @@ -83,9 +84,4 @@ Because of how the cost function works, the output path has a natural tendency t This planner is recommended to be used with local planners like DWB or TEB (or other any planner / controllers that form a local trajectory to be traversed) as these take into account the abrupt turns which might arise due to the planner not being able to find a smoother turns owing to the aforementioned reasons. -While smoother paths can be achieved by increasing the costmap resolution (ie using a costmap of 1cm resolution rather than a 5cm one) it is not recommended to do so as it comes at the cost of increased query times from the planner. Test the planners performance on the finer costmaps before making a switch to those costmaps. - - - - - +While smoother paths can be achieved by increasing the costmap resolution (ie using a costmap of 1cm resolution rather than a 5cm one) it is not recommended to do so as it comes at the cost of increased query times from the planner. Test the planners performance on the finer costmaps before making a switch to those costmaps. diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp index 2c796acc7d..286cf26473 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp @@ -70,13 +70,13 @@ class ThetaStar /// weight on the costmap traversal cost double w_traversal_cost_; - /// weight on the euclidean distance cost (as of now used for calculations of g_cost) + /// weight on the euclidean distance cost (used for calculations of g_cost) double w_euc_cost_; - /// weight on the heuristic cost (for h_cost calculations) + /// weight on the heuristic cost (used for h_cost calculations) double w_heuristic_cost_; /// parameter to set the number of adjacent nodes to be searched on int how_many_corners_; - /// the x directional and y directional lengths of the map respectively + /// the x-directional and y-directional lengths of the map respectively int size_x_, size_y_; ThetaStar(); @@ -108,8 +108,8 @@ class ThetaStar const geometry_msgs::msg::PoseStamped & goal); /** - * @brief checks whether the start and goal points have costmap costs lower than LETHAL_COST - * @return the opposite result of the check + * @brief checks whether the start and goal points have costmap costs greater than LETHAL_COST + * @return true if the cost of any one of the points is greater than LETHAL_COST */ bool isUnsafeToPlan() const { @@ -129,7 +129,7 @@ class ThetaStar /// and its size increases depending on the number of nodes searched std::vector nodes_data_; - /// this is the priority queue (open_list) to select the next node for expansion + /// this is the priority queue (open_list) to select the next node to be expanded std::priority_queue, comp> queue_; /// it is a counter like variable used to generate consecutive indices @@ -148,17 +148,17 @@ class ThetaStar tree_node * curr_node; - /** @brief it performs a line of sight (los) check between the current node and the parent node of its parent node - * if an los is found and the new costs calculated are lesser then the cost and parent node + /** @brief it performs a line of sight (los) check between the current node and the parent node of its parent node; + * if an los is found and the new costs calculated are lesser, then the cost and parent node * of the current node is updated * @param data of the current node */ void resetParent(tree_node & curr_data); /** - * @brief this function expands the neighbors of the current node + * @brief this function expands the current node * @param curr_data used to send the data of the current node - * @param curr_id used to send the index of the current node as stored in nodes_position + * @param curr_id used to send the index of the current node as stored in nodes_position_ */ void setNeighbors(const tree_node & curr_data, const int & curr_int); @@ -173,7 +173,7 @@ class ThetaStar /** * @brief it is an overloaded function to ease the cost calculations while performing the LOS check -* @param cost denotes the total straight line traversal cost, adds the traversal cost for the node (cx, cy) at every instance, it is also being returned +* @param cost denotes the total straight line traversal cost; it adds the traversal cost for the node (cx, cy) at every instance; it is also being returned * @return false if the traversal cost is greater than / equal to the LETHAL_COST and true otherwise */ bool isSafe(const int & cx, const int & cy, double & cost) const @@ -245,31 +245,31 @@ class ThetaStar /** * @brief initialises the node_position_ vector by storing -1 as index for all points(x, y) within the limits of the map - * @param size_inc is used to increase the the elements of node_position_ in case the size of the map increases + * @param size_inc is used to increase the number of elements in node_position_ in case the size of the map increases */ void initializePosn(int size_inc = 0); /** - * @brief it stores id_this in node_position_ at the index ( *cy + cx ) + * @brief it stores id_this in node_position_ at the index [ size_x_*cy + cx ] * @param id_this the index at which the data of the point(cx, cy) is stored in nodes_data_ */ - void addIndex(const int & cx, const int & cy, const int & id_this) + inline void addIndex(const int & cx, const int & cy, const int & id_this) { node_position_[size_x_ * cy + cx] = id_this; } /** * @brief retrieves the index at which the data of the point(cx, cy) is stored in nodes_data - * @return id_this + * @return id_this is the index */ - void getIndex(const int & cx, const int & cy, int & id_this) + inline void getIndex(const int & cx, const int & cy, int & id_this) { id_this = node_position_[size_x_ * cy + cx]; } /** * @brief this function depending on the size of the nodes_data_ vector allots space to store the data for a node(x, y) - * @param id_this is the index at which the data is stored for that node + * @param id_this is the index at which the data is stored/has to be stored for that node */ void addToNodesData(const int & id_this) { diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp index efc7eb1b13..c90b809291 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star_planner.hpp @@ -54,16 +54,6 @@ class ThetaStarPlanner : public nav2_core::GlobalPlanner const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal) override; - /** - * @brief interpolates points between the adjacent waypoints of the path - * @param raw_path is used to send in the path received from the planner - * @param dist_bw_points is used to send in the interpolation_resolution - * @return the final path with waypoints at a distance of the value of interpolation_resolution of each other - */ - static nav_msgs::msg::Path linearInterpolation( - const std::vector & raw_path, - const double & dist_bw_points); - protected: std::shared_ptr tf_; rclcpp::Clock::SharedPtr clock_; @@ -77,6 +67,16 @@ class ThetaStarPlanner : public nav2_core::GlobalPlanner * @return global_path is the planned path to be taken */ void getPlan(nav_msgs::msg::Path & global_path); + + /** + * @brief interpolates points between the consecutive waypoints of the path + * @param raw_path is used to send in the path received from the planner + * @param dist_bw_points is used to send in the interpolation_resolution (which has been set as the costmap resolution) + * @return the final path with waypoints at a distance of the value of interpolation_resolution of each other + */ + static nav_msgs::msg::Path linearInterpolation( + const std::vector & raw_path, + const double & dist_bw_points); }; } // namespace nav2_theta_star_planner diff --git a/nav2_theta_star_planner/src/theta_star.cpp b/nav2_theta_star_planner/src/theta_star.cpp index d3f6cac1df..7e99403488 100644 --- a/nav2_theta_star_planner/src/theta_star.cpp +++ b/nav2_theta_star_planner/src/theta_star.cpp @@ -19,8 +19,8 @@ namespace theta_star { ThetaStar::ThetaStar() -: w_traversal_cost_(2.0), - w_euc_cost_(7.0), +: w_traversal_cost_(1.0), + w_euc_cost_(5.0), w_heuristic_cost_(1.0), how_many_corners_(8), size_x_(0), @@ -157,13 +157,13 @@ void ThetaStar::backtrace(std::vector & raw_points, int curr_id) coordsW world{}; do { costmap_->mapToWorld(nodes_data_[curr_id].x, nodes_data_[curr_id].y, world.x, world.y); - path_rev.push_back({world.x, world.y}); + path_rev.push_back(world); if (path_rev.size() > 1) { curr_id = nodes_data_[curr_id].parent_id; } } while (curr_id != 0); costmap_->mapToWorld(nodes_data_[curr_id].x, nodes_data_[curr_id].y, world.x, world.y); - path_rev.push_back({world.x, world.y}); + path_rev.push_back(world); raw_points.reserve(path_rev.size()); for (int i = static_cast(path_rev.size()) - 1; i >= 0; i--) { diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index 65cf3a6b0b..932f5fd8e2 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -45,11 +45,11 @@ void ThetaStarPlanner::configure( } nav2_util::declare_parameter_if_not_declared( - node, name_ + ".w_euc_cost", rclcpp::ParameterValue(4.0)); + node, name_ + ".w_euc_cost", rclcpp::ParameterValue(1.0)); node->get_parameter(name_ + ".w_euc_cost", planner_->w_euc_cost_); nav2_util::declare_parameter_if_not_declared( - node, name_ + ".w_traversal_cost", rclcpp::ParameterValue(7.0)); + node, name_ + ".w_traversal_cost", rclcpp::ParameterValue(5.0)); node->get_parameter(name_ + ".w_traversal_cost", planner_->w_traversal_cost_); nav2_util::declare_parameter_if_not_declared( @@ -60,6 +60,7 @@ void ThetaStarPlanner::configure( void ThetaStarPlanner::cleanup() { RCLCPP_INFO(logger_, "CleaningUp plugin %s of type nav2_theta_star_planner", name_.c_str()); + planner_.reset(); } void ThetaStarPlanner::activate() @@ -92,7 +93,6 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( void ThetaStarPlanner::getPlan(nav_msgs::msg::Path & global_path) { std::vector path; - if (planner_->isUnsafeToPlan()) { RCLCPP_ERROR(logger_, "Either of the start or goal pose are an obstacle! "); global_path.poses.clear(); @@ -102,7 +102,6 @@ void ThetaStarPlanner::getPlan(nav_msgs::msg::Path & global_path) RCLCPP_ERROR(logger_, "Could not generate path between the given poses"); global_path.poses.clear(); } - path.clear(); global_path.header.stamp = clock_->now(); global_path.header.frame_id = global_frame_; } @@ -115,12 +114,12 @@ nav_msgs::msg::Path ThetaStarPlanner::linearInterpolation( for (unsigned int j = 0; j < raw_path.size() - 1; j++) { geometry_msgs::msg::PoseStamped p; - coordsW pt1 = {raw_path[j].x, raw_path[j].y}; + coordsW pt1 = raw_path[j]; p.pose.position.x = pt1.x; p.pose.position.y = pt1.y; pa.poses.push_back(p); - coordsW pt2 = {raw_path[j + 1].x, raw_path[j + 1].y}; + coordsW pt2 = raw_path[j + 1]; geometry_msgs::msg::PoseStamped p1; double distance = std::hypot(pt2.x - pt1.x, pt2.y - pt1.y); int loops = static_cast(distance / dist_bw_points); From 81e81a6f63b895bc4fa05b94ac49981ff1a8ba9a Mon Sep 17 00:00:00 2001 From: Anshumaan Singh Date: Fri, 30 Apr 2021 17:23:51 +0530 Subject: [PATCH 58/81] fix typos --- nav2_theta_star_planner/README.md | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index f20d0bdc54..2e97602d36 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -6,11 +6,11 @@ The Theta Star Planner is a global planning plugin meant to be used with the Nav - Is well suited for smaller robots of the omni-directional and differential drive kind - As it considers the costmap traversal cost during execution it tends to smoothen the paths automatically, thus mitigating the need to smoothen the path. (The presence of sharp turns depends on the resolution of the map, and it decreases as the map resolution increases.) - Allows to control the path behavior to either be any angle directed or to be in the middle of the spaces -- The planner uses A\* search along with line of sight checks to form any-angle paths thus avoiding zig-zag paths that may be present in the usual implementation of A\* +- The planner uses A\* search along with line of sight checks to form any-angle paths thus avoiding zig-zag paths that may be present in the usual implementation of A\* +- The algorithmic part of the planner has been segregated from the plugin part to allow for reusability. ## Metrics -For the below example the planner took ~46ms (averaged value) to compute the path of 87.5m - - +For the below example the planner took ~46ms (averaged value) to compute the path of 87.5m - ![example.png](img/00-37.png) The parameters were set to - `w_euc_cost: 1.0`, `w_traversal_cost: 5.0`, `w_heuristic_cost: 1.0` and the `global_costmap`'s `inflation_layer` parameters are set as - `cost_scaling_factor:5.0`, `inflation_radius: 5.5` @@ -71,17 +71,17 @@ Do note that the `global_costmap`'s `inflation_layer` values recommended for the ## Usage Notes ### Tuning the Parameters -Before starting off, do note that the costmap_cost(curr,neigh) component after being operated (before being multiplied to its parameter and being substituted in g(init_rclcpp)) varies from 0 to 1. Keep this in mind while tuning. +Before starting off, do note that the costmap_cost(curr,neigh) component after being operated (ie before being multiplied to its parameter and being substituted in g(init_rclcpp)) varies from 0 to 1. -This planner uses the costs associated with each cell from the `global_costmap` as a measure of the point's proximity to the obstacles. Providing a gentle potential field that covers the entirety of the region (with only small pocket like regions of cost = 0) is recommended in order to achieve paths that pass through the middle of the spaces. A good starting point could be to set the `inflation_layer`'s parameters as - `cost_scaling_factor:10.0`, `inflation_radius: 5.5` and then to decrease the value of `cost_scaling_factor` to achieve the said potential field. +This planner uses the costs associated with each cell from the `global_costmap` as a measure of the point's proximity to the obstacles. Providing a gentle potential field that covers the entirety of the region (thus leading to only small pocket like regions of cost = 0) is recommended in order to achieve paths that pass through the middle of the spaces. A good starting point could be to set the `inflation_layer`'s parameters as - `cost_scaling_factor:10.0`, `inflation_radius: 5.5` and then to decrease the value of `cost_scaling_factor` to achieve the said potential field. -Providing a gentle potential field over the region allows the planner to compensate for the increase in path lengths which would allow for an increase in the distance from the obstacles, which around a corner allows for naturally smoothing the turns and thus removing the requirement for the use of an external path smoother. +Providing a gentle potential field over the region allows the planner to compensate for the increase in path lengths by leading to an increase in the distance from the obstacles, which around a corner allows for naturally smoothing the turns and thus removing the requirement for the use of an external path smoother. In order to achieve paths that stay in the middle of the spaces set `w_traversal_cost` at a higher value than `w_euc_cost`. To begin with, you can set the parameters to its default values and then increase the value of `w_traversal_cost`, while also decreasing `w_euc_cost` to allow for an increase in the path length thus letting the path to settle in the middle regions of the spaces with lower costs on the costmap. While tuning the planner's parameters you can also change the `inflation_layer`'s parameters to tune the behavior of the paths. ### Path Smoothing -Because of how the cost function works, the output path has a natural tendency to form smooth curves around corners, though the smoothness of the path depends on how wide the turn is and the number of cells in that turns. +Because of how the cost function works, the output path has a natural tendency to form smooth curves around corners, though the smoothness of the path depends on how wide the turn is, and the number of cells in that turn. This planner is recommended to be used with local planners like DWB or TEB (or other any planner / controllers that form a local trajectory to be traversed) as these take into account the abrupt turns which might arise due to the planner not being able to find a smoother turns owing to the aforementioned reasons. -While smoother paths can be achieved by increasing the costmap resolution (ie using a costmap of 1cm resolution rather than a 5cm one) it is not recommended to do so as it comes at the cost of increased query times from the planner. Test the planners performance on the finer costmaps before making a switch to those costmaps. +While smoother paths can be achieved by increasing the costmap resolution (ie using a costmap of 1cm resolution rather than a 5cm one) it is not recommended doing so as it comes at the cost of increased query times from the planner. Test the planners performance on the finer costmaps before making a switch to those costmaps. From 3e0bc169c596f7d847e9dec6c8d877130b5fdead Mon Sep 17 00:00:00 2001 From: Anshumaan Singh Date: Sun, 9 May 2021 11:22:13 +0530 Subject: [PATCH 59/81] Update README.md --- nav2_theta_star_planner/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index 2e97602d36..0526325e66 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -1,5 +1,5 @@ # Nav2 Theta Star Planner -The Theta Star Planner is a global planning plugin meant to be used with the Nav2 Planner Server. The `nav2_theta_star_planner` implements a highly optimized version of the Theta Star Planner (specifically the [Lazy Theta\* P variant](http://idm-lab.org/bib/abstracts/papers/aaai10b.pdf)) meant to plan any-angle paths using A\*. The planner supports differential-drive and omni-directional robots. +The Theta Star Planner is a global planning plugin meant to be used with the Nav2 Planner Server. The `nav2_theta_star_planner` implements a highly optimized version of the Theta\* Planner (specifically the [Lazy Theta\* P variant](http://idm-lab.org/bib/abstracts/papers/aaai10b.pdf)) meant to plan any-angle paths using A\*. The planner supports differential-drive and omni-directional robots. ## Features - Uses the costs from the costmap to penalise high cost regions From 3a9983b6a85542487ce3ed8d1503b475dcc0113a Mon Sep 17 00:00:00 2001 From: Anshumaan Singh Date: Sun, 9 May 2021 11:25:07 +0530 Subject: [PATCH 60/81] Update README.md --- nav2_theta_star_planner/README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index 0526325e66..1bbd0eb66d 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -10,6 +10,7 @@ The Theta Star Planner is a global planning plugin meant to be used with the Nav - The algorithmic part of the planner has been segregated from the plugin part to allow for reusability. ## Metrics +In general the planner is capable of planning for a map of size (566 x 608) from 30ms to 80ms (averaged value, it might exceed the stated time limits here for some iterations) For the below example the planner took ~46ms (averaged value) to compute the path of 87.5m - ![example.png](img/00-37.png) From e24baa66a6d81b99e7e1f5c90715be29c6b767bb Mon Sep 17 00:00:00 2001 From: Anshumaan Singh Date: Sun, 9 May 2021 11:25:24 +0530 Subject: [PATCH 61/81] Update README.md --- nav2_theta_star_planner/README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index 1bbd0eb66d..1d0930211b 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -10,7 +10,8 @@ The Theta Star Planner is a global planning plugin meant to be used with the Nav - The algorithmic part of the planner has been segregated from the plugin part to allow for reusability. ## Metrics -In general the planner is capable of planning for a map of size (566 x 608) from 30ms to 80ms (averaged value, it might exceed the stated time limits here for some iterations) +In general the planner is capable of planning for a map of size (566 x 608) from 30ms to 80ms (averaged value, it might exceed the stated time limits here for some iterations). + For the below example the planner took ~46ms (averaged value) to compute the path of 87.5m - ![example.png](img/00-37.png) From dec0dddf02a2b758be05cb154f4016604260f715 Mon Sep 17 00:00:00 2001 From: Anshumaan Singh Date: Sun, 9 May 2021 11:26:34 +0530 Subject: [PATCH 62/81] Update README.md --- nav2_theta_star_planner/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index 1d0930211b..54831606db 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -10,7 +10,7 @@ The Theta Star Planner is a global planning plugin meant to be used with the Nav - The algorithmic part of the planner has been segregated from the plugin part to allow for reusability. ## Metrics -In general the planner is capable of planning for a map of size (566 x 608) from 30ms to 80ms (averaged value, it might exceed the stated time limits here for some iterations). +In general the planner is capable of planning for a map of size (566 x 608) from 30ms to 80ms (averaged value, it might exceed the stated time limits here for some iterations depending on how the parameters have been tuned). For the below example the planner took ~46ms (averaged value) to compute the path of 87.5m - ![example.png](img/00-37.png) From 6c33e50f606d395216cdbfc1b05d9fe42de3d9e8 Mon Sep 17 00:00:00 2001 From: Anshumaan Singh Date: Sun, 9 May 2021 12:52:33 +0530 Subject: [PATCH 63/81] Update README.md --- nav2_theta_star_planner/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index 54831606db..7a84860264 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -53,7 +53,7 @@ The parameters of the planner are : - ` .how_many_corners ` : to choose between 4-connected and 8-connected graph expansions, the accepted values are 4 and 8 - ` .w_euc_cost ` : weight applied on the length of the path. - ` .w_traversal_cost ` : it tunes how harshly the nodes of high cost are penalised. From the above g(neigh) equation you can see that the cost-aware component of the cost function forms a parabolic curve, thus this parameter would, on increasing its value, make that curve steeper allowing for a greater differentiation (as the delta of costs would increase, when the graph becomes steep) among the nodes of different costs. -- ` .w_heuristic_cost ` : it has been provided to have a admissible heuristic, so the recommendation would be to change its value only when required. Usually set is at the same value as the `w_euc_cost` or 1.0 (whichever is lower), though you may increase the value of `w_heuristic_cost` to speed up the process. +- ` .w_heuristic_cost ` : it has been provided to have a admissible heuristic, so the recommendation would be to change its value only when required. Usually set is at the same value as `w_euc_cost` or 1.0 (whichever is lower), though you may increase the value of `w_heuristic_cost` to speed up the process. Below are the default values of the parameters : ``` From cbad8ffa5af23f59d127c8a8910a0432649971ed Mon Sep 17 00:00:00 2001 From: Anshumaan Singh Date: Sun, 9 May 2021 14:20:23 +0530 Subject: [PATCH 64/81] Update README.md --- nav2_theta_star_planner/README.md | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index 7a84860264..d0de12d6a0 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -84,6 +84,9 @@ In order to achieve paths that stay in the middle of the spaces set `w_traversal ### Path Smoothing Because of how the cost function works, the output path has a natural tendency to form smooth curves around corners, though the smoothness of the path depends on how wide the turn is, and the number of cells in that turn. -This planner is recommended to be used with local planners like DWB or TEB (or other any planner / controllers that form a local trajectory to be traversed) as these take into account the abrupt turns which might arise due to the planner not being able to find a smoother turns owing to the aforementioned reasons. +This planner is recommended to be used with local planners like DWB or TEB (or other any local planner / controllers that form a local trajectory to be traversed) as these take into account the abrupt turns which might arise due to the planner not being able to find a smoother turns owing to the aforementioned reasons. While smoother paths can be achieved by increasing the costmap resolution (ie using a costmap of 1cm resolution rather than a 5cm one) it is not recommended doing so as it comes at the cost of increased query times from the planner. Test the planners performance on the finer costmaps before making a switch to those costmaps. + +### Possible Applications +This planner could be used in scenarios where planning speed matters over an extremely smooth path, which could anyways be handled by using a local planner/controller as mentioned above. Because of the any-angled nature of paths you can traverse environments diagonally (wherever it is allowed, eg: in a wide open region). From da2496e0ee94637c183e32b2da5bfc8c926ac9b4 Mon Sep 17 00:00:00 2001 From: Anshumaan Singh Date: Mon, 10 May 2021 12:37:20 +0530 Subject: [PATCH 65/81] Update README.md --- nav2_theta_star_planner/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index d0de12d6a0..fdbd6fe761 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -53,7 +53,7 @@ The parameters of the planner are : - ` .how_many_corners ` : to choose between 4-connected and 8-connected graph expansions, the accepted values are 4 and 8 - ` .w_euc_cost ` : weight applied on the length of the path. - ` .w_traversal_cost ` : it tunes how harshly the nodes of high cost are penalised. From the above g(neigh) equation you can see that the cost-aware component of the cost function forms a parabolic curve, thus this parameter would, on increasing its value, make that curve steeper allowing for a greater differentiation (as the delta of costs would increase, when the graph becomes steep) among the nodes of different costs. -- ` .w_heuristic_cost ` : it has been provided to have a admissible heuristic, so the recommendation would be to change its value only when required. Usually set is at the same value as `w_euc_cost` or 1.0 (whichever is lower), though you may increase the value of `w_heuristic_cost` to speed up the process. +- ` .w_heuristic_cost ` : it has been provided to have an admissible heuristic, so the recommendation would be to change its value only when required. Usually set is at the same value as `w_euc_cost` or 1.0 (whichever is lower), though you may increase the value of `w_heuristic_cost` to speed up the process. Below are the default values of the parameters : ``` From e8da09e420547032b0302055099424d03b24022b Mon Sep 17 00:00:00 2001 From: Anshumaan Singh Date: Tue, 18 May 2021 23:56:39 +0530 Subject: [PATCH 66/81] fix the typo - the first isSafe calls output wasn't negated, it has been fixed --- nav2_theta_star_planner/src/theta_star.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_theta_star_planner/src/theta_star.cpp b/nav2_theta_star_planner/src/theta_star.cpp index 7e99403488..488c19a42e 100644 --- a/nav2_theta_star_planner/src/theta_star.cpp +++ b/nav2_theta_star_planner/src/theta_star.cpp @@ -222,7 +222,7 @@ bool ThetaStar::losCheck( if (f != 0 && !isSafe(cx + u_x, cy + u_y, sl_cost)) { return false; } - if (dx == 0 && isSafe(cx, cy + u_y, sl_cost) && !isSafe(cx - 1, cy + u_y, sl_cost)) { + if (dx == 0 && !isSafe(cx, cy + u_y, sl_cost) && !isSafe(cx - 1, cy + u_y, sl_cost)) { return false; } cy += sy; From 4210dc9891d0b5b991c8826ced07342001dd15f1 Mon Sep 17 00:00:00 2001 From: Anshumaan Singh Date: Sat, 12 Jun 2021 00:47:48 +0530 Subject: [PATCH 67/81] Update theta_star.hpp - added the `getCost()` function (again) - replaced the use of indices with pointers to store the node's data, changes were made to the following functions - `addIndex()`, `getIndex()`, `initialisePosn()`. - the priority queue now stores the pointer to node's data and accordingly changes were made to `comp` struct - the global variable `curr_node` was renamed to `exp_node` - removed the struct `pos` --- .../nav2_theta_star_planner/theta_star.hpp | 119 ++++++++++-------- 1 file changed, 65 insertions(+), 54 deletions(-) diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp index 286cf26473..c293922b08 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp @@ -24,7 +24,7 @@ #include "nav2_costmap_2d/costmap_2d_ros.hpp" const double INF_COST = DBL_MAX; -const int LETHAL_COST = 252; +const int LETHAL_COST = 250; struct coordsM { @@ -36,30 +36,25 @@ struct coordsW double x, y; }; -struct pos -{ - int pos_id; - double f; -}; - struct tree_node { - int x, y; - double g = INF_COST; - double h = INF_COST; - int parent_id; - bool is_in_queue = false; - double f = INF_COST; + int x, y; + double g = INF_COST; + double h = INF_COST; + const tree_node* parent_id = NULL; + bool is_in_queue = false; + double f = INF_COST; }; struct comp { - bool operator()(pos & p1, pos & p2) + bool operator()(const tree_node * p1, const tree_node * p2) { - return (p1.f) > (p2.f); + return (p1->f) > (p2->f); } }; +// TODO (Anshu-man567) : Update the doc down below namespace theta_star { class ThetaStar @@ -79,6 +74,9 @@ class ThetaStar /// the x-directional and y-directional lengths of the map respectively int size_x_, size_y_; + // TODO (Anshu-man567) : Delete it after use + rclcpp::Logger logger_{rclcpp::get_logger("ThetaStarPlanner")}; + ThetaStar(); ~ThetaStar() = default; @@ -102,7 +100,7 @@ class ThetaStar /** * @brief initialises the values of the start and goal points -*/ + */ void setStartAndGoal( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal); @@ -116,12 +114,15 @@ class ThetaStar return !(isSafe(src_.x, src_.y)) || !(isSafe(dst_.x, dst_.y)); } + /// TODO (Anshu-man567): Remove this!!!!!! + int nodes_opened = 0; + protected: /// for the coordinates (x,y), it stores at node_position_[size_x_ * y + x], - /// the index at which the data of the node is present in nodes_data_ + /// the pointer to the location at which the data of the node is present in nodes_data_ /// it is initialised with size_x_ * size_y_ elements /// and its number of elements increases to account for a change in map size - std::vector node_position_; + std::vector node_position_; /// the vector nodes_data_ stores the coordinates, costs and index of the parent node, /// and whether or not the node is present in queue_, for all the nodes searched @@ -130,7 +131,7 @@ class ThetaStar std::vector nodes_data_; /// this is the priority queue (open_list) to select the next node to be expanded - std::priority_queue, comp> queue_; + std::priority_queue, comp> queue_; /// it is a counter like variable used to generate consecutive indices /// such that the data for all the nodes (in open and closed lists) could be stored @@ -146,21 +147,22 @@ class ThetaStar {1, 1}, {-1, -1}}; - tree_node * curr_node; + tree_node * exp_node; + /** @brief it performs a line of sight (los) check between the current node and the parent node of its parent node; * if an los is found and the new costs calculated are lesser, then the cost and parent node * of the current node is updated * @param data of the current node - */ - void resetParent(tree_node & curr_data); + */ + void resetParent(tree_node * curr_data); /** * @brief this function expands the current node * @param curr_data used to send the data of the current node * @param curr_id used to send the index of the current node as stored in nodes_position_ */ - void setNeighbors(const tree_node & curr_data, const int & curr_int); + void setNeighbors(const tree_node * curr_data); /** * @brief performs the line of sight check using Bresenham's Algorithm, @@ -169,16 +171,23 @@ class ThetaStar * @param sl_cost is used to return the cost thus incurred * @return true if a line of sight exists between the points */ - bool losCheck(const int & x0, const int & y0, const int & x1, const int & y1, double & sl_cost); + bool losCheck(const int & x0, const int & y0, const int & x1, const int & y1, double & sl_cost) const; + + /** + * @brief it returns the path by backtracking from the goal to the start, by using their parent nodes + * @param raw_points used to return the path thus found + * @param curr_id sends in the index of the goal coordinate, as stored in nodes_position + */ + void backtrace(std::vector & raw_points, const tree_node * curr_n) const; /** -* @brief it is an overloaded function to ease the cost calculations while performing the LOS check -* @param cost denotes the total straight line traversal cost; it adds the traversal cost for the node (cx, cy) at every instance; it is also being returned -* @return false if the traversal cost is greater than / equal to the LETHAL_COST and true otherwise -*/ + * @brief it is an overloaded function to ease the cost calculations while performing the LOS check + * @param cost denotes the total straight line traversal cost; it adds the traversal cost for the node (cx, cy) at every instance; it is also being returned + * @return false if the traversal cost is greater than / equal to the LETHAL_COST and true otherwise + */ bool isSafe(const int & cx, const int & cy, double & cost) const { - double curr_cost = costmap_->getCost(cx, cy); + double curr_cost = getCost(cx, cy); if (curr_cost < LETHAL_COST) { cost += w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST; return true; @@ -187,12 +196,25 @@ class ThetaStar } } + /* + * @brief this function scales the costmap cost by shifting the origin to 25 and then multiply + * the actual costmap cost by 0.9 to keep the output in the range of [25, 255) + */ + inline double getCost(const int & cx, const int & cy) const + { + return 25 + 0.9*costmap_->getCost(cx, cy); + } + /** - * @brief it returns the path by backtracking from the goal to the start, by using their parent nodes - * @param raw_points used to return the path thus found - * @param curr_id sends in the index of the goal coordinate, as stored in nodes_position + * @brief for the point(cx, cy), its traversal cost is calculated by + * *()^2/()^2 + * @return the traversal cost thus calculated */ - void backtrace(std::vector & raw_points, int curr_id); + inline double getTraversalCost(const int & cx, const int & cy) + { + double curr_cost = getCost(cx, cy); + return w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST; + } /** * @brief calculates the piecewise straight line euclidean distances by @@ -204,17 +226,6 @@ class ThetaStar return w_euc_cost_ * std::hypot(ax - bx, ay - by); } - /** - * @brief for the point(cx, cy), its traversal cost is calculated by - * *()^2/()^2 - * @return the traversal cost thus calculated - */ - inline double getTraversalCost(const int & cx, const int & cy) - { - double curr_cost = costmap_->getCost(cx, cy); - return w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST; - } - /** * @brief for the point(cx, cy), its heuristic cost is calculated by * * @@ -250,21 +261,21 @@ class ThetaStar void initializePosn(int size_inc = 0); /** - * @brief it stores id_this in node_position_ at the index [ size_x_*cy + cx ] - * @param id_this the index at which the data of the point(cx, cy) is stored in nodes_data_ - */ - inline void addIndex(const int & cx, const int & cy, const int & id_this) + * @brief it stores id_this in node_position_ at the index [ size_x_*cy + cx ] + * @param id_this a pointer to the location at which the data of the point(cx, cy) is stored in nodes_data_ + */ + inline void addIndex(const int & cx, const int & cy, tree_node * node_this) { - node_position_[size_x_ * cy + cx] = id_this; + node_position_[size_x_ * cy + cx] = node_this; } /** - * @brief retrieves the index at which the data of the point(cx, cy) is stored in nodes_data - * @return id_this is the index + * @brief retrieves the pointer of the location at which the data of the point(cx, cy) is stored in nodes_data + * @return id_this is the pointer to that location */ - inline void getIndex(const int & cx, const int & cy, int & id_this) + inline tree_node* getIndex(const int & cx, const int & cy) { - id_this = node_position_[size_x_ * cy + cx]; + return node_position_[size_x_ * cy + cx]; } /** @@ -290,7 +301,7 @@ class ThetaStar */ void clearQueue() { - queue_ = std::priority_queue, comp>(); + queue_ = std::priority_queue, comp>(); } }; } // namespace theta_star From f45980a753b0d9e8c5b5e2eb16bb0c7592e7a9dd Mon Sep 17 00:00:00 2001 From: Anshumaan Singh Date: Sat, 12 Jun 2021 00:48:57 +0530 Subject: [PATCH 68/81] Update theta_star.cpp --- nav2_theta_star_planner/src/theta_star.cpp | 120 ++++++++++----------- 1 file changed, 57 insertions(+), 63 deletions(-) diff --git a/nav2_theta_star_planner/src/theta_star.cpp b/nav2_theta_star_planner/src/theta_star.cpp index 488c19a42e..311e5f62b3 100644 --- a/nav2_theta_star_planner/src/theta_star.cpp +++ b/nav2_theta_star_planner/src/theta_star.cpp @@ -19,15 +19,15 @@ namespace theta_star { ThetaStar::ThetaStar() -: w_traversal_cost_(1.0), - w_euc_cost_(5.0), +: w_traversal_cost_(1.2), + w_euc_cost_(2.5), w_heuristic_cost_(1.0), how_many_corners_(8), size_x_(0), size_y_(0), index_generated_(0) { - curr_node = new tree_node; + exp_node = new tree_node; } void ThetaStar::setStartAndGoal( @@ -45,29 +45,27 @@ void ThetaStar::setStartAndGoal( bool ThetaStar::generatePath(std::vector & raw_path) { resetContainers(); - int curr_id = index_generated_; addToNodesData(index_generated_); double src_g_cost = getTraversalCost(src_.x, src_.y), src_h_cost = getHCost(src_.x, src_.y); - nodes_data_[curr_id] = - {src_.x, src_.y, src_g_cost, src_h_cost, curr_id, true, src_g_cost + src_h_cost}; - queue_.push({curr_id, (nodes_data_[curr_id].f)}); - addIndex(nodes_data_[curr_id].x, nodes_data_[curr_id].y, index_generated_); + nodes_data_[index_generated_] = + {src_.x, src_.y, src_g_cost, src_h_cost, &nodes_data_[index_generated_], true, src_g_cost + src_h_cost}; + queue_.push({&nodes_data_[index_generated_]}); + addIndex(src_.x, src_.y, &nodes_data_[index_generated_]); + tree_node* curr_data = &nodes_data_[index_generated_]; index_generated_++; - - int nodes_opened = 0; + nodes_opened = 0; while (!queue_.empty()) { nodes_opened++; - tree_node & curr_data = nodes_data_[curr_id]; - if (isGoal(curr_data)) { + if (isGoal(*curr_data)) { break; } resetParent(curr_data); - setNeighbors(curr_data, curr_id); + setNeighbors(curr_data); - curr_id = queue_.top().pos_id; + curr_data = queue_.top(); queue_.pop(); } @@ -76,40 +74,40 @@ bool ThetaStar::generatePath(std::vector & raw_path) return false; } - backtrace(raw_path, curr_id); + backtrace(raw_path, curr_data); clearQueue(); return true; } -void ThetaStar::resetParent(tree_node & curr_data) +void ThetaStar::resetParent(tree_node * curr_data) { double g_cost, los_cost = 0; - curr_data.is_in_queue = false; - tree_node & curr_par = nodes_data_[curr_data.parent_id]; - tree_node & maybe_par = nodes_data_[curr_par.parent_id]; - - if (losCheck(curr_data.x, curr_data.y, maybe_par.x, maybe_par.y, los_cost)) { - g_cost = maybe_par.g + - getEuclideanCost(curr_data.x, curr_data.y, maybe_par.x, maybe_par.y) + los_cost; - - if (g_cost < curr_data.g) { - curr_data.parent_id = curr_par.parent_id; - curr_data.g = g_cost; - curr_data.f = g_cost + curr_data.h; + curr_data->is_in_queue = false; + const tree_node * curr_par = curr_data->parent_id; + const tree_node * maybe_par = curr_par->parent_id; + + if (losCheck(curr_data->x, curr_data->y, maybe_par->x, maybe_par->y, los_cost)) { + g_cost = maybe_par->g + + getEuclideanCost(curr_data->x, curr_data->y, maybe_par->x, maybe_par->y) + los_cost; + + if (g_cost < curr_data->g) { + curr_data->parent_id = maybe_par; + curr_data->g = g_cost; + curr_data->f = g_cost + curr_data->h; } } } -void ThetaStar::setNeighbors(const tree_node & curr_data, const int & curr_id) +void ThetaStar::setNeighbors(const tree_node * curr_data) { int mx, my; - int m_id, m_par; + tree_node* m_id = NULL; double g_cost, h_cost, cal_cost; for (int i = 0; i < how_many_corners_; i++) { - mx = curr_data.x + moves[i].x; - my = curr_data.y + moves[i].y; + mx = curr_data->x + moves[i].x; + my = curr_data->y + moves[i].y; if (withinLimits(mx, my)) { if (!isSafe(mx, my)) { @@ -119,50 +117,49 @@ void ThetaStar::setNeighbors(const tree_node & curr_data, const int & curr_id) continue; } - m_par = curr_id; - g_cost = curr_data.g + getEuclideanCost(curr_data.x, curr_data.y, mx, my) + + g_cost = curr_data->g + getEuclideanCost(curr_data->x, curr_data->y, mx, my) + getTraversalCost(mx, my); - getIndex(mx, my, m_id); + m_id = getIndex(mx, my); - if (m_id == -1) { + if (m_id == NULL) { addToNodesData(index_generated_); - m_id = index_generated_; + m_id = &nodes_data_[index_generated_]; addIndex(mx, my, m_id); index_generated_++; } - curr_node = &nodes_data_[m_id]; + exp_node = m_id; h_cost = getHCost(mx, my); cal_cost = g_cost + h_cost; - if (curr_node->f > cal_cost) { - curr_node->g = g_cost; - curr_node->h = h_cost; - curr_node->f = cal_cost; - curr_node->parent_id = m_par; - if (!curr_node->is_in_queue) { - curr_node->x = mx; - curr_node->y = my; - curr_node->is_in_queue = true; - queue_.push({m_id, (curr_node->f)}); + if (exp_node->f > cal_cost) { + exp_node->g = g_cost; + exp_node->h = h_cost; + exp_node->f = cal_cost; + exp_node->parent_id = curr_data; + if (!exp_node->is_in_queue) { + exp_node->x = mx; + exp_node->y = my; + exp_node->is_in_queue = true; + queue_.push({m_id}); } } } } -void ThetaStar::backtrace(std::vector & raw_points, int curr_id) +void ThetaStar::backtrace(std::vector & raw_points, const tree_node * curr_n) const { std::vector path_rev; coordsW world{}; do { - costmap_->mapToWorld(nodes_data_[curr_id].x, nodes_data_[curr_id].y, world.x, world.y); + costmap_->mapToWorld(curr_n->x, curr_n->y, world.x, world.y); path_rev.push_back(world); if (path_rev.size() > 1) { - curr_id = nodes_data_[curr_id].parent_id; + curr_n = curr_n->parent_id; } - } while (curr_id != 0); - costmap_->mapToWorld(nodes_data_[curr_id].x, nodes_data_[curr_id].y, world.x, world.y); + } while (curr_n->parent_id != curr_n); + costmap_->mapToWorld(curr_n->x, curr_n->y, world.x, world.y); path_rev.push_back(world); raw_points.reserve(path_rev.size()); @@ -173,26 +170,23 @@ void ThetaStar::backtrace(std::vector & raw_points, int curr_id) bool ThetaStar::losCheck( const int & x0, const int & y0, const int & x1, const int & y1, - double & sl_cost) + double & sl_cost) const { sl_cost = 0; int cx, cy; int dy = abs(y1 - y0), dx = abs(x1 - x0), f = 0; - int sx, sy; - int lx, ly; // this variable is reserved for future use - sy = y1 > y0 ? 1 : -1; + int sx, sy; sx = x1 > x0 ? 1 : -1; + sy = y1 > y0 ? 1 : -1; int u_x = (sx - 1) / 2; int u_y = (sy - 1) / 2; - lx = x1; - ly = y1; cx = x0; cy = y0; if (dx >= dy) { - while (cx != lx) { + while (cx != x1) { f += dy; if (f >= dx) { if (!isSafe(cx + u_x, cy + u_y, sl_cost)) { @@ -210,7 +204,7 @@ bool ThetaStar::losCheck( cx += sx; } } else { - while (cy != ly) { + while (cy != y1) { f = f + dx; if (f >= dy) { if (!isSafe(cx + u_x, cy + u_y, sl_cost)) { @@ -256,12 +250,12 @@ void ThetaStar::initializePosn(int size_inc) if (!node_position_.empty()) { for (; i < size_x_ * size_y_; i++) { - node_position_[i] = -1; + node_position_[i] = nullptr; } } for (; i < size_inc; i++) { - node_position_.emplace_back(-1); + node_position_.push_back(nullptr); } } } // namespace theta_star From 8ed836464122a8a5bd70258346de9b0143e7e6b7 Mon Sep 17 00:00:00 2001 From: Anshu-man567 Date: Fri, 18 Jun 2021 00:19:32 +0530 Subject: [PATCH 69/81] update default parameters --- .../nav2_theta_star_planner/theta_star.hpp | 5 ----- nav2_theta_star_planner/src/theta_star.cpp | 15 ++++++++------- .../src/theta_star_planner.cpp | 3 ++- 3 files changed, 10 insertions(+), 13 deletions(-) diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp index c293922b08..1d57c997ae 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp @@ -54,7 +54,6 @@ struct comp } }; -// TODO (Anshu-man567) : Update the doc down below namespace theta_star { class ThetaStar @@ -74,9 +73,6 @@ class ThetaStar /// the x-directional and y-directional lengths of the map respectively int size_x_, size_y_; - // TODO (Anshu-man567) : Delete it after use - rclcpp::Logger logger_{rclcpp::get_logger("ThetaStarPlanner")}; - ThetaStar(); ~ThetaStar() = default; @@ -114,7 +110,6 @@ class ThetaStar return !(isSafe(src_.x, src_.y)) || !(isSafe(dst_.x, dst_.y)); } - /// TODO (Anshu-man567): Remove this!!!!!! int nodes_opened = 0; protected: diff --git a/nav2_theta_star_planner/src/theta_star.cpp b/nav2_theta_star_planner/src/theta_star.cpp index 311e5f62b3..1f3c633692 100644 --- a/nav2_theta_star_planner/src/theta_star.cpp +++ b/nav2_theta_star_planner/src/theta_star.cpp @@ -19,8 +19,8 @@ namespace theta_star { ThetaStar::ThetaStar() -: w_traversal_cost_(1.2), - w_euc_cost_(2.5), +: w_traversal_cost_(1.0), + w_euc_cost_(2.0), w_heuristic_cost_(1.0), how_many_corners_(8), size_x_(0), @@ -48,10 +48,11 @@ bool ThetaStar::generatePath(std::vector & raw_path) addToNodesData(index_generated_); double src_g_cost = getTraversalCost(src_.x, src_.y), src_h_cost = getHCost(src_.x, src_.y); nodes_data_[index_generated_] = - {src_.x, src_.y, src_g_cost, src_h_cost, &nodes_data_[index_generated_], true, src_g_cost + src_h_cost}; + {src_.x, src_.y, src_g_cost, src_h_cost, &nodes_data_[index_generated_], true, + src_g_cost + src_h_cost}; queue_.push({&nodes_data_[index_generated_]}); addIndex(src_.x, src_.y, &nodes_data_[index_generated_]); - tree_node* curr_data = &nodes_data_[index_generated_]; + tree_node * curr_data = &nodes_data_[index_generated_]; index_generated_++; nodes_opened = 0; @@ -102,7 +103,7 @@ void ThetaStar::resetParent(tree_node * curr_data) void ThetaStar::setNeighbors(const tree_node * curr_data) { int mx, my; - tree_node* m_id = NULL; + tree_node * m_id = NULL; double g_cost, h_cost, cal_cost; for (int i = 0; i < how_many_corners_; i++) { @@ -156,7 +157,7 @@ void ThetaStar::backtrace(std::vector & raw_points, const tree_node * c costmap_->mapToWorld(curr_n->x, curr_n->y, world.x, world.y); path_rev.push_back(world); if (path_rev.size() > 1) { - curr_n = curr_n->parent_id; + curr_n = curr_n->parent_id; } } while (curr_n->parent_id != curr_n); costmap_->mapToWorld(curr_n->x, curr_n->y, world.x, world.y); @@ -176,7 +177,7 @@ bool ThetaStar::losCheck( int cx, cy; int dy = abs(y1 - y0), dx = abs(x1 - x0), f = 0; - int sx, sy; + int sx, sy; sx = x1 > x0 ? 1 : -1; sy = y1 > y0 ? 1 : -1; diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index 932f5fd8e2..6fd4eb88be 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -49,7 +49,7 @@ void ThetaStarPlanner::configure( node->get_parameter(name_ + ".w_euc_cost", planner_->w_euc_cost_); nav2_util::declare_parameter_if_not_declared( - node, name_ + ".w_traversal_cost", rclcpp::ParameterValue(5.0)); + node, name_ + ".w_traversal_cost", rclcpp::ParameterValue(2.0)); node->get_parameter(name_ + ".w_traversal_cost", planner_->w_traversal_cost_); nav2_util::declare_parameter_if_not_declared( @@ -87,6 +87,7 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( auto stop_time = std::chrono::steady_clock::now(); auto dur = std::chrono::duration_cast(stop_time - start_time); RCLCPP_DEBUG(logger_, "the time taken is : %i", static_cast(dur.count())); + RCLCPP_DEBUG(logger_, "the nodes_opened are: %i", planner_->nodes_opened); return global_path; } From cffb03480e51e3d7cea90408766cc54e23ed81bb Mon Sep 17 00:00:00 2001 From: Anshumaan Singh Date: Fri, 18 Jun 2021 08:33:05 +0530 Subject: [PATCH 70/81] update default parameters --- nav2_theta_star_planner/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index fdbd6fe761..7817b0a207 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -64,8 +64,8 @@ planner_server: planner_plugin_ids: ["GridBased"] GridBased: how_many_corners: 8 - w_euc_cost: 4.0 - w_traversal_cost: 7.0 + w_euc_cost: 1.0 + w_traversal_cost: 2.0 w_heuristic_cost: 1.0 ``` Do note that the `global_costmap`'s `inflation_layer` values recommended for the above values of the parameter are - `cost_scaling_factor:1.0`, `inflation_radius: 5.5` From a7e1da0cd62b4e99c62c57a7fef3a780b45a9390 Mon Sep 17 00:00:00 2001 From: Anshumaan Singh Date: Fri, 18 Jun 2021 09:28:35 +0530 Subject: [PATCH 71/81] Update README.md --- nav2_theta_star_planner/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index 7817b0a207..d2b32de0d3 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -68,7 +68,7 @@ planner_server: w_traversal_cost: 2.0 w_heuristic_cost: 1.0 ``` -Do note that the `global_costmap`'s `inflation_layer` values recommended for the above values of the parameter are - `cost_scaling_factor:1.0`, `inflation_radius: 5.5` +Do note that the `global_costmap`'s `inflation_layer` values recommended for the above values of the parameter are - `cost_scaling_factor:4.0`, `inflation_radius: 5.5` ## Usage Notes From 4c04a66c305d41373eadd4f78f5f793d0c9adb0f Mon Sep 17 00:00:00 2001 From: Anshu-man567 Date: Sat, 19 Jun 2021 01:26:50 +0530 Subject: [PATCH 72/81] update the test file --- .../nav2_theta_star_planner/theta_star.hpp | 44 +++++----- nav2_theta_star_planner/src/theta_star.cpp | 4 +- .../src/theta_star_planner.cpp | 2 +- .../test/test_theta_star.cpp | 86 +++++++++++-------- 4 files changed, 74 insertions(+), 62 deletions(-) diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp index 1d57c997ae..d3ba6a0b86 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp @@ -38,12 +38,12 @@ struct coordsW struct tree_node { - int x, y; - double g = INF_COST; - double h = INF_COST; - const tree_node* parent_id = NULL; - bool is_in_queue = false; - double f = INF_COST; + int x, y; + double g = INF_COST; + double h = INF_COST; + const tree_node * parent_id = nullptr; + bool is_in_queue = false; + double f = INF_COST; }; struct comp @@ -110,14 +110,14 @@ class ThetaStar return !(isSafe(src_.x, src_.y)) || !(isSafe(dst_.x, dst_.y)); } - int nodes_opened = 0; + int nodes_opened = 0; protected: /// for the coordinates (x,y), it stores at node_position_[size_x_ * y + x], /// the pointer to the location at which the data of the node is present in nodes_data_ /// it is initialised with size_x_ * size_y_ elements /// and its number of elements increases to account for a change in map size - std::vector node_position_; + std::vector node_position_; /// the vector nodes_data_ stores the coordinates, costs and index of the parent node, /// and whether or not the node is present in queue_, for all the nodes searched @@ -126,7 +126,7 @@ class ThetaStar std::vector nodes_data_; /// this is the priority queue (open_list) to select the next node to be expanded - std::priority_queue, comp> queue_; + std::priority_queue, comp> queue_; /// it is a counter like variable used to generate consecutive indices /// such that the data for all the nodes (in open and closed lists) could be stored @@ -166,14 +166,16 @@ class ThetaStar * @param sl_cost is used to return the cost thus incurred * @return true if a line of sight exists between the points */ - bool losCheck(const int & x0, const int & y0, const int & x1, const int & y1, double & sl_cost) const; + bool losCheck( + const int & x0, const int & y0, const int & x1, const int & y1, + double & sl_cost) const; - /** - * @brief it returns the path by backtracking from the goal to the start, by using their parent nodes - * @param raw_points used to return the path thus found - * @param curr_id sends in the index of the goal coordinate, as stored in nodes_position - */ - void backtrace(std::vector & raw_points, const tree_node * curr_n) const; + /** +* @brief it returns the path by backtracking from the goal to the start, by using their parent nodes +* @param raw_points used to return the path thus found +* @param curr_id sends in the index of the goal coordinate, as stored in nodes_position +*/ + void backtrace(std::vector & raw_points, const tree_node * curr_n) const; /** * @brief it is an overloaded function to ease the cost calculations while performing the LOS check @@ -196,9 +198,9 @@ class ThetaStar * the actual costmap cost by 0.9 to keep the output in the range of [25, 255) */ inline double getCost(const int & cx, const int & cy) const - { - return 25 + 0.9*costmap_->getCost(cx, cy); - } + { + return 25 + 0.9 * costmap_->getCost(cx, cy); + } /** * @brief for the point(cx, cy), its traversal cost is calculated by @@ -268,7 +270,7 @@ class ThetaStar * @brief retrieves the pointer of the location at which the data of the point(cx, cy) is stored in nodes_data * @return id_this is the pointer to that location */ - inline tree_node* getIndex(const int & cx, const int & cy) + inline tree_node * getIndex(const int & cx, const int & cy) { return node_position_[size_x_ * cy + cx]; } @@ -296,7 +298,7 @@ class ThetaStar */ void clearQueue() { - queue_ = std::priority_queue, comp>(); + queue_ = std::priority_queue, comp>(); } }; } // namespace theta_star diff --git a/nav2_theta_star_planner/src/theta_star.cpp b/nav2_theta_star_planner/src/theta_star.cpp index 1f3c633692..e0267b34c0 100644 --- a/nav2_theta_star_planner/src/theta_star.cpp +++ b/nav2_theta_star_planner/src/theta_star.cpp @@ -103,7 +103,7 @@ void ThetaStar::resetParent(tree_node * curr_data) void ThetaStar::setNeighbors(const tree_node * curr_data) { int mx, my; - tree_node * m_id = NULL; + tree_node * m_id = nullptr; double g_cost, h_cost, cal_cost; for (int i = 0; i < how_many_corners_; i++) { @@ -123,7 +123,7 @@ void ThetaStar::setNeighbors(const tree_node * curr_data) m_id = getIndex(mx, my); - if (m_id == NULL) { + if (m_id == nullptr) { addToNodesData(index_generated_); m_id = &nodes_data_[index_generated_]; addIndex(mx, my, m_id); diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index 6fd4eb88be..84365b91ab 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -86,7 +86,7 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( getPlan(global_path); auto stop_time = std::chrono::steady_clock::now(); auto dur = std::chrono::duration_cast(stop_time - start_time); - RCLCPP_DEBUG(logger_, "the time taken is : %i", static_cast(dur.count())); + RCLCPP_INFO(logger_, "the time taken for pointy is : %i", static_cast(dur.count())); RCLCPP_DEBUG(logger_, "the nodes_opened are: %i", planner_->nodes_opened); return global_path; } diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp index f421b206fd..3deb02a4cc 100644 --- a/nav2_theta_star_planner/test/test_theta_star.cpp +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -45,11 +45,20 @@ class test_theta_star : public theta_star::ThetaStar bool uisGoal(const tree_node & this_node) {return isGoal(this_node);} - void uinitializePosn(int size_inc = 0) {initializePosn(size_inc);} + void uinitializePosn(int size_inc = 0) + { + node_position_.reserve(size_x_ * size_y_); initializePosn(size_inc); + } + + void uaddIndex(const int & cx, const int & cy) {addIndex(cx, cy, &nodes_data_[0]);} + + tree_node * ugetIndex(const int & cx, const int & cy) {return getIndex(cx, cy);} - void uaddIndex(const int & cx, const int & cy, const int & id_this) {addIndex(cx, cy, id_this);} + tree_node * test_getIndex() {return &nodes_data_[0];} - void ugetIndex(const int & cx, const int & cy, int & id_this) {getIndex(cx, cy, id_this);} + void uaddToNodesData(const int & id) {addToNodesData(id);} + + void uresetContainers() {nodes_data_.clear(); resetContainers();} bool runAlgo(std::vector & path) { @@ -61,82 +70,83 @@ class test_theta_star : public theta_star::ThetaStar }; init_rclcpp node; -auto planner_ = std::make_unique(); // Tests meant to test the algorithm itself and its helper functions -TEST(ThetaStarTest, test_theta_star) -{ +TEST(ThetaStarTest, test_theta_star) { + auto planner_ = std::make_unique(); planner_->costmap_ = new nav2_costmap_2d::Costmap2D(50, 50, 1.0, 0.0, 0.0, 0); - for (int i = 20; i <= 30; i++) { - for (int j = 20; j <= 30; j++) { + for (int i = 7; i <= 14; i++) { + for (int j = 7; j <= 14; j++) { planner_->costmap_->setCost(i, j, 253); } } - - planner_->size_x_ = 50; - planner_->size_y_ = 50; + coordsM s = {5, 5}, g = {18, 18}; + int size_x = 20, size_y = 20; + planner_->size_x_ = size_x; + planner_->size_y_ = size_y; geometry_msgs::msg::PoseStamped start, goal; - start.pose.position.x = 10; - start.pose.position.y = 10; + start.pose.position.x = s.x; + start.pose.position.y = s.y; start.pose.orientation.w = 1.0; - goal.pose.position.x = 40; - goal.pose.position.y = 35; + goal.pose.position.x = g.x; + goal.pose.position.y = g.y; goal.pose.orientation.w = 1.0; /// Check if the setStartAndGoal function works properly planner_->setStartAndGoal(start, goal); - EXPECT_TRUE(planner_->src_.x == 10 && planner_->src_.y == 10); - EXPECT_TRUE(planner_->dst_.x == 40 && planner_->dst_.y == 35); + EXPECT_TRUE(planner_->src_.x == s.x && planner_->src_.y == s.y); + EXPECT_TRUE(planner_->dst_.x == g.x && planner_->dst_.y == g.y); /// Check if the initializePosn function works properly - planner_->uinitializePosn(50 * 50); - EXPECT_EQ(planner_->getSizeOfNodePosition(), (50 * 50)) << "passed this 1"; + planner_->uinitializePosn(size_x * size_y); + EXPECT_EQ(planner_->getSizeOfNodePosition(), (size_x * size_y)); /// Check if the withinLimits function works properly - EXPECT_TRUE(planner_->uwithinLimits(20, 40)); + EXPECT_TRUE(planner_->uwithinLimits(18, 18)); EXPECT_FALSE(planner_->uwithinLimits(120, 140)); - tree_node n = {40, 35, 120, 0, 1, false, 20}; + tree_node n = {g.x, g.y, 120, 0, NULL, false, 20}; + n.parent_id = &n; /// Check if the isGoal function works properly - EXPECT_TRUE(planner_->uisGoal(n)); // both (x,y) are the goal coordinates + EXPECT_TRUE(planner_->uisGoal(n)); // both (x,y) are the goal coordinates n.x = 25; - EXPECT_FALSE(planner_->uisGoal(n)); // only y coordinate matches with that of goal - n.x = 40; + EXPECT_FALSE(planner_->uisGoal(n)); // only y coordinate matches with that of goal + n.x = g.x; n.y = 20; - EXPECT_FALSE(planner_->uisGoal(n)); // only x coordinate matches with that of goal + EXPECT_FALSE(planner_->uisGoal(n)); // only x coordinate matches with that of goal n.x = 30; - EXPECT_FALSE(planner_->uisGoal(n)); // both (x, y) are different from the goal coordinate + EXPECT_FALSE(planner_->uisGoal(n)); // both (x, y) are different from the goal coordinate /// Check if the isSafe functions work properly - EXPECT_TRUE(planner_->isSafe(10, 30)); // cost at this point is 0 - EXPECT_FALSE(planner_->isSafe(25, 25)); // cost at this point is 253 (>LETHAL_COST) + EXPECT_TRUE(planner_->isSafe(5, 5)); // cost at this point is 0 + EXPECT_FALSE(planner_->isSafe(10, 10)); // cost at this point is 253 (>LETHAL_COST) - int c_id; - /// Check if the addIndex & getIndex work properly + /// Check if the functions addIndex & getIndex work properly coordsM c = {20, 30}; - planner_->uaddIndex(c.x, c.y, 0); - planner_->ugetIndex(c.x, c.y, c_id); - EXPECT_EQ(c_id, 0); + planner_->uaddToNodesData(0); + planner_->uaddIndex(c.x, c.y); + tree_node * c_node = planner_->ugetIndex(c.x, c.y); + EXPECT_EQ(c_node, planner_->test_getIndex()); double sl_cost = 0.0; /// Checking for the case where the losCheck should return the value as true - EXPECT_TRUE(planner_->ulosCheck(10, 10, 40, 10, sl_cost)); + EXPECT_TRUE(planner_->ulosCheck(2, 2, 7, 20, sl_cost)); /// and as false - EXPECT_FALSE(planner_->ulosCheck(10, 25, 30, 25, sl_cost)); + EXPECT_FALSE(planner_->ulosCheck(2, 2, 18, 18, sl_cost)); + planner_->uresetContainers(); std::vector path; /// Check if the planner returns a path for the case where a path exists EXPECT_TRUE(planner_->runAlgo(path)); EXPECT_GT(static_cast(path.size()), 0); /// and where it doesn't exist path.clear(); - planner_->src_ = {25, 25}; + planner_->src_ = {10, 10}; EXPECT_FALSE(planner_->runAlgo(path)); EXPECT_EQ(static_cast(path.size()), 0); } // Smoke tests meant to detect issues arising from the plugin part rather than the algorithm -TEST(ThetaStarPlanner, test_theta_star_planner) -{ +TEST(ThetaStarPlanner, test_theta_star_planner) { rclcpp_lifecycle::LifecycleNode::SharedPtr life_node = std::make_shared("ThetaStarPlannerTest"); From e9f3177fd4d13f05a2c38d808c970d79da92e8e6 Mon Sep 17 00:00:00 2001 From: Anshu-man567 Date: Sat, 19 Jun 2021 01:33:01 +0530 Subject: [PATCH 73/81] fix linting issues --- .../include/nav2_theta_star_planner/theta_star.hpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp index d3ba6a0b86..7a81bafc58 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp @@ -95,8 +95,8 @@ class ThetaStar } /** - * @brief initialises the values of the start and goal points - */ + * @brief initialises the values of the start and goal points + */ void setStartAndGoal( const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal); @@ -171,10 +171,10 @@ class ThetaStar double & sl_cost) const; /** -* @brief it returns the path by backtracking from the goal to the start, by using their parent nodes -* @param raw_points used to return the path thus found -* @param curr_id sends in the index of the goal coordinate, as stored in nodes_position -*/ + * @brief it returns the path by backtracking from the goal to the start, by using their parent nodes + * @param raw_points used to return the path thus found + * @param curr_id sends in the index of the goal coordinate, as stored in nodes_position + */ void backtrace(std::vector & raw_points, const tree_node * curr_n) const; /** From 34e5b31e92d2346f8cd429106290eb1a8050334c Mon Sep 17 00:00:00 2001 From: Anshumaan Singh Date: Sat, 19 Jun 2021 01:44:27 +0530 Subject: [PATCH 74/81] Update README.md --- nav2_theta_star_planner/README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index d2b32de0d3..84905c2249 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -2,12 +2,12 @@ The Theta Star Planner is a global planning plugin meant to be used with the Nav2 Planner Server. The `nav2_theta_star_planner` implements a highly optimized version of the Theta\* Planner (specifically the [Lazy Theta\* P variant](http://idm-lab.org/bib/abstracts/papers/aaai10b.pdf)) meant to plan any-angle paths using A\*. The planner supports differential-drive and omni-directional robots. ## Features +- The planner uses A\* search along with line of sight (LOS) checks to form any-angle paths thus avoiding zig-zag paths that may be present in the usual implementation of A\* +- As it also considers the costmap traversal cost during execution (along with LOS checks) it tends to smoothen the paths automatically, thus mitigating the need to smoothen the path (The presence of sharp turns depends on the resolution of the map, and it decreases as the map resolution increases) - Uses the costs from the costmap to penalise high cost regions -- Is well suited for smaller robots of the omni-directional and differential drive kind -- As it considers the costmap traversal cost during execution it tends to smoothen the paths automatically, thus mitigating the need to smoothen the path. (The presence of sharp turns depends on the resolution of the map, and it decreases as the map resolution increases.) - Allows to control the path behavior to either be any angle directed or to be in the middle of the spaces -- The planner uses A\* search along with line of sight checks to form any-angle paths thus avoiding zig-zag paths that may be present in the usual implementation of A\* -- The algorithmic part of the planner has been segregated from the plugin part to allow for reusability. +- Is well suited for smaller robots of the omni-directional and differential drive kind +- The algorithmic part of the planner has been segregated from the plugin part to allow for reusability ## Metrics In general the planner is capable of planning for a map of size (566 x 608) from 30ms to 80ms (averaged value, it might exceed the stated time limits here for some iterations depending on how the parameters have been tuned). From 9b046bf325dc2d7377ff0478e726874fe92e735e Mon Sep 17 00:00:00 2001 From: Anshumaan Singh Date: Sat, 19 Jun 2021 01:47:09 +0530 Subject: [PATCH 75/81] Update README.md --- nav2_theta_star_planner/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index 84905c2249..ea358b549d 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -3,7 +3,7 @@ The Theta Star Planner is a global planning plugin meant to be used with the Nav ## Features - The planner uses A\* search along with line of sight (LOS) checks to form any-angle paths thus avoiding zig-zag paths that may be present in the usual implementation of A\* -- As it also considers the costmap traversal cost during execution (along with LOS checks) it tends to smoothen the paths automatically, thus mitigating the need to smoothen the path (The presence of sharp turns depends on the resolution of the map, and it decreases as the map resolution increases) +- As it also considers the costmap traversal cost during execution it tends to smoothen the paths automatically, thus mitigating the need to smoothen the path (The presence of sharp turns depends on the resolution of the map, and it decreases as the map resolution increases) - Uses the costs from the costmap to penalise high cost regions - Allows to control the path behavior to either be any angle directed or to be in the middle of the spaces - Is well suited for smaller robots of the omni-directional and differential drive kind From ad184e0c4c9ff4898bd933c62b4cb16efb1bd75d Mon Sep 17 00:00:00 2001 From: Anshumaan Singh Date: Sat, 19 Jun 2021 01:58:06 +0530 Subject: [PATCH 76/81] Update README.md --- nav2_theta_star_planner/README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index ea358b549d..5bc58d2ee0 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -77,16 +77,16 @@ Before starting off, do note that the costmap_cost(curr,neigh) component after b This planner uses the costs associated with each cell from the `global_costmap` as a measure of the point's proximity to the obstacles. Providing a gentle potential field that covers the entirety of the region (thus leading to only small pocket like regions of cost = 0) is recommended in order to achieve paths that pass through the middle of the spaces. A good starting point could be to set the `inflation_layer`'s parameters as - `cost_scaling_factor:10.0`, `inflation_radius: 5.5` and then to decrease the value of `cost_scaling_factor` to achieve the said potential field. -Providing a gentle potential field over the region allows the planner to compensate for the increase in path lengths by leading to an increase in the distance from the obstacles, which around a corner allows for naturally smoothing the turns and thus removing the requirement for the use of an external path smoother. +Providing a gentle potential field over the region allows the planner to exchange the increase in path lengths for a decrease in the accumulated traversal cost, thus leading to an increase in the distance from the obstacles. This around a corner allows for naturally smoothing the turns and removes the requirement for an external path smoother. -In order to achieve paths that stay in the middle of the spaces set `w_traversal_cost` at a higher value than `w_euc_cost`. To begin with, you can set the parameters to its default values and then increase the value of `w_traversal_cost`, while also decreasing `w_euc_cost` to allow for an increase in the path length thus letting the path to settle in the middle regions of the spaces with lower costs on the costmap. While tuning the planner's parameters you can also change the `inflation_layer`'s parameters to tune the behavior of the paths. +In order to achieve paths that stay in the middle of the spaces set `w_traversal_cost` at a higher value than `w_euc_cost`. To begin with, you can set the parameters to its default values and then increase the value of `w_traversal_cost` which, if required can be accompanied with a decrease in `w_euc_cost` to allow for an increase in the path length thus letting the path to settle in the middle regions of the spaces with lower costs on the costmap. While tuning the planner's parameters you can also change the `inflation_layer`'s parameters (of the global costmap) to tune the behavior of the paths. ### Path Smoothing Because of how the cost function works, the output path has a natural tendency to form smooth curves around corners, though the smoothness of the path depends on how wide the turn is, and the number of cells in that turn. This planner is recommended to be used with local planners like DWB or TEB (or other any local planner / controllers that form a local trajectory to be traversed) as these take into account the abrupt turns which might arise due to the planner not being able to find a smoother turns owing to the aforementioned reasons. -While smoother paths can be achieved by increasing the costmap resolution (ie using a costmap of 1cm resolution rather than a 5cm one) it is not recommended doing so as it comes at the cost of increased query times from the planner. Test the planners performance on the finer costmaps before making a switch to those costmaps. +While smoother paths can be achieved by increasing the costmap resolution (ie using a costmap of 1cm resolution rather than a 5cm one) it is not recommended to do so because it might increase the query times from the planner. Test the planners performance on the higher cm/px costmaps before making a switch to finer costmaps. ### Possible Applications This planner could be used in scenarios where planning speed matters over an extremely smooth path, which could anyways be handled by using a local planner/controller as mentioned above. Because of the any-angled nature of paths you can traverse environments diagonally (wherever it is allowed, eg: in a wide open region). From 94c6f6abd8deeddc42c75422638b0acaaa64d1ae Mon Sep 17 00:00:00 2001 From: Anshumaan Singh Date: Mon, 21 Jun 2021 20:49:49 +0530 Subject: [PATCH 77/81] Update theta_star_planner.cpp --- nav2_theta_star_planner/src/theta_star_planner.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index 84365b91ab..437cbd4471 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -86,7 +86,7 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( getPlan(global_path); auto stop_time = std::chrono::steady_clock::now(); auto dur = std::chrono::duration_cast(stop_time - start_time); - RCLCPP_INFO(logger_, "the time taken for pointy is : %i", static_cast(dur.count())); + RCLCPP_DEBUG(logger_, "the time taken for pointy is : %i", static_cast(dur.count())); RCLCPP_DEBUG(logger_, "the nodes_opened are: %i", planner_->nodes_opened); return global_path; } From 5d81029a1943c0c6d2dc2bda308a2e355957c8ce Mon Sep 17 00:00:00 2001 From: Anshu-man567 Date: Thu, 24 Jun 2021 10:39:11 +0530 Subject: [PATCH 78/81] update LETHAL_COST --- .../include/nav2_theta_star_planner/theta_star.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp index 7a81bafc58..dacd8a57bc 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp @@ -24,7 +24,7 @@ #include "nav2_costmap_2d/costmap_2d_ros.hpp" const double INF_COST = DBL_MAX; -const int LETHAL_COST = 250; +const int LETHAL_COST = 252; struct coordsM { @@ -199,7 +199,7 @@ class ThetaStar */ inline double getCost(const int & cx, const int & cy) const { - return 25 + 0.9 * costmap_->getCost(cx, cy); + return 26 + 0.9 * costmap_->getCost(cx, cy); } /** From 13b8cded246d7d5c8f394a10e8b0b29b5e8d7be0 Mon Sep 17 00:00:00 2001 From: Anshu-man567 Date: Thu, 24 Jun 2021 11:58:55 +0530 Subject: [PATCH 79/81] update README.md --- nav2_theta_star_planner/README.md | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index 5bc58d2ee0..ed3f810f09 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -10,8 +10,6 @@ The Theta Star Planner is a global planning plugin meant to be used with the Nav - The algorithmic part of the planner has been segregated from the plugin part to allow for reusability ## Metrics -In general the planner is capable of planning for a map of size (566 x 608) from 30ms to 80ms (averaged value, it might exceed the stated time limits here for some iterations depending on how the parameters have been tuned). - For the below example the planner took ~46ms (averaged value) to compute the path of 87.5m - ![example.png](img/00-37.png) @@ -53,7 +51,7 @@ The parameters of the planner are : - ` .how_many_corners ` : to choose between 4-connected and 8-connected graph expansions, the accepted values are 4 and 8 - ` .w_euc_cost ` : weight applied on the length of the path. - ` .w_traversal_cost ` : it tunes how harshly the nodes of high cost are penalised. From the above g(neigh) equation you can see that the cost-aware component of the cost function forms a parabolic curve, thus this parameter would, on increasing its value, make that curve steeper allowing for a greater differentiation (as the delta of costs would increase, when the graph becomes steep) among the nodes of different costs. -- ` .w_heuristic_cost ` : it has been provided to have an admissible heuristic, so the recommendation would be to change its value only when required. Usually set is at the same value as `w_euc_cost` or 1.0 (whichever is lower), though you may increase the value of `w_heuristic_cost` to speed up the process. +- ` .w_heuristic_cost ` : it has been provided to have an admissible heuristic Below are the default values of the parameters : ``` @@ -68,18 +66,23 @@ planner_server: w_traversal_cost: 2.0 w_heuristic_cost: 1.0 ``` -Do note that the `global_costmap`'s `inflation_layer` values recommended for the above values of the parameter are - `cost_scaling_factor:4.0`, `inflation_radius: 5.5` ## Usage Notes ### Tuning the Parameters Before starting off, do note that the costmap_cost(curr,neigh) component after being operated (ie before being multiplied to its parameter and being substituted in g(init_rclcpp)) varies from 0 to 1. -This planner uses the costs associated with each cell from the `global_costmap` as a measure of the point's proximity to the obstacles. Providing a gentle potential field that covers the entirety of the region (thus leading to only small pocket like regions of cost = 0) is recommended in order to achieve paths that pass through the middle of the spaces. A good starting point could be to set the `inflation_layer`'s parameters as - `cost_scaling_factor:10.0`, `inflation_radius: 5.5` and then to decrease the value of `cost_scaling_factor` to achieve the said potential field. +This planner uses the costs associated with each cell from the `global_costmap` as a measure of the point's proximity to the obstacles. Providing a gentle potential field that covers the entirety of the region (thus leading to only small pocket like regions of cost = 0) is recommended in order to achieve paths that pass through the middle of the spaces. A good starting point could be to set the `inflation_layer`'s parameters as - `cost_scaling_factor: 10.0`, `inflation_radius: 5.5` and then decrease the value of `cost_scaling_factor` to achieve the said potential field. Providing a gentle potential field over the region allows the planner to exchange the increase in path lengths for a decrease in the accumulated traversal cost, thus leading to an increase in the distance from the obstacles. This around a corner allows for naturally smoothing the turns and removes the requirement for an external path smoother. -In order to achieve paths that stay in the middle of the spaces set `w_traversal_cost` at a higher value than `w_euc_cost`. To begin with, you can set the parameters to its default values and then increase the value of `w_traversal_cost` which, if required can be accompanied with a decrease in `w_euc_cost` to allow for an increase in the path length thus letting the path to settle in the middle regions of the spaces with lower costs on the costmap. While tuning the planner's parameters you can also change the `inflation_layer`'s parameters (of the global costmap) to tune the behavior of the paths. +To begin with, you can set the parameters to its default values and then try increasing the value of `w_traversal_cost` to achieve those middling paths, but this would adversly make the paths less taut. So it is recommend ed that you simultaneously tune the value of `w_euc_cost`. Increasing `w_euc_cost` increases the tautness of the path, which leads to more straight line like paths (any-angled paths). Do note that the query time from the planner would increase for higher values of `w_traversal_cost` as more nodes would have to be expanded to lower the cost of the path, to counteract this you may also try setting `w_euc_cost` to a higher value and check if it reduces the query time. + +Usually set `w_heuristic_cost` at the same value as `w_euc_cost` or 1.0 (whichever is lower). Though as a last resort you may increase the value of `w_heuristic_cost` to speed up the planning process, this is not recommended as it might not always lead to shorter query times, but would definitely give you sub optimal paths + +Also note that changes to `w_traversal_cost` might cause slow downs, in case the number of node expanisions increase thus tuning it along with `w_euc_cost` is the way to go to. + +While tuning the planner's parameters you can also change the `inflation_layer`'s parameters (of the global costmap) to tune the behavior of the paths. ### Path Smoothing Because of how the cost function works, the output path has a natural tendency to form smooth curves around corners, though the smoothness of the path depends on how wide the turn is, and the number of cells in that turn. From bad3d94ddb507a4217c99125eafa874be97eeb77 Mon Sep 17 00:00:00 2001 From: Anshu-man567 Date: Thu, 24 Jun 2021 12:10:28 +0530 Subject: [PATCH 80/81] update dependency list --- nav2_theta_star_planner/CMakeLists.txt | 2 +- nav2_theta_star_planner/package.xml | 19 +++++++++++-------- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/nav2_theta_star_planner/CMakeLists.txt b/nav2_theta_star_planner/CMakeLists.txt index 6eabb292c2..8cf8a28c35 100644 --- a/nav2_theta_star_planner/CMakeLists.txt +++ b/nav2_theta_star_planner/CMakeLists.txt @@ -13,7 +13,6 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(tf2_ros REQUIRED) -find_package(ament_cmake_gtest REQUIRED) nav2_package() #Calls the nav2_package.cmake file add_compile_options(-O3) @@ -69,6 +68,7 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) set(gtest_disable_pthreads OFF) ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(test_theta_star test/test_theta_star.cpp) ament_target_dependencies(test_theta_star ${dependencies}) target_link_libraries(test_theta_star ${library_name}) diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index 7624bfebe1..2c99e52bf0 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -9,20 +9,23 @@ Apache-2.0 ament_cmake - rclcpp + + builtin_interfaces + nav2_common nav2_core + nav2_costmap_2d + nav2_msgs + nav2_util + pluginlib + rclcpp rclcpp_action rclcpp_lifecycle - std_msgs - visualization_msgs - nav2_util - nav2_msgs - builtin_interfaces tf2_ros - nav2_costmap_2d - pluginlib + ament_lint_auto ament_lint_common + ament_gtest + ament_cmake From 984a7a052105cdcf151853e06af9756194591700 Mon Sep 17 00:00:00 2001 From: Anshumaan Singh Date: Thu, 24 Jun 2021 12:15:08 +0530 Subject: [PATCH 81/81] Update package.xml --- nav2_theta_star_planner/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index 2c99e52bf0..67a4839abf 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -24,7 +24,7 @@ ament_lint_auto ament_lint_common - ament_gtest + ament_cmake_gtest ament_cmake