Issues #17 and #18

.gitignore

@@ -2,4 +2,4 @@ __pycache__
 build
 install
 log
-.vscode
+.vscode

src/planning/path_planner/CMakeLists.txt

@@ -0,0 +1,55 @@
+#
+# Package:   path_planner
+# Filename:  CMakeLists.txt
+# Author:    Jim Moore
+# Email:     jim3moore@gmail.com
+# Copyright: 2021, Nova UTD
+# License:   MIT License
+#
+cmake_minimum_required(VERSION 3.5)
+
+project(path_planner)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# dependencies
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+# Build a library from our source files. This is just a convenience
+# mechanism to prevent having to explicitly list all files for every
+# executable.
+ament_auto_add_library(path_planner_lib STATIC
+  src/path_planner.cpp
+  src/parameterized_spline.cpp
+  src/map_utils.cpp
+  include/path_planner/spline.hpp
+  include/path_planner/lane_points.hpp
+)
+target_include_directories(path_planner_lib PUBLIC include)
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)   # General test dependency
+  ament_lint_auto_find_test_dependencies() # Find any test dependencies declared in package.xml
+
+  ament_add_gtest(tests
+    test/test_path_planner.cpp
+    test/test_parameterized_spline.cpp)
+
+  ament_target_dependencies(tests
+    autoware_auto_common autoware_auto_geometry
+    autoware_auto_msgs
+    had_map_utils)
+
+  target_link_libraries(tests path_planner_lib gtest_main)
+endif()
+
+# ament package generation and installing
+ament_auto_package()

src/planning/path_planner/design/path_planner-design.md

@@ -0,0 +1,8 @@
+Path planner {#path-planner}
+===========
+
+This is the design document for the `path_planner` package.
+
+# Purpose / Use cases
+Path Planner is used to determine trajectory
+It will use centerline of lanes obtained from HADMap, convert it to cubic spline, and convert it into a set of potential trajectories. It selects the trajectory that minimizes a cost function.

src/planning/path_planner/include/path_planner/lane_points.hpp

@@ -0,0 +1,36 @@
+/*
+ * Package:   path_planner
+ * Filename:  lane_points.hpp
+ * Author:    Jim Moore
+ * Email:     jim3moore@gmail.com
+ * Copyright: 2021, Nova UTD
+ * License:   MIT License
+ */
+
+#ifndef PATH_PLANNER__LANE_POINTS_HPP_
+#define PATH_PLANNER__LANE_POINTS_HPP_
+
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/primitives/Point.h>
+
+namespace navigator
+{
+    namespace path_planner
+    {
+        //represents a sampling of points for the left boundary, center line, and right boundary of a lane
+
+        class LanePoints
+        {
+        public:
+            const lanelet::LineString3d left;
+            const lanelet::LineString3d center;
+            const lanelet::LineString3d right;
+            LanePoints(lanelet::LineString3d left, lanelet::LineString3d center, lanelet::LineString3d right)
+                : left(left), center(center), right(right)
+            {
+            }
+        };
+    }
+}
+
+#endif

src/planning/path_planner/include/path_planner/map_utils.hpp

@@ -0,0 +1,42 @@
+/*
+ * Package:   path_planner
+ * Filename:  map_utils.hpp
+ * Author:    Jim Moore
+ * Email:     jim3moore@gmail.com
+ * Copyright: 2021, Nova UTD
+ * License:   MIT License
+ */
+
+#ifndef PATH_PLANNER_MAP_UTILS_HPP
+#define PATH_PLANNER_MAP_UTILS_HPP
+
+
+#include "autoware_auto_msgs/msg/trajectory.hpp"
+#include <lanelet2_core/geometry/Lanelet.h>
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/primitives/Point.h>
+#include <lanelet2_core/geometry/LineString.h>
+#include <autoware_auto_msgs/msg/trajectory.hpp>
+#include <autoware_auto_msgs/msg/had_map_route.hpp>
+#include <had_map_utils/had_map_utils.hpp>
+#include <path_planner/lane_points.hpp>
+#include <common/types.hpp>
+#include <geometry/common_2d.hpp>
+
+using namespace autoware::common::types;
+using TrajectoryPoint = autoware_auto_msgs::msg::TrajectoryPoint;
+using autoware_auto_msgs::msg::HADMapRoute;
+using autoware_auto_msgs::msg::Trajectory;
+
+namespace navigator
+{
+    namespace path_planner
+    {
+        namespace map_utils {
+            LanePoints sample_center_line_and_boundaries(
+            const lanelet::ConstLanelet &lanelet_obj, const float64_t resolution);
+        }
+    }
+}
+
+#endif

src/planning/path_planner/include/path_planner/parameterized_spline.hpp

@@ -0,0 +1,45 @@
+/*
+ * Package:   path_planner
+ * Filename:  parameterized_spline.hpp
+ * Author:    Jim Moore
+ * Email:     jim3moore@gmail.com
+ * Copyright: 2021, Nova UTD
+ * License:   MIT License
+ */
+
+#ifndef PATH_PLANNER__PARAMETERIZED_SPLINE_HPP_
+#define PATH_PLANNER__PARAMETERIZED_SPLINE_HPP_
+
+#include <utility>
+#include <path_planner/spline.hpp>
+
+#pragma GCC diagnostic ignored "-Wsubobject-linkage" //disable this warning since the spline library uses an anonymous namespace
+
+namespace navigator
+{
+    namespace path_planner
+    {
+        // represents a cubic spline:
+        //   x(s) = a_x*s^3 + b_x*s^2 + c_x*s + d_x
+        //   y(s) = a_y*s^3 + b_y*s^2 + c_y*s + d_y
+        // we can sample a point at a given length along the curve s by evaluating (x(s),y(s))
+        //      the arc length is based on the chord between points, so it's an approximation.
+        //      using points close in distance helps
+        // NOTE: points should be < 0.1 units away from each other for a good approximation
+
+        class ParameterizedSpline
+        {
+        private:
+            tk::spline sx, sy;
+
+        public:
+            ParameterizedSpline(tk::spline sx, tk::spline sy)
+                : sx(sx), sy(sy) {}
+            // NOTE: points should be < 0.1 units away from each other for a good approximation
+            ParameterizedSpline(const std::vector<double> &x, const std::vector<double> &y);
+            std::pair<double, double> sample(double s);
+        };
+    }
+}
+
+#endif

src/planning/path_planner/include/path_planner/path_planner.hpp

@@ -0,0 +1,50 @@
+/*
+ * Package:   path_planner
+ * Filename:  path_planner.hpp
+ * Author:    Jim Moore
+ * Email:     jim3moore@gmail.com
+ * Copyright: 2021, Nova UTD
+ * License:   MIT License
+ */
+
+#ifndef PATH_PLANNER__PATH_PLANNER_HPP_
+#define PATH_PLANNER__PATH_PLANNER_HPP_
+
+#include <path_planner/parameterized_spline.hpp>
+#include <path_planner/lane_points.hpp>
+#include <autoware_auto_msgs/msg/trajectory.hpp>
+#include <autoware_auto_msgs/msg/had_map_route.hpp>
+#include <common/types.hpp>
+
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/primitives/Point.h>
+#include <iostream>
+#include <memory>
+#include <vector>
+
+using TrajectoryPoint = autoware_auto_msgs::msg::TrajectoryPoint;
+using autoware_auto_msgs::msg::HADMapRoute;
+using autoware_auto_msgs::msg::Trajectory;
+
+
+namespace navigator
+{
+    namespace path_planner
+    {
+        //generates potential paths using cubic splines and gives them costs according to several factors
+        //right now only generates the center line for a lane and the spline for that
+        class PathPlanner
+        {
+        public:
+            std::vector<TrajectoryPoint> generate_position_trajectory(const HADMapRoute &route, const lanelet::LaneletMapConstPtr &map);
+            ParameterizedSpline get_center_line_spline(const std::vector<autoware_auto_msgs::msg::TrajectoryPoint> &line_points);
+            std::vector<autoware_auto_msgs::msg::TrajectoryPoint> get_center_line_points(const HADMapRoute &route, const lanelet::LaneletMapConstPtr &map, double resolution);
+
+        private:
+            //probably need to find a way to bound start and end window
+
+        };
+    }
+}
+
+#endif // PATH_PLANNER__PATH_PLANNER_HPP_