Add lazy_theta_star

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

lazy_theta_star_planner/global_planner_plugin.xml

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

lazy_theta_star_planner/include/lazy_theta_star_planner/lazy_theta_star2.h

@@ -0,0 +1,136 @@
+// 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 <cmath>
+#include <vector>
+#include <queue>
+#include <algorithm>
+#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<typename coordsT>
+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<id> posn;
+  std::vector<tree_node> data;
+  std::priority_queue<pos, std::vector<pos>, comp> pq;
+  int sizeX, sizeY;
+  int how_many_corners;
+  id id_gen;
+  coords<map_pts> 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<coords<world_pts>> & 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<coords<world_pts>> & 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_

lazy_theta_star_planner/include/lazy_theta_star_planner/lazy_theta_star_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_PLANNER__LAZY_THETA_STAR_PLANNER_H_
+#define LAZY_THETA_STAR_PLANNER__LAZY_THETA_STAR_PLANNER_H_
+
+#include <iostream>
+#include <cmath>
+#include <string>
+#include <chrono>
+#include <queue>
+#include <algorithm>
+#include <memory>
+#include <vector>
+#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<tf2_ros::Buffer> 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<lazyThetaStar::LazyThetaStar> planner_;
+
+  unsigned int src_[2], dst_[2];
+
+  void configure(
+    rclcpp_lifecycle::LifecycleNode::SharedPtr parent,
+    std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> 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<coords<world_pts>> & raw_path,
+    double dist_bw_points);
+};
+}   // namespace lazyThetaStarPlanner
+
+#endif  // LAZY_THETA_STAR_PLANNER__LAZY_THETA_STAR_PLANNER_H_

lazy_theta_star_planner/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_planner</name>
+  <version>0.0.1</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">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>