Feature/game controller hsl cleanup

scripts/ros.plugin.sh

@@ -33,6 +33,12 @@ update_argcompletes() {
 }
 
 setup_alises() {
+  if [ -n "$IS_ROBOT" ]; then
+    local PIXI_ENVIRONMENT="robot"
+  else
+    local PIXI_ENVIRONMENT="default"
+  fi
+
   # check if we are in subdir of $ROS_WORKSPACE and switch to it otherwise
   alias cdc='[[ "$PWD" = "$ROS_WORKSPACE"* ]] || cd "$ROS_WORKSPACE"'
 
@@ -53,7 +59,7 @@ setup_alises() {
 
   # colcon aliases
   alias colcon='cdc && pixi run colcon'
-  alias cba='cdc && pixi run build'
+  alias cba="cdc && pixi run -e $PIXI_ENVIRONMENT build"
   alias cbs='cba --packages-select'
   alias cb='cba --packages-up-to'
 

src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/game_status_capsule.py

@@ -69,7 +69,7 @@ def get_seconds_since_any_goal(self) -> float:
     def get_seconds_remaining(self) -> float:
         # Time from the message minus time passed since receiving it
         return max(
-            self.gamestate.secs_remaining - (self._node.get_clock().now().nanoseconds / 1e9 - self.last_update), 0.0
+            self.gamestate.seconds_remaining - (self._node.get_clock().now().nanoseconds / 1e9 - self.last_update), 0.0
         )
 
     def get_secondary_seconds_remaining(self) -> float:

src/bitbots_misc/bitbots_bringup/launch/highlevel.launch

@@ -24,6 +24,9 @@
             <arg name="team_id_param_name" value="team_id" />
             <arg name="bot_id_param_name" value="bot_id" />
         </include>
+        <include file="$(find-pkg-share bitbots_player_state)/launch/player_state_aggregator.launch">
+            <arg name="sim" value="$(var sim)" />
+        </include>
     </group>
 
     <!-- launch vision -->

src/bitbots_misc/bitbots_bringup/package.xml

@@ -23,9 +23,11 @@
     <exec_depend>bitbots_head_mover</exec_depend>
     <exec_depend>bitbots_localization</exec_depend>
     <exec_depend>bitbots_odometry</exec_depend>
+    <exec_depend>bitbots_player_state</exec_depend>
     <exec_depend>bitbots_rl_motion</exec_depend>
     <exec_depend>bitbots_rl_walk</exec_depend>
     <exec_depend>bitbots_robot_description</exec_depend>
+    <exec_depend>bitbots_team_communication</exec_depend>
     <exec_depend>bitbots_utils</exec_depend>
     <exec_depend>bitbots_vision</exec_depend>
     <exec_depend>foxglove_bridge</exec_depend>

src/bitbots_misc/bitbots_player_state/bitbots_player_state/__init__.py

@@ -0,0 +1 @@
+

src/bitbots_misc/bitbots_player_state/bitbots_player_state/player_state_aggregator.py

@@ -0,0 +1,101 @@
+import rclpy
+from game_controller_hsl_interfaces.msg import PlayerStateResponse
+from geometry_msgs.msg import PointStamped, PoseStamped, PoseWithCovarianceStamped
+from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
+from rclpy.experimental.events_executor import EventsExecutor
+from rclpy.node import Node
+
+from bitbots_msgs.msg import RobotControlState
+
+
+class PlayerStateAggregator(Node):
+    """Aggregate native robot state into the GameController response interface."""
+
+    FALLEN_STATES = {
+        RobotControlState.FALLING,
+        RobotControlState.FALLEN,
+        RobotControlState.GETTING_UP,
+    }
+
+    def __init__(self) -> None:
+        super().__init__("player_state_aggregator")
+
+        robot_state_topic = self.declare_parameter("robot_state_topic", "robot_state").value
+        pose_topic = self.declare_parameter("pose_topic", "pose_with_covariance").value
+        ball_topic = self.declare_parameter("ball_topic", "ball_position_relative_filtered").value
+        response_topic = self.declare_parameter("response_topic", "player_state_response").value
+        publish_rate = self.declare_parameter("publish_rate", 1.0).value
+
+        if publish_rate <= 0.0:
+            raise ValueError("publish_rate must be greater than zero")
+
+        self._fallen = False
+        self._pose: PoseStamped | None = None
+        self._relative_ball: PointStamped | None = None
+
+        self._publisher = self.create_publisher(
+            PlayerStateResponse, response_topic, 1, callback_group=MutuallyExclusiveCallbackGroup()
+        )
+        self.create_subscription(
+            RobotControlState,
+            robot_state_topic,
+            self._robot_state_callback,
+            1,
+            callback_group=MutuallyExclusiveCallbackGroup(),
+        )
+        self.create_subscription(
+            PoseWithCovarianceStamped,
+            pose_topic,
+            self._pose_callback,
+            1,
+            callback_group=MutuallyExclusiveCallbackGroup(),
+        )
+        self.create_subscription(
+            PoseWithCovarianceStamped,
+            ball_topic,
+            self._ball_callback,
+            1,
+            callback_group=MutuallyExclusiveCallbackGroup(),
+        )
+        self.create_timer(1.0 / publish_rate, self._publish, callback_group=MutuallyExclusiveCallbackGroup())
+
+    def _robot_state_callback(self, msg: RobotControlState) -> None:
+        self._fallen = msg.state in self.FALLEN_STATES
+
+    def _pose_callback(self, msg: PoseWithCovarianceStamped) -> None:
+        self._pose = PoseStamped(header=msg.header, pose=msg.pose.pose)
+
+    def _ball_callback(self, msg: PoseWithCovarianceStamped) -> None:
+        self._relative_ball = PointStamped(header=msg.header, point=msg.pose.pose.position)
+
+    def build_response(self) -> PlayerStateResponse:
+        """Build a consistent snapshot from the most recently received data."""
+        response = PlayerStateResponse()
+        response.header.stamp = self.get_clock().now().to_msg()
+        response.fallen = self._fallen
+
+        if self._pose is not None:
+            response.pose = self._pose
+
+        if self._relative_ball is not None:
+            response.ball = self._relative_ball
+
+        return response
+
+    def _publish(self) -> None:
+        self._publisher.publish(self.build_response())
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = PlayerStateAggregator()
+
+    executor = EventsExecutor()
+    executor.add_node(node)
+    try:
+        executor.spin()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()

src/bitbots_misc/bitbots_player_state/config/player_state_aggregator.yaml

@@ -0,0 +1,7 @@
+player_state_aggregator:
+  ros__parameters:
+    robot_state_topic: robot_state
+    pose_topic: pose_with_covariance
+    ball_topic: ball_position_relative_filtered
+    response_topic: player_state_response
+    publish_rate: 1.0

src/bitbots_misc/bitbots_player_state/launch/player_state_aggregator.launch

@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<launch>
+    <arg name="sim" default="false" description="Whether to use simulation time" />
+    <arg name="config" default="$(find-pkg-share bitbots_player_state)/config/player_state_aggregator.yaml"
+         description="Player state aggregator configuration file" />
+
+    <node pkg="bitbots_player_state" exec="player_state_aggregator" output="screen">
+        <param from="$(var config)" />
+        <param name="use_sim_time" value="$(var sim)" />
+    </node>
+</launch>

src/bitbots_misc/bitbots_player_state/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>bitbots_player_state</name>
+  <version>0.0.0</version>
+  <description>Aggregates robot state for the RoboCup GameController return message.</description>
+
+  <maintainer email="info@bit-bots.de">Hamburg Bit-Bots</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <depend>bitbots_msgs</depend>
+  <depend>game_controller_hsl_interfaces</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclpy</depend>
+
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/bitbots_misc/bitbots_player_state/resource/bitbots_player_state

@@ -0,0 +1 @@
+

src/bitbots_misc/bitbots_player_state/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/bitbots_player_state
+[install]
+install_scripts=$base/lib/bitbots_player_state

src/bitbots_misc/bitbots_player_state/setup.py

@@ -0,0 +1,29 @@
+from glob import glob
+
+from setuptools import find_packages, setup
+
+package_name = "bitbots_player_state"
+
+setup(
+    name=package_name,
+    version="0.0.0",
+    packages=find_packages(exclude=["test"]),
+    data_files=[
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+        ("share/" + package_name + "/config", glob("config/*.yaml")),
+        ("share/" + package_name + "/launch", glob("launch/*.launch")),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="Hamburg Bit-Bots",
+    maintainer_email="info@bit-bots.de",
+    description="Aggregates robot state for the RoboCup GameController return message.",
+    license="MIT",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "player_state_aggregator = bitbots_player_state.player_state_aggregator:main",
+        ],
+    },
+)

src/bitbots_misc/bitbots_player_state/test/test_player_state_aggregator.py

@@ -0,0 +1,46 @@
+from unittest.mock import Mock
+
+import pytest
+from bitbots_player_state.player_state_aggregator import PlayerStateAggregator
+from builtin_interfaces.msg import Time
+from geometry_msgs.msg import Point, PoseWithCovarianceStamped, Quaternion
+from std_msgs.msg import Header
+
+from bitbots_msgs.msg import RobotControlState
+
+
+def test_build_response_aggregates_latest_state(aggregator):
+    pose = PoseWithCovarianceStamped(
+        header=Header(stamp=Time(sec=10), frame_id="map"),
+    )
+    pose.pose.pose.position.x = 1.0
+    pose.pose.pose.position.y = -2.0
+    pose.pose.pose.orientation = Quaternion(w=1.0)
+    ball = PoseWithCovarianceStamped(
+        header=Header(stamp=Time(sec=11), frame_id="base_footprint"),
+    )
+    ball.pose.pose.position = Point(x=0.4, y=-0.2)
+
+    aggregator._pose_callback(pose)
+    aggregator._robot_state_callback(RobotControlState(state=RobotControlState.FALLEN))
+    aggregator._ball_callback(ball)
+
+    response = aggregator.build_response()
+
+    assert response.header == Header(stamp=Time(sec=12), frame_id="")
+    assert response.fallen is True
+    assert response.pose.header == Header(stamp=Time(sec=10), frame_id="map")
+    assert response.pose.pose.position == Point(x=1.0, y=-2.0)
+    assert response.ball.header == Header(stamp=Time(sec=11), frame_id="base_footprint")
+    assert response.ball.point == Point(x=0.4, y=-0.2)
+
+
+@pytest.fixture
+def aggregator():
+    node = object.__new__(PlayerStateAggregator)
+    node._fallen = False
+    node._pose = None
+    node._relative_ball = None
+    node._clock = Mock()
+    node._clock.now.return_value.to_msg.return_value = Time(sec=12)
+    return node

src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/fallen.py

@@ -2,7 +2,6 @@
 
 import numpy as np
 from bitbots_utils.transforms import quat2fused
-from std_msgs.msg import Bool
 
 from bitbots_hcm.hcm_dsd.decisions import AbstractHCMDecisionElement
 
@@ -24,43 +23,32 @@ def __init__(self, blackboard, dsd, parameters):
     def perform(self, reevaluate=False):
         # Check of the fallen detection is active
         if not self.blackboard.is_stand_up_active:
-            self.publish_if_fallen(False)
             return "NOT_FALLEN"
 
         # Get angular velocity from the IMU
         angular_velocity = self.blackboard.gyro
 
         # Check if the robot is rotating
         if np.mean(np.abs(angular_velocity)) >= 0.2:
-            self.publish_if_fallen(False)
             return "NOT_FALLEN"
 
         # Convert quaternion to fused angles
         fused_roll, fused_pitch, _, _ = quat2fused(self.blackboard.quaternion, order="xyzw")
 
         # Decides which side is facing downwards.
         if fused_pitch > self.fallen_orientation_thresh:
-            self.publish_if_fallen(True)
             return "FALLEN_FRONT"
 
         if fused_pitch < -self.fallen_orientation_thresh:
-            self.publish_if_fallen(True)
             return "FALLEN_BACK"
 
         if fused_roll > self.fallen_orientation_thresh:
-            self.publish_if_fallen(True)
             return "FALLEN_RIGHT"
 
         if fused_roll < -self.fallen_orientation_thresh:
-            self.publish_if_fallen(True)
             return "FALLEN_LEFT"
 
-        self.publish_if_fallen(False)
         return "NOT_FALLEN"
 
-    def publish_if_fallen(self, is_fallen):
-        # publishes if robot is fallen
-        self.blackboard.is_fallen_publisher.publish(Bool(data=is_fallen))
-
     def get_reevaluate(self):
         return True

src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm_blackboard.py

@@ -8,7 +8,6 @@
 from rclpy.task import Future
 from rclpy.time import Time
 from sensor_msgs.msg import Imu, JointState
-from std_msgs.msg import Bool
 from std_msgs.msg import Empty as EmptyMsg
 
 from bitbots_hcm.type_utils import T_RobotControlState
@@ -45,7 +44,6 @@ def __init__(self, node: Node):
         self.cancel_path_planning_pub = self.node.create_publisher(EmptyMsg, "pathfinding/cancel", 1)
         self.speak_publisher = self.node.create_publisher(TTS, "speak", 1)
         self.torque_publisher = self.node.create_publisher(JointTorque, "set_torque_individual", 10)
-        self.is_fallen_publisher = self.node.create_publisher(Bool, "hsl_gamecontroller/is_fallen", 1)
 
         # Latest imu data
         self.accel = numpy.array([0, 0, 0])

src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication.py

@@ -11,7 +11,7 @@
 from bitbots_tf_buffer import Buffer
 from bitbots_utils.utils import get_parameter_dict, get_parameters_from_other_node
 from builtin_interfaces.msg import Time as TimeMsg
-from game_controller_hsl_interfaces.msg import GameState, PlayerStatusPose
+from game_controller_hsl_interfaces.msg import GameState
 from geometry_msgs.msg import PoseWithCovarianceStamped, Quaternion, Twist, TwistWithCovarianceStamped
 from numpy import double
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
@@ -121,9 +121,6 @@ def try_to_establish_connection(self):
 
     def create_publishers(self):
         self.team_data_publisher = self.node.create_publisher(TeamData, self.topics["team_data_topic"], qos_profile=1)
-        self.game_controller_player_pose_publisher = self.node.create_publisher(
-            PlayerStatusPose, "hsl_gamecontroller/pose_stamped", 1
-        )
 
     def create_subscribers(self):
         self.node.create_subscription(
@@ -196,15 +193,6 @@ def gamestate_cb(self, msg: GameState):
     def pose_cb(self, msg: PoseWithCovarianceStamped):
         self.pose = msg
 
-        player_pose_msg = PlayerStatusPose()
-        player_pose_msg.header = msg.header
-        player_pose_msg.pose = [
-            msg.pose.pose.position.x,
-            msg.pose.pose.position.y,
-            self.extract_orientation_yaw_angle(msg.pose.pose.orientation),
-        ]
-        self.game_controller_player_pose_publisher.publish(player_pose_msg)
-
     def cmd_vel_cb(self, msg: Twist):
         self.cmd_vel = msg
         self.cmd_vel_time = self.get_current_time().to_msg()