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/

.vscode/settings.json

@@ -222,11 +222,9 @@
     "ros.distro": "iron",
     "search.useIgnoreFiles": false,
     "python.autoComplete.extraPaths": [
-        "/opt/openrobots/lib/python3.10/site-packages",
         "/opt/ros/iron/lib/python3.10/site-packages"
     ],
     "python.analysis.extraPaths": [
-        "/opt/openrobots/lib/python3.10/site-packages",
         "/opt/ros/iron/lib/python3.10/site-packages"
     ],
     "cmake.configureOnOpen": false,

Makefile

@@ -17,7 +17,8 @@ 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 pip --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.py

@@ -1,4 +1,5 @@
 import math
+from typing import Optional
 
 import numpy as np
 import tf2_ros as tf2
@@ -17,7 +18,7 @@ def __init__(self, blackboard, dsd, parameters):
         self.point = float(parameters.get("x", 0)), float(parameters.get("y", 0)), float(parameters.get("t", 0))
         self.threshold = float(parameters.get("threshold", 0.1))
         self.first = True
-        self.odom_goal_pose: PoseStamped = None
+        self.goal_pose: Optional[PoseStamped] = None
 
     def perform(self, reevaluate=False):
         if self.first:
@@ -32,18 +33,18 @@ def perform(self, reevaluate=False):
             goal_pose.pose.orientation = quat_from_yaw(math.radians(self.point[2]))
 
             try:
-                self.odom_goal_pose = self.blackboard.tf_buffer.transform(
-                    goal_pose, self.blackboard.odom_frame, timeout=Duration(seconds=0.5)
+                self.goal_pose = self.blackboard.tf_buffer.transform(
+                    goal_pose, self.blackboard.map_frame, timeout=Duration(seconds=0.5)
                 )
-                self.blackboard.pathfinding.publish(self.odom_goal_pose)
+                self.blackboard.pathfinding.publish(self.goal_pose)
             except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e:
                 self.blackboard.node.get_logger().warning("Could not transform goal pose: " + str(e))
                 self.first = False
         else:
-            current_position = self.blackboard.world_model.get_current_position(self.blackboard.odom_frame)
-            if self.odom_goal_pose is not None and current_position is not None:
+            current_position = self.blackboard.world_model.get_current_position()
+            if self.goal_pose is not None and current_position is not None:
                 position = np.array(current_position[:2])
-                goal = np.array([self.odom_goal_pose.pose.position.x, self.odom_goal_pose.pose.position.y])
+                goal = np.array([self.goal_pose.pose.position.x, self.goal_pose.pose.position.y])
                 distance = np.linalg.norm(goal - position)
                 if distance < self.threshold:
                     self.pop()

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
 
@@ -23,14 +22,15 @@ def __init__(self, blackboard, dsd, parameters):
         self.blocking = parameters.get("blocking", True)
         self.distance = parameters.get("distance", self.blackboard.config["ball_approach_dist"])
         # Offset so we kick the ball with one foot instead of the center between the feet
-        self.side_offset = parameters.get("side_offset", 0.08)
+        self.side_offset = parameters.get("side_offset", 0.05)
 
     def perform(self, reevaluate=False):
         pose_msg = self.blackboard.pathfinding.get_ball_goal(self.target, self.distance, self.side_offset)
         self.blackboard.pathfinding.publish(pose_msg)
 
         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_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/turn.py

@@ -64,7 +64,7 @@ def __init__(self, blackboard, dsd, parameters):
 
     def perform(self, reevaluate=False):
         # Increase the rotation speed if we are not at max speed
-        if abs(self.current_rotation_vel) < self.max_speed:
+        if abs(self.current_rotation_vel) < abs(self.max_speed):
             self.current_rotation_vel += math.copysign(0.05, self.max_speed)
 
         # Create the cmd_vel message
@@ -81,3 +81,13 @@ def perform(self, reevaluate=False):
             # Check if the duration is over
             if (self.blackboard.node.get_clock().now() - self.start_time).nanoseconds / 1e9 > self.duration:
                 self.pop()
+
+
+class TurnLastSeenBallSide(Turn):
+    """
+    Turns to the side where the ball was last seen for a given duration.
+    """
+
+    def __init__(self, blackboard, dsd, parameters):
+        super().__init__(blackboard, dsd, parameters)
+        self.max_speed = math.copysign(abs(self.max_speed), self.blackboard.world_model.get_ball_position_uv()[1])

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd

@@ -1,6 +1,6 @@
 #SearchBall
 $DoOnce
-    NOT_DONE --> @ChangeAction + action:searching, @LookAtFieldFeatures, @WalkInPlace + duration:3, @Turn + duration:15, @GoToAbsolutePositionFieldFraction + x:0.5 + blocking:false
+    NOT_DONE --> @ChangeAction + action:searching, @LookAtFieldFeatures, @WalkInPlace + duration:3, @TurnLastSeenBallSide + duration:15, @GoToAbsolutePositionFieldFraction + x:0.5 + blocking:false
     DONE --> $ReachedAndAlignedToPathPlanningGoalPosition + threshold:0.5 + latch:true
         NO --> @LookAtFieldFeatures, @GoToAbsolutePositionFieldFraction + x:0.5
         YES --> $DoOnce

bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml

@@ -61,10 +61,10 @@ body_behavior:
     # An area in which the ball can be kicked
     # defined by min/max x/y values in meters which represent ball positions relative to base_footprint
     # http://www.ros.org/reps/rep-0103.html#axis-orientation
-    kick_x_enter: 0.2
+    kick_x_enter: 0.25
     kick_x_leave: 0.25
     kick_y_enter: 0.12
-    kick_y_leave: 0.14
+    kick_y_leave: 0.12
 
     # defines the radius around the goal (in form of a box)
     # in this area, the goalie will react to the ball.

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/controller.py

@@ -66,7 +66,7 @@ def step(self, path: Path) -> tuple[Twist, PointStamped]:
 
         # Calculate the heading angle from our current position to the final position of the global plan
         final_walk_angle = math.atan2(
-            end_pose.position.y - current_pose.position.y, end_pose.position.x - current_pose.position.x
+            goal_pose.position.y - current_pose.position.y, goal_pose.position.x - current_pose.position.x
         )
 
         if len(path.poses) < 3: