Submit custom planner

arena_simulation_setup/configs/nav2/controllers/crowdnav/controller_config.yaml

@@ -1,5 +1,23 @@
 controller_plugins:
   - FollowPath
+
 controller_plugins_dict:
   FollowPath:
-    plugin: "nav2py_pas_crowdnav_controller::PasCrowdNavController"
+    plugin: "nav2_pas_crowdnav_controller/PasCrowdNavController"
+    lookahead_dist: 0.8
+    v_max: 2.0               # Normal cruise speed
+    v_min: 0.2               # Creep speed when arriving at goal
+    slowdown_dist: 1.0       # Start slowing down 1 meter before the goal
+    rotation_threshold: 0.35
+    transform_tolerance: 0.2
+
+  goal_checker:
+    plugin: "nav2_controller::SimpleGoalChecker"
+    xy_goal_tolerance: 0.35
+    yaw_goal_tolerance: 1.35
+    stateful: True
+
+  progress_checker:
+    plugin: "nav2_controller::SimpleProgressChecker"
+    required_movement_radius: 0.1
+    movement_time_allowance: 10.0

planners/nav2py_pas_crowdnav_controller/CMakeLists.txt

@@ -0,0 +1,57 @@
+cmake_minimum_required(VERSION 3.8)
+project(nav2_pas_crowdnav_controller)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(nav2_common REQUIRED)
+find_package(nav2_core REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
+find_package(nav2_util REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
+include_directories(include)
+
+set(dependencies
+  rclcpp
+  geometry_msgs
+  nav2_costmap_2d
+  pluginlib
+  nav_msgs
+  nav2_util
+  nav2_core
+  tf2
+  tf2_geometry_msgs
+)
+
+add_library(${PROJECT_NAME} SHARED
+  src/custom_planner.cpp
+)
+
+ament_target_dependencies(${PROJECT_NAME} ${dependencies})
+
+# Export the plugin
+pluginlib_export_plugin_description_file(nav2_core nav2py_pas_crowdnav_controller.xml)
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+ament_export_dependencies(${dependencies})
+
+ament_package()

planners/nav2py_pas_crowdnav_controller/include/nav2_pas_crowdnav_controller/custom_planner.hpp

@@ -0,0 +1,63 @@
+#ifndef NAV2_PAS_CROWDNAV_CONTROLLER__CUSTOM_PLANNER_HPP_
+#define NAV2_PAS_CROWDNAV_CONTROLLER__CUSTOM_PLANNER_HPP_
+
+#include <string>
+#include <vector>
+#include <memory>
+
+#include "nav2_core/controller.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "pluginlib/class_loader.hpp"
+#include "pluginlib/class_list_macros.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+
+namespace nav2_pas_crowdnav_controller
+{
+
+class PasCrowdNavController : public nav2_core::Controller
+{
+public:
+  PasCrowdNavController() = default;
+  ~PasCrowdNavController() override = default;
+
+  void configure(
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr & 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;
+  void setSpeedLimit(const double & speed_limit, const bool & percentage) override;
+
+  geometry_msgs::msg::TwistStamped computeVelocityCommands(
+    const geometry_msgs::msg::PoseStamped & pose,
+    const geometry_msgs::msg::Twist & velocity,
+    nav2_core::GoalChecker * goal_checker) override;
+
+  void setPlan(const nav_msgs::msg::Path & path) override;
+
+protected:
+  rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+  std::string plugin_name_;
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
+  rclcpp::Logger logger_ {rclcpp::get_logger("PasCrowdNavController")};
+
+  // Controller Parameters
+  double lookahead_dist_;
+  double v_max_;
+  double v_min_;           // NEW: Minimum speed
+  double slowdown_dist_;   // NEW: Distance to start slowing down
+  double desired_linear_vel_;
+  double rotation_threshold_; 
+  rclcpp::Duration transform_tolerance_ {0, 0};
+
+  nav_msgs::msg::Path global_plan_;
+  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> local_plan_pub_;
+};
+
+}  // namespace nav2_pas_crowdnav_controller
+
+#endif

planners/nav2py_pas_crowdnav_controller/model/custom_planner/_stable_baselines3_version

@@ -0,0 +1 @@
+0.10.0

planners/nav2py_pas_crowdnav_controller/nav2py_pas_crowdnav_controller.xml

@@ -0,0 +1,11 @@
+<class_libraries>
+  <library path="nav2_pas_crowdnav_controller">
+    <class name="nav2_pas_crowdnav_controller/PasCrowdNavController" 
+           type="nav2_pas_crowdnav_controller::PasCrowdNavController" 
+           base_class_type="nav2_core::Controller">
+      <description>
+        Pure Pursuit Local Planner.
+      </description>
+    </class>
+  </library>
+</class_libraries>

planners/nav2py_pas_crowdnav_controller/nav2py_pas_crowdnav_controller/nav2py_pas_crowdnav_controller/__init__.py

@@ -0,0 +1,79 @@
+import typing
+from dataclasses import dataclass
+
+import numpy as np
+
+
+@dataclass(frozen=True)
+class Orientation:
+    x: float
+    y: float
+    z: float
+    w: float
+
+    @classmethod
+    def parse(cls, obj: typing.Any) -> "Orientation":
+        return cls(x=obj['x'], y=obj['y'], z=obj['z'], w=obj['w'])
+
+
+@dataclass(frozen=True)
+class Position:
+    x: float
+    y: float
+    z: float
+
+    @classmethod
+    def parse(cls, obj: typing.Any) -> "Position":
+        return cls(x=obj['x'], y=obj['y'], z=obj['z'])
+
+
+@dataclass(frozen=True)
+class Pose:
+    position: Position
+    orientation: Orientation
+
+    @classmethod
+    def parse(cls, obj: typing.Any) -> "Pose":
+        """
+        Parse PoseStamped message
+        """
+        return cls(
+            position=Position.parse(obj['pose']['position']),
+            orientation=Orientation.parse(obj['pose']['orientation']),
+        )
+
+
+@dataclass(frozen=True)
+class Path:
+    poses: typing.List[Pose]
+
+    @classmethod
+    def parse(cls, obj: typing.Any) -> "Path":
+        return cls(poses=[Pose.parse(pose) for pose in obj["poses"]])
+
+
+@dataclass(frozen=True)
+class LaserScan:
+    angle_min: float
+    angle_max: float
+    angle_increment: float
+    time_increment: float
+    scan_time: float
+    range_min: float
+    range_max: float
+    ranges: np.ndarray
+    intensities: typing.List[float]
+
+    @classmethod
+    def parse(cls, obj: typing.Any) -> "LaserScan":
+        return cls(
+            angle_min=obj['angle_min'],
+            angle_max=obj['angle_max'],
+            angle_increment=obj['angle_increment'],
+            time_increment=obj['time_increment'],
+            scan_time=obj['scan_time'],
+            range_min=obj['range_min'],
+            range_max=obj['range_max'],
+            ranges=np.clip(np.array(obj['ranges'], dtype=float), obj['range_min'], obj['range_max']),
+            intensities=obj['intensities'],
+        )

planners/nav2py_pas_crowdnav_controller/nav2py_pas_crowdnav_controller/nav2py_pas_crowdnav_controller/__main__.py

@@ -0,0 +1,79 @@
+import logging
+import os
+import typing
+
+import nav2py
+import nav2py.interfaces
+import yaml
+
+from . import LaserScan, Path, Pose
+from .controller import Controller
+
+
+class nav2py_pas_crowdnav_controller(nav2py.interfaces.nav2py_costmap_controller):
+
+    def __init__(self, *args, **kwargs):
+        super().__init__(*args, **kwargs)
+        self._register_callback('scan', self._scan_callback)
+        self._register_callback('odom', self._odom_callback)
+        self._register_callback('path', self._path_callback)
+        self._register_callback('speed_limit', self._speed_limit_callback)
+
+        self.logger = logging.getLogger('nav2py_pas_crowdnav_controller')
+        self.frame_count = 0
+        self.path = None
+
+        self.logger.info("nav2py_pas_crowdnav_controller initialized")
+
+        # Initializing the Pure Pursuit controller from controller.py
+        self.controller = Controller(self.logger)
+
+    def _scan_callback(self, scan_: typing.List[bytes]):
+        """
+        Process scan data from C++ controller
+        """
+        self.logger.info('Process scan data from C++ controller')
+        if scan_:
+            scan = yaml.safe_load(scan_[0].decode())
+            self.controller.scan_callback(LaserScan.parse(scan))
+
+    def _path_callback(self, path_: typing.List[bytes]):
+        """
+        Process path data from C++ controller
+        """
+        self.logger.info('Process path data from C++ controller')
+        if path_:
+            path = yaml.safe_load(path_[0].decode())
+            self.controller.setPath(Path.parse(path))
+
+    def _odom_callback(self, data: typing.List[bytes]):
+        """
+        Process data from C++ controller
+        """
+        self.logger.warning('Process odom data from C++ controller')
+
+        parsed_data = yaml.safe_load(data[0].decode())
+
+        pose = Pose.parse(parsed_data["pose"])
+        velocity = parsed_data["velocity"]
+
+        linear_x, angular_z = self.controller.computeVelocityCommands(pose)
+
+        self.logger.info(f"Sending control commands: linear_x={linear_x:.2f}, angular_z={angular_z:.2f}")
+
+        self._send_cmd_vel(linear_x, angular_z)
+
+    def _speed_limit_callback(self, speed_limit: typing.List[bytes]):
+        """
+        Process speed limit data from C++ controller
+        """
+        self.logger.info('Process speed limit data from C++ controller')
+
+        if len(speed_limit) == 2:
+            speed_limit = yaml.safe_load(speed_limit[0].decode())
+            is_percentage = yaml.safe_load(speed_limit[1].decode())
+            self.controller.setSpeedLimit(speed_limit, is_percentage)
+
+
+if __name__ == "__main__":
+    nav2py.main(nav2py_pas_crowdnav_controller)