Merge remote-tracking branch 'origin' into

createRoute

Author: alyashour

CMakeLists.txt

@@ -11,6 +11,9 @@ find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(ap1_msgs REQUIRED)
+find_package(lanelet2_core REQUIRED)
+find_package(lanelet2_io REQUIRED)
+find_package(lanelet2_projection REQUIRED)
 
 add_executable(planner_node
   src/main.cpp
@@ -29,6 +32,9 @@ ament_target_dependencies(
   std_msgs
   geometry_msgs
   ap1_msgs
+  lanelet2_core
+  lanelet2_io
+  lanelet2_projection
 )
 
 install(TARGETS

README.md

@@ -1,5 +1,10 @@
 # Planning
 
+## Dependencies
+
+Make sure you pull, build, and install `ap1_msgs`. 
+You'll also need `ros-jazzy-lanelet2`. So `sudo apt install ros-jazzy-lanelet2` if you haven't already.
+
 ## Compilaton
 
 > Note: If you want clangd to provide good autocorrect do: `colcon build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=1` and `cp build/compile_commands.json .`

include/ap1/planning/planner_node.hpp

@@ -7,11 +7,18 @@
 #define AP1_PLANNING_NODE_HPP
 
 #include <cmath>
+#include <functional>
 #include <rclcpp/timer.hpp>
+#include <string>
+#include <vector>
 
 #include "geometry_msgs/msg/point.hpp"
 #include "rclcpp/rclcpp.hpp"
+
 #include "std_msgs/msg/float32_multi_array.hpp"
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_io/Io.h>
+#include <lanelet2_projection/UTM.h>
 
 #include "ap1_msgs/msg/speed_profile_stamped.hpp"
 #include "ap1_msgs/msg/target_path_stamped.hpp"
@@ -44,10 +51,19 @@ class PlannerNode : public rclcpp::Node
      */
     PlannerNode(double rate_hz = 60.0);
 
+    struct Lane
+    {
+        std::vector<geometry_msgs::msg::Point> left_boundary;
+        std::vector<geometry_msgs::msg::Point> right_boundary;
+    };
+
   private:
     float speed_ = 0;
     const double rate_hz_;
     Point target_location_;
+    Lane current_lane_;
+    std::string map_file_path_;
+    lanelet::LaneletMapPtr lanelet_map_;
 
     TimerBase::SharedPtr timer_;
 
@@ -69,6 +85,18 @@ class PlannerNode : public rclcpp::Node
     void on_target_location(const geometry_msgs::msg::Point::SharedPtr loc);
     void on_target_speed(const VehicleSpeedStamped::SharedPtr);
 
+    /**
+     * @brief Mocks map data for testing.
+     */
+    void process_map_data();
+
+    /**
+     * @brief Calculates the centerline from the current lane boundaries.
+     * @param lane The lane containing left and right boundaries.
+     * @return std::vector<geometry_msgs::msg::Point> The calculated centerline.
+     */
+    std::vector<geometry_msgs::msg::Point> calculate_centerline(const Lane& lane);
+
     /**
      * @brief Planning loop callback runs rate_hz times per second.
      * This callback is responsible for sending commands & updates to control.

package.xml

@@ -13,6 +13,9 @@
   <depend>std_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>ap1_msgs</depend>
+  <depend>lanelet2_core</depend>
+  <depend>lanelet2_io</depend>
+  <depend>lanelet2_projection</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>