From e35a16f1a1f4c3bdf911d23fadde73b2890f0143 Mon Sep 17 00:00:00 2001
From: Florian Vahl <7vahl@informatik.uni-hamburg.de>
Date: Thu, 28 Nov 2024 11:18:58 +0100
Subject: [PATCH] Add dummy path planner for out of map navigation (demo mode)

---
 .../bitbots_bringup/launch/demo.launch        |  8 ++++
 .../bitbots_path_planning/path_planning.py    |  4 +-
 .../bitbots_path_planning/planner.py          | 42 +++++++++++++++++--
 .../config/path_planning_parameters.yaml      |  6 +++
 .../launch/path_planning.launch               |  2 +
 5 files changed, 56 insertions(+), 6 deletions(-)

diff --git a/bitbots_misc/bitbots_bringup/launch/demo.launch b/bitbots_misc/bitbots_bringup/launch/demo.launch
index e3c1ef3cd..6076d5490 100644
--- a/bitbots_misc/bitbots_bringup/launch/demo.launch
+++ b/bitbots_misc/bitbots_bringup/launch/demo.launch
@@ -10,5 +10,13 @@
         <arg name="localization" value="false" />
         <arg name="sim" value="$(var sim)" />
         <arg name="teamcom" value="false" />
+        <arg name="path_planning" value="false" />
+    </include>
+
+    <!-- load the path planning node in dummy mode, because we are limited by the map size otherwise and together with no localization 
+    this could lead to the robot not working after a while, because due to odometry errors the robot could be outside of the map -->
+    <include file="$(find-pkg-share bitbots_path_planning)/launch/path_planning.launch">
+        <arg name="sim" value="$(var sim)" />
+        <arg name="dummy" value="true" />
     </include>
 </launch>
diff --git a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py
index e655ae375..3a812d5c1 100644
--- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py
+++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py
@@ -12,7 +12,7 @@
 from bitbots_path_planning.controller import Controller
 from bitbots_path_planning.map import Map
 from bitbots_path_planning.path_planning_parameters import bitbots_path_planning as parameters
-from bitbots_path_planning.planner import Planner
+from bitbots_path_planning.planner import planner_factory
 
 
 class PathPlanning(Node):
@@ -30,7 +30,7 @@ def __init__(self) -> None:
 
         # Create submodules
         self.map = Map(node=self, buffer=self.tf_buffer)
-        self.planner = Planner(node=self, buffer=self.tf_buffer, map=self.map)
+        self.planner = planner_factory(self)(node=self, buffer=self.tf_buffer, map=self.map)
         self.controller = Controller(node=self, buffer=self.tf_buffer)
 
         # Subscriber
diff --git a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py
index 9c3254d0e..08a5392cb 100644
--- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py
+++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py
@@ -1,7 +1,7 @@
 import numpy as np
 import pyastar2d
 import tf2_ros as tf2
-from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import PoseStamped, Vector3
 from nav_msgs.msg import Path
 from rclpy.duration import Duration
 from rclpy.node import Node
@@ -44,6 +44,14 @@ def active(self) -> bool:
         """
         return self.goal is not None
 
+    def get_my_position(self) -> Vector3:
+        """
+        Returns the current position of the robot
+        """
+        return self.buffer.lookup_transform(
+            self.map.frame, self.base_footprint_frame, Time(), Duration(seconds=0.2)
+        ).transform.translation
+
     def step(self) -> Path:
         """
         Generates a new A* path to the goal pose with respect to the costmap
@@ -54,9 +62,7 @@ def step(self) -> Path:
         navigation_grid = self.map.get_map()
 
         # Get my pose and position on the map
-        my_position = self.buffer.lookup_transform(
-            self.map.frame, self.base_footprint_frame, Time(), Duration(seconds=0.2)
-        ).transform.translation
+        my_position = self.get_my_position()
 
         # Transform goal pose to map frame if needed
         if goal.header.frame_id != self.map.frame:
@@ -94,3 +100,31 @@ def get_path(self) -> Path | None:
         Returns the most recent path
         """
         return self.path
+
+
+class DummyPlanner(Planner):
+    def __init__(self, node: Node, buffer: tf2.Buffer, map: Map) -> None:
+        super().__init__(node, buffer, map)
+
+    def step(self) -> Path:
+        return self.get_path()
+
+    def get_path(self) -> Path:
+        pose = PoseStamped()
+        my_position = self.get_my_position()
+        pose.pose.position.x = my_position.x
+        pose.pose.position.y = my_position.y
+
+        self.path = Path(
+            header=Header(frame_id=self.map.get_frame(), stamp=self.node.get_clock().now().to_msg()),
+            poses=[pose, self.goal],
+        )
+
+        return self.path
+
+
+def planner_factory(node: Node) -> type:
+    if node.config.planner.dummy:
+        return DummyPlanner
+    else:
+        return Planner
diff --git a/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml b/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml
index 1f7c94211..498c04350 100644
--- a/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml
+++ b/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml
@@ -17,6 +17,12 @@ bitbots_path_planning:
     validation:
       bounds<>: [0.0, 100.0]
 
+  planner:
+    dummy:
+      type: bool
+      default_value: false
+      description: 'If true, the dummy planner is used, which just returns a straight line to the goal. This ignores all obstacles, but also has no limitations on the map size.'
+
   map:
     planning_frame:
       type: string
diff --git a/bitbots_navigation/bitbots_path_planning/launch/path_planning.launch b/bitbots_navigation/bitbots_path_planning/launch/path_planning.launch
index 0774f196a..d87afb55c 100755
--- a/bitbots_navigation/bitbots_path_planning/launch/path_planning.launch
+++ b/bitbots_navigation/bitbots_path_planning/launch/path_planning.launch
@@ -1,7 +1,9 @@
 <launch>
     <arg name="sim" default="false" description="true: activates simulation time" />
+    <arg name="dummy" default="false" description="true: activates dummy mode, that just walks a straight line" />
 
     <node pkg="bitbots_path_planning" exec="path_planning" name="bitbots_path_planning" output="screen">
         <param name="use_sim_time" value="$(var sim)"/>
+        <param name="planner.dummy" value="$(var dummy)"/>
     </node>
 </launch>