Add dummy path planner for out of map navigation (demo mode)

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>

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

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

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

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>