Add lazy_theta_star

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()

lazy_theta_star_b/global_planner_plugin.xml

@@ -0,0 +1,5 @@
+<library path="lazy_theta_star_b_plugin">
+    <class name="lazy_theta_star_b/LazyThetaStarB" type="lazy_theta_star_b::LazyThetaStarB" base_class_type="nav2_core::GlobalPlanner">
+        <description>The implementation of lazy theta_star</description>
+    </class>
+</library>

lazy_theta_star_b/include/lazy_theta_star_b/lazy_theta_star_b.h

@@ -0,0 +1,176 @@
+/*********************************************************************
+ *
+ * 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<cmath>
+#include<iostream>
+#include<cstdlib>
+#include<cstring>
+#include <string>
+#include<queue>
+#include<algorithm>
+
+#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<typename ptsT>
+    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 <tf2_ros::Buffer> tf_;
+        nav2_util::LifecycleNode::SharedPtr node_;
+        nav2_costmap_2d::Costmap2D *costmap_;
+        std::string global_frame_, name_;
+
+        double interpolation_dist;
+        int how_many_corners_;
+        pts<int> src_{}, dst_{};
+
+        //stores the cell data
+        vector<tree_node> data;
+
+        //stores the raw path given by the planner on the basis of their index in the vector data
+        vector<id> path;
+        nav_msgs::msg::Path global_path;
+
+        // is the priority queue
+        vector<pos> pq;
+        int sizeX = 0;
+        int sizeY = 0;
+        int how_many_corners;
+        pts<int> src{}, dst{};
+
+
+        void configure(
+                rclcpp_lifecycle::LifecycleNode::SharedPtr parent,
+                std::string name, std::shared_ptr <tf2_ros::Buffer> tf,
+                std::shared_ptr <nav2_costmap_2d::Costmap2DROS> 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<id> lp, double dist_bw_points);
+
+        pts<double> 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 <id> *waypt, id curr_id);
+
+        //stores the index at which the node data is stored for a particular co-ordinate
+        vector<id> 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<id> lp);
+    };
+
+}
+

lazy_theta_star_b/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>lazy_theta_star_b</name>
+  <version>0.0.1</version>
+  <description>TODO: Package description</description>
+  <maintainer email="Anshu-man567@todo.todo">anshu-man</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>rclcpp</depend>
+  <depend>nav2_core</depend>
+  <depend>rclcpp_action</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>std_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>nav2_util</depend>
+  <depend>nav2_msgs</depend>
+  <depend>builtin_interfaces</depend>
+  <depend>tf2_ros</depend>
+  <depend>nav2_costmap_2d</depend>
+  <depend>pluginlib</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+    <nav2_core plugin="${prefix}/global_planner_plugin.xml" />
+  </export>
+</package>