From 38480e96502264dd30b66e93cf1d8277976e27b2 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Fri, 21 Jun 2024 15:57:30 +0000 Subject: [PATCH 1/6] Add distance to ball close target --- .../bitbots_blackboard/capsules/pathfinding_capsule.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py index 8f39c0081..87b25ab1d 100644 --- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py +++ b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py @@ -176,7 +176,9 @@ def get_ball_goal(self, target: BallGoalType, distance: float) -> PoseStamped: 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 + 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.") From b8283f0f3aae7d2b532c10981e9a870811d7cac8 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Fri, 21 Jun 2024 15:57:45 +0000 Subject: [PATCH 2/6] Add demo behavior --- .../behavior_dsd/demo.dsd | 45 +++++++++++++++++++ 1 file changed, 45 insertions(+) create mode 100644 bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd new file mode 100644 index 000000000..e35d7df90 --- /dev/null +++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd @@ -0,0 +1,45 @@ +#SearchBall +@ChangeAction + action:searching, @LookAtFieldFeatures, @WalkInPlace + duration:3, @Turn + +#DoNothing +@ChangeAction + action:waiting, @LookForward, @Stand + +#StandAndLook +@ChangeAction + action:waiting, @LookAtFieldFeatures, @Stand + +#PerformKickLeft +@ChangeAction + action:kicking, @LookAtFront, @Stand + duration:1.0 + r:false, @LookForward + r:false, @KickBallStatic + foot:left + r:false, @LookAtFieldFeatures + r:false, @Stand + duration:2 + r:false + +#PerformKickRight +@ChangeAction + action:kicking, @LookAtFront, @Stand + duration:1.0 + r:false, @LookForward + r:false, @KickBallStatic + foot:right + r:false, @LookAtFieldFeatures + r:false, @Stand + duration:2 + r:false + +#KickWithAvoidance +$AvoidBall + NO --> $BallClose + distance:%body.ball_reapproach_dist + angle:%body.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:%body.ball_far_approach_dist + YES --> $ReachedPathPlanningGoalPosition + threshold:%body.ball_far_approach_position_thresh + YES --> @AvoidBallInactive + NO --> @ChangeAction + action:going_to_ball, @LookAtFieldFeatures, @GoToBall + target:close + distance:%body.ball_far_approach_dist + +#Init +@Stand + duration:0.1 + r:false, @ChangeAction + action:waiting, @LookForward, @Stand + +#NormalBehavior +$BallSeen + NO --> #SearchBall + YES --> #StrikerRole + +-->BodyBehavior +$IsPenalized + YES --> #DoNothing + ELSE --> #NormalBehavior + INITIAL --> #Init + READY --> #StandAndLook + SET --> #StandAndLook + FINISHED --> @Stand + duration:0.5 + r:false, @PlaySound + file:fanfare.wav, @PlayAnimationCheering + r:false, @LookForward, @Stand + PLAYING --> #NormalBehavior From 1d50cd50f9507cdb3ccab21d25f3966e14db7a82 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Wed, 27 Nov 2024 20:10:10 +0100 Subject: [PATCH 3/6] Add sim demo behavior --- .../capsules/pathfinding_capsule.py | 8 +++++--- .../behavior_dsd/actions/go_to_ball.py | 4 +++- .../behavior_dsd/demo.dsd | 19 +++++++------------ .../bitbots_bringup/launch/demo.launch | 14 ++++++++++++++ 4 files changed, 29 insertions(+), 16 deletions(-) create mode 100644 bitbots_misc/bitbots_bringup/launch/demo.launch diff --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py index 9c8490430..a621bf0a3 100644 --- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py +++ b/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 @@ -176,17 +175,20 @@ def get_ball_goal(self, target: BallGoalType, distance: float, side_offset: floa ball_u, ball_v = self._blackboard.world_model.get_ball_position_uv() angle = math.atan2(ball_v, ball_u) goal_u = ball_u - math.cos(angle) * distance - goal_v = ball_v - math.sin(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 diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py index 86a04b5e1..ebb24ec83 100644 --- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_ball.py +++ b/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() diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd index e35d7df90..e8b4b4e67 100644 --- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd +++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd @@ -8,23 +8,23 @@ @ChangeAction + action:waiting, @LookAtFieldFeatures, @Stand #PerformKickLeft -@ChangeAction + action:kicking, @LookAtFront, @Stand + duration:1.0 + r:false, @LookForward + r:false, @KickBallStatic + foot:left + r:false, @LookAtFieldFeatures + r:false, @Stand + duration:2 + r:false +@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, @Stand + duration:1.0 + r:false, @LookForward + r:false, @KickBallStatic + foot:right + r:false, @LookAtFieldFeatures + r:false, @Stand + duration:2 + r:false +@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:%body.ball_reapproach_dist + angle:%body.ball_reapproach_angle + 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:%body.ball_far_approach_dist - YES --> $ReachedPathPlanningGoalPosition + threshold:%body.ball_far_approach_position_thresh + 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:%body.ball_far_approach_dist + NO --> @ChangeAction + action:going_to_ball, @LookAtFieldFeatures, @GoToBall + target:close + distance:%ball_far_approach_dist #Init @Stand + duration:0.1 + r:false, @ChangeAction + action:waiting, @LookForward, @Stand @@ -32,14 +32,9 @@ $AvoidBall #NormalBehavior $BallSeen NO --> #SearchBall - YES --> #StrikerRole + YES --> #KickWithAvoidance -->BodyBehavior $IsPenalized YES --> #DoNothing ELSE --> #NormalBehavior - INITIAL --> #Init - READY --> #StandAndLook - SET --> #StandAndLook - FINISHED --> @Stand + duration:0.5 + r:false, @PlaySound + file:fanfare.wav, @PlayAnimationCheering + r:false, @LookForward, @Stand - PLAYING --> #NormalBehavior diff --git a/bitbots_misc/bitbots_bringup/launch/demo.launch b/bitbots_misc/bitbots_bringup/launch/demo.launch new file mode 100644 index 000000000..e3c1ef3cd --- /dev/null +++ b/bitbots_misc/bitbots_bringup/launch/demo.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + From 775f90983e57eddb8ae366ec4522552197089120 Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Thu, 28 Nov 2024 11:18:20 +0100 Subject: [PATCH 4/6] Cleanup demo dsd file --- .../behavior_dsd/demo.dsd | 22 +++++-------------- 1 file changed, 5 insertions(+), 17 deletions(-) diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd index e8b4b4e67..4383b6b43 100644 --- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd +++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd @@ -1,12 +1,6 @@ #SearchBall @ChangeAction + action:searching, @LookAtFieldFeatures, @WalkInPlace + duration:3, @Turn -#DoNothing -@ChangeAction + action:waiting, @LookForward, @Stand - -#StandAndLook -@ChangeAction + action:waiting, @LookAtFieldFeatures, @Stand - #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 @@ -26,15 +20,9 @@ $AvoidBall YES --> @AvoidBallInactive NO --> @ChangeAction + action:going_to_ball, @LookAtFieldFeatures, @GoToBall + target:close + distance:%ball_far_approach_dist -#Init -@Stand + duration:0.1 + r:false, @ChangeAction + action:waiting, @LookForward, @Stand - -#NormalBehavior -$BallSeen - NO --> #SearchBall - YES --> #KickWithAvoidance - -->BodyBehavior -$IsPenalized - YES --> #DoNothing - ELSE --> #NormalBehavior +$DoOnce + NOT_DONE --> @Stand + duration:5 + DONE --> $BallSeen + YES --> #KickWithAvoidance + NO --> #SearchBall 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 5/6] 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 @@ + + + + + + + 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 @@ + + From cb9b4cd0347aaca4c069d4725156f97c3d6c5f7e Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Thu, 28 Nov 2024 12:19:14 +0100 Subject: [PATCH 6/6] Increase initial wait --- .../bitbots_body_behavior/behavior_dsd/demo.dsd | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd index 4383b6b43..f9b0a3048 100644 --- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd +++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/demo.dsd @@ -22,7 +22,7 @@ $AvoidBall -->BodyBehavior $DoOnce - NOT_DONE --> @Stand + duration:5 + NOT_DONE --> @Stand + duration:15 DONE --> $BallSeen YES --> #KickWithAvoidance NO --> #SearchBall