Demo behavior (#629)

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py

@@ -26,7 +26,6 @@ class PathfindingCapsule(AbstractBlackboardCapsule):
 
     def __init__(self, node, blackboard):
         super().__init__(node, blackboard)
-        self.map_frame: str = self._node.get_parameter("map_frame").value
         self.position_threshold: str = self._node.get_parameter("pathfinding_position_threshold").value
         self.orientation_threshold: str = self._node.get_parameter("pathfinding_orientation_threshold").value
 
@@ -175,16 +174,21 @@ def get_ball_goal(self, target: BallGoalType, distance: float, side_offset: floa
         elif BallGoalType.CLOSE == target:
             ball_u, ball_v = self._blackboard.world_model.get_ball_position_uv()
             angle = math.atan2(ball_v, ball_u)
-            ball_point = (ball_u, ball_v, angle, self._blackboard.world_model.base_footprint_frame)
+            goal_u = ball_u - math.cos(angle) * distance
+            goal_v = ball_v - math.sin(angle) * distance + side_offset
+            ball_point = (goal_u, goal_v, angle, self._blackboard.world_model.base_footprint_frame)
 
         else:
             self._node.get_logger().error(f"Target {target} for go_to_ball action not implemented.")
             return
 
+        # Create the goal pose message
         pose_msg = PoseStamped()
-        pose_msg.header.stamp = self._node.get_clock().now().to_msg()
         pose_msg.header.frame_id = ball_point[3]
         pose_msg.pose.position = Point(x=ball_point[0], y=ball_point[1], z=0.0)
         pose_msg.pose.orientation = quat_from_yaw(ball_point[2])
 
+        # Convert the goal to the map frame
+        pose_msg = self._blackboard.tf_buffer.transform(pose_msg, self._blackboard.map_frame)
+
         return pose_msg

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

@@ -22,9 +22,11 @@ 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)
 
     def perform(self, reevaluate=False):
-        pose_msg = self.blackboard.pathfinding.get_ball_goal(self.target, self.distance, 0.08)
+        pose_msg = self.blackboard.pathfinding.get_ball_goal(self.target, self.distance, self.side_offset)
         self.blackboard.pathfinding.publish(pose_msg)
 
         approach_marker = Marker()

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd

@@ -0,0 +1,28 @@
+#SearchBall
+@ChangeAction + action:searching, @LookAtFieldFeatures, @WalkInPlace + duration:3, @Turn
+
+#PerformKickLeft
+@ChangeAction + action:kicking, @LookAtFront, @WalkKick + foot:left + r:false, @WalkInPlace + duration:1 + r:false, @ForgetBall + r:false, @WalkInPlace + duration:1 + r:false, @LookAtFieldFeatures + r:false
+
+#PerformKickRight
+@ChangeAction + action:kicking, @LookAtFront, @WalkKick + foot:right + r:false, @WalkInPlace + duration:1 + r:false, @ForgetBall + r:false, @WalkInPlace + duration:1 + r:false, @LookAtFieldFeatures + r:false
+
+#KickWithAvoidance
+$AvoidBall
+    NO --> $BallClose + distance:%ball_reapproach_dist + angle:%ball_reapproach_angle
+        YES --> $BallKickArea
+            NEAR --> $FootSelection
+                LEFT --> #PerformKickLeft
+                RIGHT --> #PerformKickRight
+            FAR --> @ChangeAction + action:going_to_ball, @LookAtFront, @GoToBall + target:close
+        NO --> @ChangeAction + action:going_to_ball + r:false, @LookAtFieldFeatures + r:false, @AvoidBallActive + r:false, @GoToBall + target:close + blocking:false + distance:%ball_far_approach_dist
+    YES --> $ReachedPathPlanningGoalPosition + threshold:%ball_far_approach_position_thresh
+        YES --> @AvoidBallInactive
+        NO --> @ChangeAction + action:going_to_ball, @LookAtFieldFeatures, @GoToBall + target:close + distance:%ball_far_approach_dist
+
+-->BodyBehavior
+$DoOnce
+    NOT_DONE --> @Stand + duration:15
+    DONE --> $BallSeen
+        YES --> #KickWithAvoidance
+        NO --> #SearchBall

bitbots_misc/bitbots_bringup/launch/demo.launch

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="sim" default="false" description="Whether the robot is running in simulation or on real hardware" />
+    <arg name="behavior_dsd_file" default="demo.dsd" description="The behavior dsd file that should be used" />
+
+    <!-- load teamplayer software stack without some unnecessary stuff, that is not needed in the demo -->
+    <include file="$(find-pkg-share bitbots_bringup)/launch/teamplayer.launch">
+        <arg name="behavior_dsd_file" value="$(var behavior_dsd_file)" />
+        <arg name="game_controller" value="false"/>
+        <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>