Add rust visibility graph based path planning

.gitignore

@@ -227,3 +227,5 @@ doku/*
 
 # Workspace git status file from the deploy tool
 **/workspace_status.json
+
+.pytest_cache/

Makefile

@@ -17,7 +17,7 @@ install-no-root: pull-init update-no-root
 
 pip:
 	# Install and upgrade pip dependencies
-	pip install --upgrade -r requirements/dev.txt --user
+	pip install --upgrade -r requirements/dev.txt --user -v
 
 pre-commit:
 	# Install pre-commit hooks for all submodules that have a .pre-commit-config.yaml file

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py

@@ -166,8 +166,17 @@ def get_ball_goal(self, target: BallGoalType, distance: float, side_offset: floa
             elif ball_x > self._blackboard.world_model.field_length / 2 - 0.2:
                 goal_angle = math.pi + np.copysign(math.pi / 4, ball_y)
 
-            goal_x = ball_x - math.cos(goal_angle) * distance + math.sin(goal_angle) * side_offset
-            goal_y = ball_y - math.sin(goal_angle) * distance + math.cos(goal_angle) * side_offset
+            # We don't want to walk into the ball, so we add an offset to stop before the ball
+            approach_offset_x = math.cos(goal_angle) * distance
+            approach_offset_y = math.sin(goal_angle) * distance
+
+            # We also want to kick the ball with one foot instead of the center between the feet
+            side_offset_x = math.cos(goal_angle - math.pi / 2) * side_offset
+            side_offset_y = math.sin(goal_angle - math.pi / 2) * side_offset
+
+            # Calculate the goal position (has nothing to do with the soccer goal)
+            goal_x = ball_x - approach_offset_x + side_offset_x
+            goal_y = ball_y - approach_offset_y + side_offset_y
 
             ball_point = (goal_x, goal_y, goal_angle, self._blackboard.map_frame)
 

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py

@@ -2,7 +2,6 @@
 from bitbots_blackboard.capsules.pathfinding_capsule import BallGoalType
 from dynamic_stack_decider.abstract_action_element import AbstractActionElement
 from geometry_msgs.msg import Vector3
-from rclpy.duration import Duration
 from std_msgs.msg import ColorRGBA
 from visualization_msgs.msg import Marker
 
@@ -31,6 +30,7 @@ def perform(self, reevaluate=False):
 
         approach_marker = Marker()
         approach_marker.pose.position.x = self.distance
+        approach_marker.pose.position.y = self.side_offset
         approach_marker.type = Marker.SPHERE
         approach_marker.action = Marker.MODIFY
         approach_marker.id = 1
@@ -40,7 +40,6 @@ def perform(self, reevaluate=False):
         color.b = 1.0
         color.a = 1.0
         approach_marker.color = color
-        approach_marker.lifetime = Duration(seconds=0.5).to_msg()
         scale = Vector3(x=0.2, y=0.2, z=0.2)
         approach_marker.scale = scale
         approach_marker.header.stamp = self.blackboard.node.get_clock().now().to_msg()

bitbots_misc/bitbots_bringup/launch/demo.launch

@@ -10,13 +10,6 @@
         <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" />
+        <arg name="path_planning" value="true" />
     </include>
 </launch>

bitbots_misc/bitbots_ipm/config/soccer_ipm.yaml

@@ -20,8 +20,8 @@ soccer_ipm:
     robots:
       footpoint_out_of_image_threshold: 0.8
       object_default_dimensions:
-        x: 0.2
-        y: 0.2
+        x: 0.5
+        y: 0.5
         z: 1.0
 
     output_frame: 'base_footprint'

bitbots_misc/bitbots_ipm/launch/ipm.launch

@@ -2,7 +2,7 @@
 <launch>
     <arg name="sim" default="false"/>
     <arg name="full_image" default="false" description="Whether to project the full-size RGB image for debugging or showcasing"/>
-    <arg name="markers" default="false" description="Whether to publish markers for visualization of the detected objects in RViz"/>
+    <arg name="markers" default="true" description="Whether to publish markers for visualization of the detected objects in RViz"/>
     <arg name="rviz" default="false" description="Whether to start RViz with the ipm configuration"/>
 
     <node pkg="ipm_image_node" exec="ipm" name="ipm_line_mask" output="screen">

bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py

@@ -2,16 +2,14 @@
 import soccer_vision_3d_msgs.msg as sv3dm
 from bitbots_tf_buffer import Buffer
 from geometry_msgs.msg import PointStamped, PoseStamped, PoseWithCovarianceStamped, Twist
-from nav_msgs.msg import OccupancyGrid, Path
-from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
+from nav_msgs.msg import Path
 from rclpy.duration import Duration
-from rclpy.executors import MultiThreadedExecutor
 from std_msgs.msg import Bool, Empty
+from visualization_msgs.msg import MarkerArray
 
 from bitbots_path_planning import NodeWithConfig
 from bitbots_path_planning.controller import Controller
-from bitbots_path_planning.map import Map
-from bitbots_path_planning.planner import planner_factory
+from bitbots_path_planning.planner import VisibilityPlanner
 
 
 class PathPlanning(NodeWithConfig):
@@ -25,57 +23,31 @@ def __init__(self) -> None:
         # We need to create a tf buffer
         self.tf_buffer = Buffer(self, Duration(seconds=self.config.tf_buffer_duration))
 
-        # Create submodules
-        self.map = Map(node=self, buffer=self.tf_buffer)
-        self.planner = planner_factory(self)(node=self, buffer=self.tf_buffer, map=self.map)
+        self.planner = VisibilityPlanner(node=self, buffer=self.tf_buffer)
         self.controller = Controller(node=self, buffer=self.tf_buffer)
 
+        self.measurements = dict()
+
         # Subscriber
-        self.create_subscription(
-            PoseWithCovarianceStamped,
-            self.config.map.ball_update_topic,
-            self.map.set_ball,
-            5,
-            callback_group=MutuallyExclusiveCallbackGroup(),
-        )
-        self.create_subscription(
-            sv3dm.RobotArray,
-            self.config.map.robot_update_topic,
-            self.map.set_robots,
-            5,
-            callback_group=MutuallyExclusiveCallbackGroup(),
-        )
-        self.goal_sub = self.create_subscription(
-            PoseStamped, "goal_pose", self.planner.set_goal, 5, callback_group=MutuallyExclusiveCallbackGroup()
-        )
-        self.create_subscription(
-            Empty,
-            "pathfinding/cancel",
-            lambda _: self.planner.cancel(),
-            5,
-            callback_group=MutuallyExclusiveCallbackGroup(),
-        )
+        self.create_subscription(PoseWithCovarianceStamped, self.config.map.ball_update_topic, self.planner.set_ball, 5)
+        self.create_subscription(sv3dm.RobotArray, self.config.map.robot_update_topic, self.planner.set_robots, 5)
+        self.goal_sub = self.create_subscription(PoseStamped, "goal_pose", self.planner.set_goal, 5)
+        self.create_subscription(Empty, "pathfinding/cancel", lambda _: self.planner.cancel_goal(), 5)
         self.create_subscription(
             Bool,
             "ball_obstacle_active",
-            lambda msg: self.map.avoid_ball(msg.data),  # type: ignore
+            lambda msg: self.planner.avoid_ball(msg.data),  # type: ignore
             5,
-            callback_group=MutuallyExclusiveCallbackGroup(),
         )
 
         # Publisher
         self.cmd_vel_pub = self.create_publisher(Twist, "cmd_vel", 1)
-        self.costmap_pub = self.create_publisher(OccupancyGrid, "costmap", 1)
         self.path_pub = self.create_publisher(Path, "path", 1)
         self.carrot_pub = self.create_publisher(PointStamped, "carrot", 1)
+        self.graph_pub = self.create_publisher(MarkerArray, "visibility_graph", 1)
 
         # Timer that updates the path and command velocity at a given rate
-        self.create_timer(
-            1 / self.config.rate,
-            self.step,
-            clock=self.get_clock(),
-            callback_group=MutuallyExclusiveCallbackGroup(),
-        )
+        self.create_timer(1 / self.config.rate, self.step, clock=self.get_clock())
 
     def step(self) -> None:
         """
@@ -85,11 +57,6 @@ def step(self) -> None:
             self.param_listener.refresh_dynamic_parameters()
             self.config = self.param_listener.get_params()
         try:
-            # Update the map with the latest ball and robot positions
-            self.map.update()
-            # Publish the costmap for visualization
-            self.costmap_pub.publish(self.map.to_msg())
-
             if self.planner.active():
                 # Calculate the path to the goal pose considering the current map
                 path = self.planner.step()
@@ -109,10 +76,7 @@ def main(args=None):
     rclpy.init(args=args)
     node = PathPlanning()
 
-    # choose number of threads by number of callback_groups + 1 for simulation time
-    ex = MultiThreadedExecutor(num_threads=8)
-    ex.add_node(node)
-    ex.spin()
+    rclpy.spin(node)
 
     node.destroy_node()
     rclpy.shutdown()