Change sim fov

.gitignore

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

.vscode/settings.json

@@ -223,10 +223,12 @@
     "ros.distro": "jazzy",
     "search.useIgnoreFiles": false,
     "python.autoComplete.extraPaths": [
+
         "/opt/ros/jazzy/lib/python3.10/site-packages"
     ],
     "python.analysis.extraPaths": [
         "/opt/ros/jazzy/lib/python3.10/site-packages"
+
     ],
     "cmake.configureOnOpen": false,
     "editor.formatOnSave": true,

Makefile

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

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/animation_capsule.py

@@ -14,6 +14,8 @@ def __init__(self, node, blackboard):
         self.active = False
 
         # Config
+        self.grab_ball_animation: str = self._node.get_parameter("Animations.Throw_in.grabBall").value
+        self.throw_animation: str = self._node.get_parameter("Animations.Throw_in.throw").value
         self.goalie_arms_animation: str = self._node.get_parameter("Animations.Goalie.goalieArms").value
         self.goalie_falling_right_animation: str = self._node.get_parameter("Animations.Goalie.fallRight").value
         self.goalie_falling_left_animation: str = self._node.get_parameter("Animations.Goalie.fallLeft").value

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/get_walkready.py → bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/dynup.py

@@ -5,11 +5,23 @@
 from bitbots_msgs.action import Dynup
 
 
-class GetWalkready(AbstractActionElement):
+class PlayAnimationDynup(AbstractActionElement):
     blackboard: BodyBlackboard
 
     def __init__(self, blackboard, dsd, parameters):
         super().__init__(blackboard, dsd, parameters)
+        self.direction = parameters.get("direction")
+        assert self.direction in [
+            Dynup.Goal.DIRECTION_FRONT,
+            Dynup.Goal.DIRECTION_FRONT_ONLY,
+            Dynup.Goal.DIRECTION_BACK,
+            Dynup.Goal.DIRECTION_BACK_ONLY,
+            Dynup.Goal.DIRECTION_RISE,
+            Dynup.Goal.DIRECTION_DESCEND,
+            Dynup.Goal.DIRECTION_WALKREADY,
+            Dynup.Goal.DIRECTION_RISE_NO_ARMS,
+            Dynup.Goal.DIRECTION_DESCEND_NO_ARMS,
+        ], f"Direction '{self.direction}' not supported"
         self.first_perform = True
         self.active = False
 
@@ -58,7 +70,7 @@ def start_animation(self) -> bool:
 
         # Dynup action server is running, we can start the walkready action
         goal = Dynup.Goal()
-        goal.direction = Dynup.Goal.DIRECTION_WALKREADY
+        goal.direction = self.direction
         self.active = True
         self.dynup_action_current_goal = self.blackboard.animation.dynup_action_client.send_goal_async(goal)
         self.dynup_action_current_goal.add_done_callback(
@@ -78,3 +90,11 @@ def animation_finished(self) -> bool:
         :return: True if the animation is finished, False if not
         """
         return not self.active
+
+
+class GetWalkready(PlayAnimationDynup):
+    blackboard: BodyBlackboard
+
+    def __init__(self, blackboard, dsd, parameters):
+        parameters["direction"] = Dynup.Goal.DIRECTION_WALKREADY
+        super().__init__(blackboard, dsd, parameters)

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
 
@@ -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_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/play_animation.py

@@ -75,6 +75,18 @@ def get_animation_name(self):
         return self.blackboard.animation.cheering_animation
 
 
+class PlayAnimationGrabBall(AbstractPlayAnimation):
+    def get_animation_name(self):
+        self.blackboard.node.get_logger().info("PLAYING GRAB-BALL ANIMATION")
+        return self.blackboard.animation.grab_ball_animation
+
+
+class PlayAnimationThrow(AbstractPlayAnimation):
+    def get_animation_name(self):
+        self.blackboard.node.get_logger().info("PLAYING THROW-IN ANIMATION")
+        return self.blackboard.animation.throw_animation
+
+
 class PlayAnimationInit(AbstractPlayAnimation):
     def get_animation_name(self):
         return self.blackboard.animation.init_animation

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
@@ -117,6 +117,19 @@ $DoOnce
                         SECOND --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToCornerKickPosition + mode:supporter
                         THIRD --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToDefensePosition
                     OTHER --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToCornerKickPosition + mode:others
+                THROW_IN --> $SecondaryStateTeamDecider
+                    OUR --> $RankToBallNoGoalie
+                        FIRST --> $DoOnce
+                            NOT_DONE --> @ChangeAction + action:going_to_ball, @LookAtFieldFeatures, @AvoidBallActive, @GoToBall + target:gradient + distance:0.3 + blocking:false
+                            DONE --> $ReachedAndAlignedToPathPlanningGoalPosition + threshold:0.03 + latch:true
+                                NO --> @GoToBall + target:gradient + distance:0.3
+                                YES --> @LookForward, @ChangeAction + action:kicking, @Stand + duration:1.5 + r:false, @PlayAnimationDynup + direction:descend_no_arms, @PlayAnimationGrabBall + r:false, @PlayAnimationDynup + direction:rise_no_arms, @PlayAnimationThrow + r:false, @GetWalkready + r:false, @Stand
+                        SECOND --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToPassPreparePosition
+                        THIRD --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToDefensePosition
+                    OTHER --> $RankToBallNoGoalie
+                        FIRST --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToDefensePosition + mode:freekick_first
+                        SECOND --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToDefensePosition + mode:freekick_second
+                        THIRD --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToDefensePosition
                 ELSE --> $SecondaryStateTeamDecider
                     OUR --> $RankToBallNoGoalie
                         FIRST --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @AvoidBallActive, @GoToBall + target:gradient + distance:0.5

bitbots_behavior/bitbots_body_behavior/config/animations.yaml

@@ -6,6 +6,9 @@
         fallLeft: "goalie_falling_left"
         fallRight: "goalie_falling_right"
         fallCenter: "goalie_falling_center"
+      Throw_in:
+        grabBall: "grab_ball"
+        throw: "throw"
       Misc:
         cheering: "cheering"
         init: "init_sim"

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.23
     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.
@@ -146,7 +146,7 @@ body_behavior:
     dribble_ball_distance_threshold: 0.5
     dribble_kick_angle: 0.6
 
-    kick_decision_smoothing: 1.0
+    kick_decision_smoothing: 10.0
 
     ##################
     # costmap params #

bitbots_lowlevel/bitbots_ros_control/launch/ros_control.launch

@@ -4,6 +4,7 @@
     <arg name="torqueless_mode" default="false"/>
     <arg name="only_imu" default="false"/>
     <arg name="only_pressure" default="false"/>
+    <arg name="tts" default="true" description="Whether to speak" />
 
     <let if="$(env IS_ROBOT false)" name="taskset" value="taskset -c 0"/>
     <let unless="$(env IS_ROBOT false)" name="taskset" value=""/>
@@ -25,9 +26,11 @@
     <group>
         <include file="$(find-pkg-share system_monitor)/launch/system_monitor.launch"/>
         <include file="$(find-pkg-share bitbots_buttons)/launch/buttons.launch"/>
-        <include file="$(find-pkg-share bitbots_tts)/launch/tts.launch"/>
         <!--include file="$(find-pkg-share bitbots_ros_control)/launch/pressure_converter.launch"/-->
     </group>
 
+    <group if="$(var tts)">
+        <include file="$(find-pkg-share bitbots_tts)/launch/tts.launch"/>
+    </group >
 </launch>
 

bitbots_lowlevel/bitbots_ros_control/scripts/pose_check.py

@@ -126,37 +126,6 @@ def main():
             f"{Style.BRIGHT}Press enter to continue.{Style.RESET_ALL}"
         )
 
-        ################
-        # Arms-Up-Pose #
-        ################
-
-        step += 1
-        input(
-            f"\n{Fore.YELLOW}{num_to_emoji(step)}: The robot will now move {Style.BRIGHT}its arms up and its head to the right.{Style.RESET_ALL}\n"
-            f"{Style.BRIGHT}Press enter to move.{Style.RESET_ALL}"
-        )
-
-        move_to_joint_position(
-            pub,
-            {
-                "LElbow": -math.pi / 2,
-                "LShoulderRoll": -math.pi,
-                "RElbow": math.pi / 2,
-                "RShoulderRoll": math.pi,
-                "HeadPan": -math.pi / 2,
-            },
-        )
-
-        input(
-            f"{Style.BRIGHT}\nChecks:{Style.RESET_ALL}\n"
-            "- Check that all limbs are straight.\n"
-            "- Check that both arms are symmetrical.\n"
-            "- Check that the head looks 90 degrees to the right.\n"
-            "- Check that the head is horizontal.\n"
-            "- Check for backlash and loose parts.\n"
-            f"{Style.BRIGHT}Press enter to continue.{Style.RESET_ALL}"
-        )
-
         ##########################
         # Arms-To-The-Front-Pose #
         ##########################

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_bringup/launch/monitoring_pc.launch

@@ -7,7 +7,7 @@
     <include file="$(find-pkg-share foxglove_bridge)/launch/foxglove_bridge_launch.xml" />
 
     <!-- start foxglove gui -->
-    <executable cmd="foxglove-studio --no-sandbox" name="foxglove-studio" output="screen"/>
+    <executable cmd="lichtblick --no-sandbox" name="lichtblick" output="screen"/>
 
     <!-- start dynamic_stack_decider_visualization dsd_gui -->
     <node pkg="dynamic_stack_decider_visualization" exec="dsd_gui" name="dsd_gui" output="screen" />

bitbots_misc/bitbots_bringup/launch/motion.launch

@@ -4,23 +4,25 @@
     <arg name="viz" default="false"/>
     <arg name="walking" default="true" description="start the walking" />
     <arg name="torqueless_mode" default="false" description="start without torque, for example for testing the falling detection"/>
+    <arg name="tts" default="true" description="Whether to speak" />
 
     <!-- launch the base footprint -->
     <node pkg="humanoid_base_footprint" exec="base_footprint" output="screen">
-      <param name="support_state_topics" value="[walk_support_state, dynamic_kick_support_state]"/>
-      <param name="use_sim_time" value="$(var sim)"/>
+        <param name="support_state_topics" value="[walk_support_state, dynamic_kick_support_state]"/>
+        <param name="use_sim_time" value="$(var sim)"/>
     </node>
 
     <!-- launch the odometry -->
     <include file="$(find-pkg-share bitbots_odometry)/launch/odometry.launch">
-      <arg name="sim" value="$(var sim)"/>
+        <arg name="sim" value="$(var sim)"/>
     </include>
 
     <!-- if not in simulation or visualization, launch ros_control -->
     <group unless="$(var sim)">
         <group unless="$(var viz)">
             <include file="$(find-pkg-share bitbots_ros_control)/launch/ros_control.launch">
                 <arg name="torqueless_mode" value="$(var torqueless_mode)" />
+                <arg name="tts" value="$(var tts)" />
             </include>
             <node name="set_volume" pkg="bitbots_utils" exec="set_volume.sh" args="100%" />
         </group>

bitbots_misc/bitbots_bringup/launch/teamplayer.launch

@@ -14,6 +14,8 @@
     <arg name="world_model" default="true" description="Whether the world model should be started"/>
     <arg name="monitoring" default="true" description="Whether the system monitor and udp bridge should be started" />
     <arg name="record" default="false" description="Whether the ros bag recording should be started" />
+    <arg name="tts" default="true" description="Whether to speak" />
+
 
     <!-- load the global parameters -->
     <include file="$(find-pkg-share bitbots_parameter_blackboard)/launch/parameter_blackboard.launch">
@@ -32,6 +34,7 @@
     <group if="$(var motion)">
         <include file="$(find-pkg-share bitbots_bringup)/launch/motion.launch">
             <arg name="sim" value="$(var sim)"/>
+            <arg name="tts" value="$(var tts)" />
         </include>
     </group>