diff --git a/scripts/ros.plugin.sh b/scripts/ros.plugin.sh index 9461ff2af..f28f144c3 100755 --- a/scripts/ros.plugin.sh +++ b/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' diff --git a/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/game_status_capsule.py b/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/game_status_capsule.py index 6e1abba8d..94bc20237 100644 --- a/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/game_status_capsule.py +++ b/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: diff --git a/src/bitbots_misc/bitbots_bringup/launch/highlevel.launch b/src/bitbots_misc/bitbots_bringup/launch/highlevel.launch index f674c92e5..71a4679a9 100644 --- a/src/bitbots_misc/bitbots_bringup/launch/highlevel.launch +++ b/src/bitbots_misc/bitbots_bringup/launch/highlevel.launch @@ -24,6 +24,9 @@ + + + diff --git a/src/bitbots_misc/bitbots_bringup/package.xml b/src/bitbots_misc/bitbots_bringup/package.xml index 2ef2d7316..51cd1facb 100644 --- a/src/bitbots_misc/bitbots_bringup/package.xml +++ b/src/bitbots_misc/bitbots_bringup/package.xml @@ -23,9 +23,11 @@ bitbots_head_mover bitbots_localization bitbots_odometry + bitbots_player_state bitbots_rl_motion bitbots_rl_walk bitbots_robot_description + bitbots_team_communication bitbots_utils bitbots_vision foxglove_bridge diff --git a/src/bitbots_misc/bitbots_player_state/bitbots_player_state/__init__.py b/src/bitbots_misc/bitbots_player_state/bitbots_player_state/__init__.py new file mode 100644 index 000000000..8b1378917 --- /dev/null +++ b/src/bitbots_misc/bitbots_player_state/bitbots_player_state/__init__.py @@ -0,0 +1 @@ + diff --git a/src/bitbots_misc/bitbots_player_state/bitbots_player_state/player_state_aggregator.py b/src/bitbots_misc/bitbots_player_state/bitbots_player_state/player_state_aggregator.py new file mode 100644 index 000000000..96c7f5f7f --- /dev/null +++ b/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() diff --git a/src/bitbots_misc/bitbots_player_state/config/player_state_aggregator.yaml b/src/bitbots_misc/bitbots_player_state/config/player_state_aggregator.yaml new file mode 100644 index 000000000..d5fe3b0ce --- /dev/null +++ b/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 diff --git a/src/bitbots_misc/bitbots_player_state/launch/player_state_aggregator.launch b/src/bitbots_misc/bitbots_player_state/launch/player_state_aggregator.launch new file mode 100644 index 000000000..c2e862459 --- /dev/null +++ b/src/bitbots_misc/bitbots_player_state/launch/player_state_aggregator.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/src/bitbots_misc/bitbots_player_state/package.xml b/src/bitbots_misc/bitbots_player_state/package.xml new file mode 100644 index 000000000..5fa7f3428 --- /dev/null +++ b/src/bitbots_misc/bitbots_player_state/package.xml @@ -0,0 +1,23 @@ + + + + bitbots_player_state + 0.0.0 + Aggregates robot state for the RoboCup GameController return message. + + Hamburg Bit-Bots + MIT + + ament_python + + bitbots_msgs + game_controller_hsl_interfaces + geometry_msgs + rclpy + + python3-pytest + + + ament_python + + diff --git a/src/bitbots_misc/bitbots_player_state/resource/bitbots_player_state b/src/bitbots_misc/bitbots_player_state/resource/bitbots_player_state new file mode 100644 index 000000000..8b1378917 --- /dev/null +++ b/src/bitbots_misc/bitbots_player_state/resource/bitbots_player_state @@ -0,0 +1 @@ + diff --git a/src/bitbots_misc/bitbots_player_state/setup.cfg b/src/bitbots_misc/bitbots_player_state/setup.cfg new file mode 100644 index 000000000..5716f3bc5 --- /dev/null +++ b/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 diff --git a/src/bitbots_misc/bitbots_player_state/setup.py b/src/bitbots_misc/bitbots_player_state/setup.py new file mode 100644 index 000000000..6ebcd47d5 --- /dev/null +++ b/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", + ], + }, +) diff --git a/src/bitbots_misc/bitbots_player_state/test/test_player_state_aggregator.py b/src/bitbots_misc/bitbots_player_state/test/test_player_state_aggregator.py new file mode 100644 index 000000000..5c60a4043 --- /dev/null +++ b/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 diff --git a/src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/fallen.py b/src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/fallen.py index 79652a3e5..0429ed5bf 100644 --- a/src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/fallen.py +++ b/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,7 +23,6 @@ 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 @@ -32,7 +30,6 @@ def perform(self, reevaluate=False): # 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 @@ -40,27 +37,18 @@ def perform(self, reevaluate=False): # 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 diff --git a/src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm_blackboard.py b/src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm_blackboard.py index 46051d24e..61af7f7eb 100644 --- a/src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm_blackboard.py +++ b/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]) diff --git a/src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication.py b/src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication.py index defdcd458..afa2dc960 100755 --- a/src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication.py +++ b/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() diff --git a/src/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py b/src/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py index 518706c49..5be9db095 100755 --- a/src/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py +++ b/src/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py @@ -14,7 +14,7 @@ from ros2_numpy import msgify, numpify from sensor_msgs.msg import CameraInfo from soccer_vision_3d_msgs.msg import Ball, BallArray -from std_msgs.msg import Float32, Header +from std_msgs.msg import Header from std_srvs.srv import Trigger from tf2_geometry_msgs import PointStamped, PoseStamped @@ -39,8 +39,6 @@ def __init__(self) -> None: # Initialize parameters self.update_params() self.logger.info(f"Using frame '{self.config.filter.frame}' for ball filtering") - self.last_ball_time: Time = self.get_clock().now() - self.camera_info: Optional[CameraInfo] = None # Initialize state @@ -50,10 +48,6 @@ def __init__(self) -> None: self.ball_pose_publisher = self.create_publisher( PoseWithCovarianceStamped, self.config.ros.ball_position_publish_topic, 1 ) - self.game_controller_ball_position_publisher = self.create_publisher( - PointStamped, "hsl_gamecontroller/ball_position", 1 - ) - self.ball_age_publisher = self.create_publisher(Float32, "hsl_gamecontroller/ball_age", 1) # Create callback group self.callback_group = MutuallyExclusiveCallbackGroup() @@ -146,8 +140,6 @@ def ball_callback(self, msg: BallArray) -> None: # Store the ball measurement self.ball_state_position = numpify(ball_measurement_map.point) self.ball_state_covariance = covariance - # @TODO: actually give last ball time - self.last_ball_time = self.get_clock().now() ball_measurement_updated = True # If we did not get a ball measurement, we can check if we should have seen the ball @@ -254,21 +246,6 @@ def filter_step(self) -> None: pose_msg.pose.pose.orientation.w = 1.0 self.ball_pose_publisher.publish(pose_msg) - ball_position_msg = PointStamped() - ball_position_msg.point = msgify(Point, self.ball_state_position) - ball_position_msg.header = Header( - stamp=Time.to_msg(self.get_clock().now()), - frame_id=self.config.filter.frame, - ) - self.game_controller_ball_position_publisher.publish(ball_position_msg) - - # Build message for Ball age - ball_age_msg = Float32() - seconds_since_last_ball = (self.get_clock().now() - self.last_ball_time).nanoseconds / 1e9 - ball_age_msg.data = seconds_since_last_ball - - self.ball_age_publisher.publish(ball_age_msg) - def main(args=None) -> None: rclpy.init(args=args) diff --git a/src/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml b/src/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml index 2cd6e8ece..604d8fa5e 100644 --- a/src/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml +++ b/src/bitbots_world_model/bitbots_ball_filter/config/ball_filter_parameters.yaml @@ -17,8 +17,8 @@ bitbots_ball_filter: default_value: 'ball_position_relative_filtered' read_only: true description: 'Topic to publish the filtered ball position' - - ball_filter_reset_service_name: + + ball_filter_reset_service_name: type: string default_value: 'ball_filter_reset' read_only: true @@ -38,11 +38,11 @@ bitbots_ball_filter: default_value: 62 description: 'Filtering rate in Hz' read_only: true - validation: - bounds<>: [0, 100] + validation: + bounds<>: [0, 100] covariance: - process_noise: + process_noise: type: double default_value: 0.002 description: 'Noise which is added to the estimated position without new measurements' @@ -55,14 +55,14 @@ bitbots_ball_filter: description: 'Ball measurement certainty in filter' validation: bounds<>: [0.0, 1.0] - - distance_factor: + + distance_factor: type: double default_value: 0.02 description: 'Factor to increase the noise if the ball measurement is further away. This also depends on the reference distance.' validation: bounds<>: [0.0, 1.0] - + negative_observation: value: type: double diff --git a/src/lib/game_controller_hsl/.gitrepo b/src/lib/game_controller_hsl/.gitrepo index 991c15b70..4685eeb92 100644 --- a/src/lib/game_controller_hsl/.gitrepo +++ b/src/lib/game_controller_hsl/.gitrepo @@ -5,8 +5,8 @@ ; [subrepo] remote = git@github.com:bit-bots/game_controller_hsl.git - branch = rolling - commit = b92a2693616b8ca4849eca14363dda06e8e1f3d8 - parent = 21a107e9266bd5711d75963818a3c364c50a180b + branch = feature/abstract-for-open-source + commit = 7c48f279e21d6cad9d280e8c46056a061f435a2d + parent = 6a3c87d724cbb1dad813fabb55ef94248ac4a1b7 method = merge cmdver = 0.4.9 diff --git a/src/lib/game_controller_hsl/game_controller_hsl/config/game_controller_settings.yaml b/src/lib/game_controller_hsl/game_controller_hsl/config/game_controller_settings.yaml index 7f8c2c873..1e9134805 100644 --- a/src/lib/game_controller_hsl/game_controller_hsl/config/game_controller_settings.yaml +++ b/src/lib/game_controller_hsl/game_controller_hsl/config/game_controller_settings.yaml @@ -6,3 +6,4 @@ game_controller_hsl: listen_host: '0.0.0.0' listen_port: 3838 answer_port: 3939 + lost_timeout: 5 diff --git a/src/lib/game_controller_hsl/game_controller_hsl/game_controller_hsl/data/__init__.py b/src/lib/game_controller_hsl/game_controller_hsl/game_controller_hsl/data/__init__.py new file mode 100644 index 000000000..4820389d8 --- /dev/null +++ b/src/lib/game_controller_hsl/game_controller_hsl/game_controller_hsl/data/__init__.py @@ -0,0 +1,2 @@ +from game_controller_hsl.data.game_control_data import GameControlDataStruct +from game_controller_hsl.data.game_control_return_data import GameControlReturnDataStruct diff --git a/src/lib/game_controller_hsl/game_controller_hsl/game_controller_hsl/data/game_control_data.py b/src/lib/game_controller_hsl/game_controller_hsl/game_controller_hsl/data/game_control_data.py new file mode 100644 index 000000000..5884f6a1a --- /dev/null +++ b/src/lib/game_controller_hsl/game_controller_hsl/game_controller_hsl/data/game_control_data.py @@ -0,0 +1,128 @@ +#!/usr/bin/env python +# -*- coding:utf-8 -*- + +from construct import ( + Array, + Byte, + Const, + Enum, + Flag, + Int16sl, + Int16ul, + Struct, +) + +GAME_CONTROLLER_STRUCT_VERSION = 20 +GAME_CONTROLLER_STRUCT_HEADER = b"RGme" + +MAX_NUM_PLAYERS = 20 + +Short = Int16ul + +RobotInfoStruct = Struct( + "penalty" / Enum( + Byte, + PENALTY_NONE=0, + PENALTY_ILLEGAL_POSITIONING=1, + PENALTY_MOTION_IN_SET=2, + PENALTY_MOTION_IN_STOP=3, + PENALTY_LOCAL_GAME_STUCK=4, + PENALTY_INCAPABLE_ROBOT=5, + PENALTY_PICK_UP=6, + PENALTY_BALL_HOLDING=7, + PENALTY_LEAVING_THE_FIELD=8, + PENALTY_PLAYING_WITH_ARMS_HANDS=9, + PENALTY_PUSHING=10, + PENALTY_CAUTIONED=11, + PENALTY_SENT_OFF=12, + PENALTY_SUBSTITUTE=13, + ), + "secs_till_unpenalized" / Byte, + "cautions" / Byte, +) + +TeamInfoStruct = Struct( + "team_number" / Byte, + "field_player_color" + / Enum( + Byte, + BLUE=0, + RED=1, + YELLOW=2, + BLACK=3, + WHITE=4, + GREEN=5, + ORANGE=6, + PURPLE=7, + BROWN=8, + GRAY=9, + ), + "goalkeeper_color" + / Enum( + Byte, + BLUE=0, + RED=1, + YELLOW=2, + BLACK=3, + WHITE=4, + GREEN=5, + ORANGE=6, + PURPLE=7, + BROWN=8, + GRAY=9, + ), + "goalkeeper" / Byte, + "score" / Byte, + "penalty_shot" / Byte, # penalty shot counter + "single_shots" / Short, # bits represent penalty shot success + "message_budget" / Short, # number of team messages the team is allowed to send for the remainder of the game + "players" / Array(MAX_NUM_PLAYERS, RobotInfoStruct), +) + +GameControlDataStruct = Struct( + "header" / Const(GAME_CONTROLLER_STRUCT_HEADER), + "version" / Const(GAME_CONTROLLER_STRUCT_VERSION, Byte), + "packet_number" / Byte, + "players_per_team" / Byte, + "competition_type" + / Enum( + Byte, + COMPETITION_TYPE_SMALL=0, + COMPETITION_TYPE_MIDDLE=1, + COMPETITION_TYPE_LARGE=2, + ), + "stopped" / Flag, + "game_phase" + / Enum( + Byte, + GAME_PHASE_NORMAL=0, + GAME_PHASE_PENALTY_SHOOT_OUT=1, + GAME_PHASE_EXTRA_TIME=2, + GAME_PHASE_TIMEOUT=3, + ), + "state" + / Enum( + Byte, + STATE_INITIAL=0, + STATE_READY=1, # Go to (starting) position + STATE_SET=2, # Hold position (no motion allowed) + STATE_PLAYING=3, # Normal play + STATE_FINISHED=4, # Game over + ), + "set_play" + / Enum( + Byte, + SET_PLAY_NONE=0, + SET_PLAY_DIRECT_FREE_KICK=1, + SET_PLAY_INDIRECT_FREE_KICK=2, + SET_PLAY_PENALTY_KICK=3, + SET_PLAY_THROW_IN=4, + SET_PLAY_GOAL_KICK=5, + SET_PLAY_CORNER_KICK=6, + ), + "first_half" / Flag, + "kicking_team" / Byte, + "secs_remaining" / Int16sl, + "secondary_time" / Int16sl, + "teams" / Array(2, TeamInfoStruct), +) diff --git a/src/lib/game_controller_hsl/game_controller_hsl/game_controller_hsl/data/game_control_return_data.py b/src/lib/game_controller_hsl/game_controller_hsl/game_controller_hsl/data/game_control_return_data.py new file mode 100644 index 000000000..daa9a8720 --- /dev/null +++ b/src/lib/game_controller_hsl/game_controller_hsl/game_controller_hsl/data/game_control_return_data.py @@ -0,0 +1,22 @@ +#!/usr/bin/env python +from construct import ( + Byte, + Const, + Flag, + Float32l, + Struct, +) + +GAME_CONTROLLER_RETURN_STRUCT_VERSION = 4 +GAME_CONTROLLER_RETURN_STRUCT_HEADER = b"RGrt" + +GameControlReturnDataStruct = Struct( + "header" / Const(GAME_CONTROLLER_RETURN_STRUCT_HEADER), + "version" / Const(GAME_CONTROLLER_RETURN_STRUCT_VERSION, Byte), + "player_number" / Byte, + "team_number" / Byte, + "fallen" / Flag, + "pose" / Float32l[3], + "ball_age" / Float32l, + "ball" / Float32l[2], +) diff --git a/src/lib/game_controller_hsl/game_controller_hsl/game_controller_hsl/receiver.py b/src/lib/game_controller_hsl/game_controller_hsl/game_controller_hsl/receiver.py index 27cbf2cf5..7eb243cf2 100755 --- a/src/lib/game_controller_hsl/game_controller_hsl/game_controller_hsl/receiver.py +++ b/src/lib/game_controller_hsl/game_controller_hsl/game_controller_hsl/receiver.py @@ -13,30 +13,38 @@ # limitations under the License. import socket + import rclpy -from construct import ConstError +from ros2_numpy import numpify + +from construct import ConstError, Container +from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus +from rclpy.duration import Duration from rclpy.node import Node from rclpy.time import Time -from rclpy.duration import Duration from std_msgs.msg import Header -from construct import Container -from geometry_msgs.msg import PointStamped -from std_msgs.msg import Float32, Bool -from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus -from game_controller_hsl.gamestate import GameStateStruct, ResponseStruct + +from game_controller_hsl.data import GameControlDataStruct, GameControlReturnDataStruct from game_controller_hsl.utils import get_parameters_from_other_node -from game_controller_hsl_interfaces.msg import GameState, PlayerStatusPose +from game_controller_hsl_interfaces.msg import GameState, PlayerStateResponse + +from tf_transformations import euler_from_quaternion class GameStateReceiver(Node): - """This class puts up a simple UDP Server which receives the - *addr* parameter to listen to the packages from the game_controller. + """ + This class provides a simple UDP Server which listens to packages from the game_controller on *listen_host* and *listen_port* + and respond with the robots state on *answer_port*. - If it receives a package it will be interpreted with the construct data - structure and the :func:`on_new_gamestate` will be called with the content. + If it receives a package it will be interpreted with the construct data structure and the :func:`build_game_state_msg` + will be called with the content to build a ros message of type :class:`game_controller_hsl_interfaces.msg.GameState` + which is then published on the topic */gamestate*. - After this we send a package back to the GC""" + On a received ros message of type :class:`game_controller_hsl_interfaces.msg.PlayerStateResponse` on topic + */player_state_response* the current state is updated and will be sent back to the game controller on the next + received package. + """ def __init__(self, *args, **kwargs): super().__init__( @@ -51,125 +59,100 @@ def __init__(self, *args, **kwargs): if self.has_parameter("team_id") and self.has_parameter("bot_id"): self.get_logger().info("Found team_id and bot_id parameter, using them") # Get the parameters from our node - self.team_number = self.get_parameter("team_id").value - self.player_number = self.get_parameter("bot_id").value + self.team_number: int = self.get_parameter("team_id").get_parameter_value().integer_value + self.player_number: int = self.get_parameter("bot_id").get_parameter_value().integer_value else: self.get_logger().info( "No team_id and bot_id parameter set in game_controller, getting them from blackboard" ) # Get the parameter names from the parameter server - param_blackboard_name: str = self.get_parameter("parameter_blackboard_name").value - team_id_param_name: str = self.get_parameter("team_id_param_name").value - bot_id_param_name: str = self.get_parameter("bot_id_param_name").value - # Get the parameters from the blackboard + param_blackboard_name: str = self.get_parameter("parameter_blackboard_name").get_parameter_value().string_value + team_id_param_name: str = self.get_parameter("team_id_param_name").get_parameter_value().string_value + bot_id_param_name: str = self.get_parameter("bot_id_param_name").get_parameter_value().string_value + params = get_parameters_from_other_node( self, param_blackboard_name, [team_id_param_name, bot_id_param_name] ) - # Set the parameters self.team_number = params[team_id_param_name] self.player_number = params[bot_id_param_name] - self.is_fallen: bool = False - self.ball_age: float = 100 # Start with high value to show low confidence - self.ball_position_msg: PointStamped = PointStamped() - self.pose_msg: PlayerStatusPose = PlayerStatusPose() - - # Create subscribers - self.create_subscription(PointStamped, "hsl_gamecontroller/ball_position", self.ball_position_cb, 1) - self.create_subscription(Float32, "hsl_gamecontroller/ball_age", self.ball_age_cb, 1) - self.create_subscription(Bool, "hsl_gamecontroller/is_fallen", self.is_fallen_cb, 1) - self.create_subscription(PlayerStatusPose, "hsl_gamecontroller/pose", self.pose_cb, 1) + self.player_state_msg: PlayerStateResponse = PlayerStateResponse() + self.create_subscription(PlayerStateResponse, "player_state_response", self.player_state_cb, 1) self.get_logger().info(f"We are playing as player {self.player_number} in team {self.team_number}") - # The publisher for the game state self.state_publisher = self.create_publisher(GameState, "gamestate", 1) - - # The publisher for the diagnostics self.diagnostic_pub = self.create_publisher(DiagnosticArray, "diagnostics", 1) - # The time in seconds after which we assume the game controller is lost - # and we tell the robot to move - self.game_controller_lost_time = 5 + # The time in seconds after which we assume the game controller is lost and publish a warning in the diagnostics + self.game_controller_lost_timeout: int = self.get_parameter("lost_timeout").get_parameter_value().integer_value # The address listening on and the port for sending back the robots meta data - self.addr = (self.get_parameter("listen_host").value, self.get_parameter("listen_port").value) + self.listening_address = (self.get_parameter("listen_host").value, self.get_parameter("listen_port").value) self.answer_port = self.get_parameter("answer_port").value # The time of the last package self.last_package_time: Time = self.get_clock().now() # Create the socket we want to use for the communications - self.socket = self._open_socket() - - def ball_position_cb(self, msg: PointStamped): - self.ball_position_msg = msg + self.socket = self._open_socket(self.listening_address) - def ball_age_cb(self, msg: Float32): - self.ball_age = msg.data + def player_state_cb(self, msg: PlayerStateResponse): + self.player_state_msg = msg - def is_fallen_cb(self, msg: Bool): - self.is_fallen = msg.data + def _open_socket(self, address: tuple[str, str]) -> socket.socket: + """Creates a UDP socket to listen on the given address and port with a timeout of 2 seconds.""" + udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + udp_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + udp_socket.bind(address) + udp_socket.settimeout(2) - def pose_cb(self, msg: PlayerStatusPose): - self.pose_msg = msg - - def _open_socket(self) -> socket.socket: - """Creates the socket""" - new_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) - new_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) - new_socket.bind(self.addr) - new_socket.settimeout(2) - return new_socket + return udp_socket def receive_forever(self): - """Waits in a loop for new packages""" + """Waits in a loop for new packages from the game controller.""" while rclpy.ok(): # Try to receive a package self.receive_and_answer_once() # Check if we didn't receive a package for a long time for publishing diagnostics received_message_lately = self.get_time_since_last_package() < Duration( - seconds=self.game_controller_lost_time + seconds=self.game_controller_lost_timeout ) self.publish_diagnostics(received_message_lately) def receive_and_answer_once(self): - """Receives a package, interprets it and sends an answer.""" + """ + Receives a package, parses it and publishes the GameState. + Then sends back the current player state to the game controller. + """ try: # Receive the package - data, peer = self.socket.recvfrom(GameStateStruct.sizeof()) + data, peer = self.socket.recvfrom(GameControlDataStruct.sizeof()) - # Parse the package based on the GameStateStruct + # Parse the package based on the game controllers GameControlDataStruct. # This throws a ConstError if it doesn't work - parsed_state = GameStateStruct.parse(data) - - # Assign the new package after it parsed successful to the state + parsed_state = GameControlDataStruct.parse(data) self.last_package_time = self.get_clock().now() - # Build the game state message and publish it self.state_publisher.publish(self.build_game_state_msg(parsed_state)) + self.answer_to_game_controller(peer) - # Answer the GameController - self.answer_to_gamecontroller(peer) - - except AssertionError as ae: - self.get_logger().error(str(ae)) - except socket.timeout: + except AssertionError as e: + self.get_logger().error(str(e)) + except TimeoutError: self.get_logger().info("No GameController message received (socket timeout)", throttle_duration_sec=5) - except ConstError: - self.get_logger().warn("Parse Error: Probably using an old protocol!") - except IOError as e: + except ConstError as e: + self.get_logger().error(f"Parse Error: Probably using an old protocol! {str(e)}") + except OSError as e: self.get_logger().warn(f"Error while sending keep-alive: {str(e)}") def publish_diagnostics(self, received_message_lately: bool): """ - This publishes a Diagnostics Array. + This publishes a Diagnostics Array message with the status of the game controller connection. """ - # initialize DiagnsticArray message diag_array = DiagnosticArray() - - # configure DiagnosticStatus message diag = DiagnosticStatus(name="Game Controller", hardware_id="Game Controller") + if not received_message_lately: self.get_logger().info("No GameController message received", throttle_duration_sec=5) diag.message = ( @@ -188,36 +171,58 @@ def publish_diagnostics(self, received_message_lately: bool): diag_array.header.stamp = self.get_clock().now().to_msg() self.diagnostic_pub.publish(diag_array) - def answer_to_gamecontroller(self, peer): - """Sends a life sign to the game controller""" - # Build the answer package - data = ResponseStruct.build( + def build_game_control_return_data(self, player_state: PlayerStateResponse) -> bytes: + """ + Convert a player state message into a serialized GameControlReturnData packet. + Which expects standardized units of millimeters and uses the euler yaw angle for player orientation. + """ + pose = [ + player_state.pose.pose.position.x * 1000.0, + player_state.pose.pose.position.y * 1000.0, + euler_from_quaternion(numpify(player_state.pose.pose.orientation))[2] + ] + + ball_age = -1.0 + ball_timestamp = Time.from_msg(player_state.ball.header.stamp) + if ball_timestamp.nanoseconds > 0: + ball_age = (self.get_clock().now() - ball_timestamp).nanoseconds / 1e9 + + ball = [ + player_state.ball.point.x * 1000.0, + player_state.ball.point.y * 1000.0, + ] + + return GameControlReturnDataStruct.build( dict( player_number=self.player_number, team_number=self.team_number, - fallen=self.is_fallen, - pose=self.pose_msg.pose, - ball_age=self.ball_age, - ball=[self.ball_position_msg.point.x, self.ball_position_msg.point.y], + fallen=player_state.fallen, + pose=pose, + ball_age=ball_age, + ball=ball ) ) - # Send the package + + def answer_to_game_controller(self, peer): + """Send the latest player state to the GameController.""" + data = self.build_game_control_return_data(self.player_state_msg) + self.get_logger().debug(f"Sending answer to {peer[0]}:{self.answer_port}") try: self.socket.sendto(data, (peer[0], self.answer_port)) except Exception as e: self.get_logger().error(f"Network Error: {str(e)}") - def build_game_state_msg(self, state) -> GameState: - """Builds a GameState message from the game state""" + def build_game_state_msg(self, game_control_data) -> GameState: + """Builds a GameState ros message from the game state received from the game controller""" # Get the team objects sorted into own and rival team - own_team = GameStateReceiver.select_team_by(lambda team: team.team_number == self.team_number, state.teams) - rival_team = GameStateReceiver.select_team_by(lambda team: team.team_number != self.team_number, state.teams) + own_team = GameStateReceiver.select_team_by(lambda team: team.team_number == self.team_number, game_control_data.teams) + rival_team = GameStateReceiver.select_team_by(lambda team: team.team_number != self.team_number, game_control_data.teams) # Add some assertions to make sure everything is fine assert not (own_team is None or rival_team is None), ( - f"Team {self.team_number} not playing, only {state.teams[0].team_number} and {state.teams[1].team_number}" + f"Team {self.team_number} not playing, only {game_control_data.teams[0].team_number} and {game_control_data.teams[1].team_number}" ) assert self.player_number <= len(own_team.players), f"Robot {self.player_number} not playing" @@ -226,41 +231,38 @@ def build_game_state_msg(self, state) -> GameState: return GameState( header=Header(stamp=self.get_clock().now().to_msg()), - players_per_team=state.players_per_team, - competition_type=state.competition_type.intvalue, - game_phase=state.game_phase.intvalue, - main_state=state.state.intvalue, - set_play=state.set_play.intvalue, - kicking_team=state.kicking_team, - first_half=state.first_half, - stopped=state.stopped, + players_per_team=game_control_data.players_per_team, + competition_type=game_control_data.competition_type.intvalue, + game_phase=game_control_data.game_phase.intvalue, + main_state=game_control_data.state.intvalue, + set_play=game_control_data.set_play.intvalue, + kicking_team=game_control_data.kicking_team, + first_half=game_control_data.first_half, + stopped=game_control_data.stopped, own_score=own_team.score, rival_score=rival_team.score, - secs_remaining=state.secs_remaining, - secondary_time=state.secondary_time, - penalized=this_robot.penalty != 0, - #2 is the motion in set penalty - penalized_in_place=this_robot.penalty == 2, + seconds_remaining=game_control_data.secs_remaining, + secondary_time=game_control_data.secondary_time, + penalized=this_robot.penalty.intvalue != GameState.PENALTY_NONE, + # In the current rules the only in place penalty is the motion in set + penalized_in_place=this_robot.penalty.intvalue == GameState.PENALTY_MOTION_IN_SET, seconds_till_unpenalized=this_robot.secs_till_unpenalized, - warings=this_robot.warnings, cautions=this_robot.cautions, + has_yellow_card=this_robot.cautions== 1, + has_red_card=this_robot.penalty.intvalue == GameState.PENALTY_SENT_OFF, own_player_color=own_team.field_player_color.intvalue, own_goalie_color=own_team.goalkeeper_color.intvalue, rival_player_color=rival_team.field_player_color.intvalue, rival_goalie_color=rival_team.goalkeeper_color.intvalue, - # --- Gibt es nicht mehr? --- - # drop_in_team = state.drop_in_team, - # drop_in_time = state.drop_in_time, penalty_shot=own_team.penalty_shot, single_shots=own_team.single_shots, - # --- Gibt es nicht mehr? Waren die ähnlich wie Message Budget? --- - # coach_message = own_team.coach_message, message_budget=own_team.message_budget, - team_mates_with_penalty=[player.penalty != 0 for player in own_team.players], - # --- Gibt es nicht mehr? Selber bemerken ob man rot hat? --- - # team_mates_with_red_card = [player.number_of_red_cards != 0 for player in own_team.players], + team_mates_with_penalty=[player.penalty.intvalue != GameState.PENALTY_NONE for player in own_team.players], + team_mates_with_yellow_card = [player.cautions == 1 for player in own_team.players], + team_mates_with_red_card = [player.penalty.intvalue == GameState.PENALTY_SENT_OFF for player in own_team.players], ) + def get_time_since_last_package(self) -> Duration: """Returns the time in seconds since the last package was received""" return self.get_clock().now() - self.last_package_time diff --git a/src/lib/game_controller_hsl/game_controller_hsl/game_controller_hsl/utils.py b/src/lib/game_controller_hsl/game_controller_hsl/game_controller_hsl/utils.py index 58a24d571..4eae9cd82 100644 --- a/src/lib/game_controller_hsl/game_controller_hsl/game_controller_hsl/utils.py +++ b/src/lib/game_controller_hsl/game_controller_hsl/game_controller_hsl/utils.py @@ -13,13 +13,14 @@ def get_parameters_from_other_node(own_node: Node, """ Used to receive parameters from other running nodes. Returns a dict with requested parameter name as dict key and parameter value as dict value. - + From bitbots_utils (https://github.com/bit-bots/bitbots_misc) """ client = own_node.create_client(GetParameters, f'{other_node_name}/get_parameters') ready = client.wait_for_service(timeout_sec=service_timeout_sec) if not ready: - raise RuntimeError(f'Wait for {other_node_name} parameter service timed out') + raise TimeoutError(f'Wait for {other_node_name} parameter service timed out') + request = GetParameters.Request() request.names = parameter_names future = client.call_async(request) @@ -29,4 +30,5 @@ def get_parameters_from_other_node(own_node: Node, results = {} # Received parameter for i, param in enumerate(parameter_names): results[param] = parameter_value_to_python(response.values[i]) - return results \ No newline at end of file + + return results diff --git a/src/lib/game_controller_hsl/game_controller_hsl/launch/game_controller.launch b/src/lib/game_controller_hsl/game_controller_hsl/launch/game_controller.launch index 1e4cedbc6..4c031436e 100644 --- a/src/lib/game_controller_hsl/game_controller_hsl/launch/game_controller.launch +++ b/src/lib/game_controller_hsl/game_controller_hsl/launch/game_controller.launch @@ -28,7 +28,7 @@ - + diff --git a/src/lib/game_controller_hsl/game_controller_hsl/package.xml b/src/lib/game_controller_hsl/game_controller_hsl/package.xml index e86607bf8..c2916b5d5 100644 --- a/src/lib/game_controller_hsl/game_controller_hsl/package.xml +++ b/src/lib/game_controller_hsl/game_controller_hsl/package.xml @@ -5,11 +5,10 @@ 1.1.0 The game_controller_hsl packages receives packets from the GameController and - republishes them as GameState ROS messages. It sends response packets - back to the GameController. + republishes them as GameState ROS messages. It sends response packets based on the + PlayerStateResponse ROS messages back to the GameController. - Timon Engelke Hamburg Bit-Bots Apache License 2.0 @@ -19,11 +18,14 @@ rosidl_default_generators rosidl_default_runtime - diagnostic_msgs game_controller_hsl_interfaces + tf_transformations python3-construct + ros2-numpy rclpy std_msgs + geometry_msgs + diagnostic_msgs ament_python diff --git a/src/lib/game_controller_hsl/game_controller_hsl/scripts/sim_gamestate.py b/src/lib/game_controller_hsl/game_controller_hsl/scripts/sim_gamestate.py index a235d4c47..b89207165 100755 --- a/src/lib/game_controller_hsl/game_controller_hsl/scripts/sim_gamestate.py +++ b/src/lib/game_controller_hsl/game_controller_hsl/scripts/sim_gamestate.py @@ -5,158 +5,258 @@ # The script provides a simple mechanism to test robot behavior in different game states, # when no game controller is running -import sys import select +import sys import termios import tty import rclpy +from game_controller_hsl.utils import get_parameters_from_other_node +from game_controller_hsl_interfaces.msg import GameState from rclpy.node import Node -from rclpy.qos import QoSProfile, DurabilityPolicy +from rclpy.qos import DurabilityPolicy, QoSProfile + + +class SimGameState(Node): + DEFAULT_PLAYERS_PER_TEAM = 4 + STATE_KEYS = { + "0": ("STATE_INITIAL", GameState.STATE_INITIAL), + "1": ("STATE_READY", GameState.STATE_READY), + "2": ("STATE_SET", GameState.STATE_SET), + "3": ("STATE_PLAYING", GameState.STATE_PLAYING), + "4": ("STATE_FINISHED", GameState.STATE_FINISHED), + } + COMPETITION_TYPE_KEYS = { + "5": ("COMPETITION_TYPE_SMALL", GameState.COMPETITION_TYPE_SMALL), + "6": ("COMPETITION_TYPE_MIDDLE", GameState.COMPETITION_TYPE_MIDDLE), + "7": ("COMPETITION_TYPE_LARGE", GameState.COMPETITION_TYPE_LARGE), + } + GAME_PHASE_KEYS = { + "a": ("GAME_PHASE_NORMAL", GameState.GAME_PHASE_NORMAL), + "b": ("GAME_PHASE_PENALTY_SHOOT_OUT", GameState.GAME_PHASE_PENALTY_SHOOT_OUT), + "c": ("GAME_PHASE_EXTRA_TIME", GameState.GAME_PHASE_EXTRA_TIME), + "d": ("GAME_PHASE_TIMEOUT", GameState.GAME_PHASE_TIMEOUT), + } + SET_PLAY_KEYS = { + "e": ("SET_PLAY_NONE", GameState.SET_PLAY_NONE), + "f": ("SET_PLAY_DIRECT_FREE_KICK", GameState.SET_PLAY_DIRECT_FREE_KICK), + "g": ("SET_PLAY_INDIRECT_FREE_KICK", GameState.SET_PLAY_INDIRECT_FREE_KICK), + "h": ("SET_PLAY_PENALTY_KICK", GameState.SET_PLAY_PENALTY_KICK), + "i": ("SET_PLAY_THROW_IN", GameState.SET_PLAY_THROW_IN), + "j": ("SET_PLAY_GOAL_KICK", GameState.SET_PLAY_GOAL_KICK), + "k": ("SET_PLAY_CORNER_KICK", GameState.SET_PLAY_CORNER_KICK), + } -from game_controller_hsl_interfaces.msg import GameState -from game_controller_hsl.utils import get_parameters_from_other_node + def __init__(self): + super().__init__("sim_gamestate") + self.logger = self.get_logger() + params = self.load_parameters() + self.team_id = int(params["team_id"]) + self.player_number = int(params["bot_id"]) + self.has_kick = True -class SimGamestate(Node): - msg = """Setting the GameState by entering a number: + self.settings = termios.tcgetattr(sys.stdin) -0: STATE_INITIAL = 0 -1: STATE_READY = 1 -2: STATE_SET = 2 -3: STATE_PLAYING =3 -4: STATE_FINISHED = 4 + self.publisher = self.create_publisher( + GameState, + "gamestate", + QoSProfile(durability=DurabilityPolicy.TRANSIENT_LOCAL, depth=1), + ) -5: COMPETITION_TYPE_SMALL = 0 -6: COMPETITION_TYPE_MIDDLE = 1 -7: COMPETITION_TYPE_LARGE = 3 + def load_parameters(self) -> dict[str, int | str]: + """ + Tries fetching team_id and bot_id from parameter blackboard. + If the blackboard is unavailable, fall back to manual/default values. + """ + try: + self.logger.info("Trying to fetch team_id and bot_id from parameter blackboard") + params = get_parameters_from_other_node( + self, + "parameter_blackboard", + ["team_id", "bot_id"], + service_timeout_sec=5.0, + ) + except TimeoutError: + self.logger.warn("Parameter blackboard not available") + params = {} + + team_id = params.get("team_id") + bot_id = params.get("bot_id") + + if team_id is None: + self.logger.warn("No team_id found in parameter blackboard") + params["team_id"]= int(input("Please enter team id: ")) + + if bot_id is None: + self.logger.warn("No bot_id found in parameter blackboard, assuming player 1") + params["bot_id"] = 1 + + return params + + def create_game_state_msg(self): + game_state_msg = GameState() + game_state_msg.header.stamp = self.get_clock().now().to_msg() + game_state_msg.players_per_team = self.DEFAULT_PLAYERS_PER_TEAM + game_state_msg.kicking_team = self.team_id + + self.sync_team_mates(game_state_msg) + return game_state_msg + + def apply_key(self, game_state_msg, key): + if key == "\x03": + return False + elif key in self.STATE_KEYS: + game_state_msg.main_state = self.STATE_KEYS[key][1] + elif key in self.COMPETITION_TYPE_KEYS: + game_state_msg.competition_type = self.COMPETITION_TYPE_KEYS[key][1] + elif key in self.GAME_PHASE_KEYS: + game_state_msg.game_phase = self.GAME_PHASE_KEYS[key][1] + elif key in self.SET_PLAY_KEYS: + game_state_msg.set_play = self.SET_PLAY_KEYS[key][1] + elif key == "p": + game_state_msg.penalized = not game_state_msg.penalized + + if game_state_msg.penalized: + # Use any generic penalty type, which is not in place as they + # all lead to the same timeout penalty and are likely handled + # the same way + game_state_msg.penalty = GameState.PENALTY_ILLEGAL_POSITIONING + else: + # If penalized is toggled off, we also cannot be penalized in place anymore + game_state_msg.penalized_in_place = False + elif key == "l": + game_state_msg.penalized_in_place = not game_state_msg.penalized_in_place + game_state_msg.penalized = game_state_msg.penalized_in_place + game_state_msg.penalty = ( + GameState.PENALTY_MOTION_IN_SET if game_state_msg.penalized_in_place else GameState.PENALTY_NONE + ) + elif key == "y": + game_state_msg.has_yellow_card = not game_state_msg.has_yellow_card + if game_state_msg.has_yellow_card: + game_state_msg.has_red_card = False + game_state_msg.cautions = 1 + else: + game_state_msg.cautions = 0 + elif key == "r": + game_state_msg.has_red_card = not game_state_msg.has_red_card + if game_state_msg.has_red_card: + game_state_msg.has_yellow_card = False + game_state_msg.cautions = 2 + game_state_msg.penalized = True + game_state_msg.penalized_in_place = False + game_state_msg.penalty = GameState.PENALTY_SENT_OFF + else: + game_state_msg.cautions = 0 + game_state_msg.penalized = False + game_state_msg.penalty = GameState.PENALTY_NONE + elif key == "t": + if game_state_msg.kicking_team == self.team_id: + game_state_msg.kicking_team = self.team_id + 1 + else: + game_state_msg.kicking_team = self.team_id + elif key == "s": + game_state_msg.stopped = not game_state_msg.stopped + elif key == "+": + game_state_msg.own_score += 1 + game_state_msg.main_state = GameState.STATE_READY + elif key == "-": + game_state_msg.rival_score += 1 + game_state_msg.main_state = GameState.STATE_READY + + self.sync_team_mates(game_state_msg) + return True + + def sync_team_mates(self, game_state_msg): + player_count = max(game_state_msg.players_per_team, self.player_number) + player_index = self.player_number - 1 + + game_state_msg.team_mates_with_penalty = [False] * player_count + game_state_msg.team_mates_with_yellow_card = [False] * player_count + game_state_msg.team_mates_with_red_card = [False] * player_count + + game_state_msg.team_mates_with_penalty[player_index] = game_state_msg.penalized + game_state_msg.team_mates_with_yellow_card[player_index] = game_state_msg.has_yellow_card + + def key_mapping_info(self ) -> str: + def build_key_mapping_string(key_mapping: dict[str, tuple[str, int]]) -> str: + bindings = [] + for key, mapping in key_mapping.items(): + bindings.append(f"{key}: {mapping[0]} = {mapping[1]}") + + return "\n".join(bindings) + + return f""" +{build_key_mapping_string(self.STATE_KEYS)} + +{build_key_mapping_string(self.COMPETITION_TYPE_KEYS)} Set the game phase by entering: -a: GAME_PHASE_TIMEOUT = 0 -b: GAME_PHASE_NORMAL = 1 -c: GAME_PHASE_EXTRA_TIME = 2 -d: GAME_PHASE_PENALTY_SHOOT_OUT = 3 +{build_key_mapping_string(self.GAME_PHASE_KEYS)} Set play states by entering: -e: SET_PLAY_NONE = 0 -f: SET_PLAY_DIRECT_FREE_KICK = 1 -g: SET_PLAY_INDIRECT_FREE_KICK = 2 -h: SET_PLAY_PENALTY_KICK = 3 -i: SET_PLAY_THROW_IN = 4 -j: SET_PLAY_GOAL_KICK = 5 -k: SET_PLAY_CORNER_KICK = 6 +{build_key_mapping_string(self.SET_PLAY_KEYS)} p: toggle penalized l: toggle in place penalty +y: toggle yellow card +r: toggle red card t: toggle kicking team s: toggle stopped state +: increase own score by 1 +-: increase rival score by 1 ----start dynamic--- -Competition Type: -Game Phase: -Set Play: -Main State: +Competition Type: 0, Game Phase: 0 +Main State: 0, Set Play: 0 Kicking Team: - Penalized: In Place Penalized: +Yellow Card: +Red Card: Stopped: Goals(Own : Rival): -CTRL-C to quit -""" +CTRL-C to quit""" - def __init__(self): - super().__init__("sim_gamestate") - self.logger = self.get_logger() + def game_state_info(self, game_state_msg: GameState): + return f""" +Competition Type: {game_state_msg.competition_type}, Game Phase: {game_state_msg.game_phase} +Main State: {game_state_msg.main_state}, Set Play: {game_state_msg.set_play} - # Try fetching team id from parameter blackboard or ask user for input - try: - self.team_id = get_parameters_from_other_node(self, "parameter_blackboard", ["team_id"])["team_id"] - except KeyError: - self.logger.error("No team id found in parameter blackboard") - self.team_id = int(input("Please enter team id: ")) - - self.has_kick = True +Kicking Team: {game_state_msg.kicking_team} +Penalized: {game_state_msg.penalized} +In Place Penalized: {game_state_msg.penalized_in_place} +Yellow Card: {game_state_msg.has_yellow_card} +Red Card: {game_state_msg.has_red_card} +Stopped: {game_state_msg.stopped} - self.settings = termios.tcgetattr(sys.stdin) +Goals(Own : Rival): {game_state_msg.own_score} : {game_state_msg.rival_score} - self.publisher = self.create_publisher( - GameState, - "gamestate", - QoSProfile(durability=DurabilityPolicy.TRANSIENT_LOCAL, depth=1), - ) +CTRL-C to quit""" def loop(self): - game_state_msg = GameState() - game_state_msg.header.stamp = self.get_clock().now().to_msg() - - # Init kicking team to our teamID - game_state_msg.kicking_team = self.team_id + game_state_msg = self.create_game_state_msg() try: - print(self.msg.replace("---start dynamic---", "")) + print(self.key_mapping_info()) + while True: key = self.get_key() - if key == "\x03": + if not self.apply_key(game_state_msg, key): break - elif key in ["0", "1", "2", "3", "4"]: - int_key = int(key) - game_state_msg.main_state = int_key - elif key in ["5", "6", "7"]: - int_key = int(key) - game_state_msg.competition_type = int_key - 5 - elif key == "p": # penalize / unpenalize - game_state_msg.penalized = not game_state_msg.penalized - elif key == "l": # toggle in place penalty - game_state_msg.penalized_in_place = not game_state_msg.penalized_in_place - game_state_msg.penalized = not game_state_msg.penalized - elif key in [chr(ord("a") + x) for x in range(4)]: - game_state_msg.game_phase = ord(key) - ord("a") - elif key in [chr(ord("e") + x) for x in range(7)]: - game_state_msg.set_play = ord(key) - ord("e") - elif key == "t": - if game_state_msg.kicking_team == self.team_id: - game_state_msg.kicking_team = self.team_id + 1 - else: - game_state_msg.kicking_team = self.team_id - elif key == "s": - game_state_msg.stopped = not game_state_msg.stopped - elif key == "+": - game_state_msg.own_score += 1 - self.publisher.publish(game_state_msg) - # Override the lines: Each \n in the message + the extra \n from print - line_count = self.msg.count("\n", self.msg.index("---start dynamic---")) + 1 + game_state_str = self.game_state_info(game_state_msg) + # Override the lines: Each \n in the str + the extra \n from print + line_count = game_state_str.count("\n") + 1 sys.stdout.write("\x1b[A\x1b[K" * line_count) - print( - f""" -Competition Type: {game_state_msg.competition_type} -Game Phase: {game_state_msg.game_phase} -Set Play: {game_state_msg.set_play} -Main State: {game_state_msg.main_state} - -Kicking Team: {game_state_msg.kicking_team} - -Penalized: {game_state_msg.penalized} -In Place Penalized: {game_state_msg.penalized_in_place} -Stopped: {game_state_msg.stopped} - -Goals(Own : Rival): {game_state_msg.own_score} : {game_state_msg.rival_score} - -CTRL-C to quit -""" - ) + print(game_state_str) + self.publisher.publish(game_state_msg) except Exception as e: print(e) - finally: - print() - termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings) def get_key(self): @@ -169,7 +269,7 @@ def get_key(self): if __name__ == "__main__": rclpy.init(args=None) - node = SimGamestate() + node = SimGameState() node.loop() node.destroy_node() rclpy.shutdown() diff --git a/src/lib/game_controller_hsl/game_controller_hsl/test/__init__.py b/src/lib/game_controller_hsl/game_controller_hsl/test/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/lib/game_controller_hsl/game_controller_hsl/test/test_receiver.py b/src/lib/game_controller_hsl/game_controller_hsl/test/test_receiver.py new file mode 100644 index 000000000..7b63afd60 --- /dev/null +++ b/src/lib/game_controller_hsl/game_controller_hsl/test/test_receiver.py @@ -0,0 +1,161 @@ +import math +from unittest.mock import Mock + +import pytest +from builtin_interfaces.msg import Time as TimeMsg +from game_controller_hsl.data import GameControlDataStruct, GameControlReturnDataStruct +from game_controller_hsl.receiver import GameStateReceiver +from game_controller_hsl_interfaces.msg import GameState, PlayerStateResponse +from geometry_msgs.msg import Point, PointStamped, Pose, PoseStamped, Quaternion +from rclpy.clock_type import ClockType +from rclpy.time import Time +from std_msgs.msg import Header + +def test_build_game_control_return_data_converts_ros_msg_to_protocol(receiver) -> None: + receiver._clock.now.return_value = Time(seconds=12, clock_type=ClockType.ROS_TIME) + + player_pose_yaw = 0.5 + response = player_state_response(player_pose_yaw) + + packet = GameControlReturnDataStruct.parse(receiver.build_game_control_return_data(response)) + + assert packet.player_number == 2 + assert packet.team_number == 6 + assert packet.fallen is True + assert packet.pose == pytest.approx([1000.0, -2000.0, player_pose_yaw]) + assert packet.ball_age == pytest.approx(2.0) + assert packet.ball == pytest.approx([300.0, -400.0]) + + +def test_build_game_control_return_data_marks_unseen_ball(receiver) -> None: + packet = GameControlReturnDataStruct.parse( + receiver.build_game_control_return_data(PlayerStateResponse()) + ) + + assert packet.ball_age == -1.0 + assert packet.pose == pytest.approx([0.0, 0.0, 0.0]) + assert packet.ball == pytest.approx([0.0, 0.0]) + assert packet.fallen is False + +def test_build_game_state_msg_converts_protocol_to_ros_msg(receiver) -> None: + packet = GameControlDataStruct.parse(game_control_data()) + + msg = receiver.build_game_state_msg(packet) + + assert msg.players_per_team == 4 + assert msg.competition_type == 0 + assert msg.game_phase == GameState.GAME_PHASE_NORMAL + assert msg.main_state == GameState.STATE_PLAYING + assert msg.set_play == GameState.SET_PLAY_NONE + assert msg.kicking_team == 1 + assert msg.first_half is True + assert msg.stopped is False + assert msg.own_score == 1 + assert msg.rival_score == 0 + assert msg.seconds_remaining == 300 + assert msg.secondary_time == 0 + assert msg.penalized is True + assert msg.penalized_in_place is True + assert msg.seconds_till_unpenalized == 16 + assert msg.cautions == 1 + assert msg.has_yellow_card is True + assert msg.has_red_card is False + assert msg.own_player_color == GameState.TEAM_RED + assert msg.own_goalie_color == GameState.TEAM_BLACK + assert msg.rival_player_color == GameState.TEAM_BLUE + assert msg.rival_goalie_color == GameState.TEAM_WHITE + assert msg.penalty_shot == 0 + assert msg.single_shots == 0b0000000000000000 + assert msg.message_budget == 10 + assert msg.team_mates_with_penalty == [True, True] + [False] * 18 + assert msg.team_mates_with_yellow_card == [False, True] + [False] * 18 + assert msg.team_mates_with_red_card == [True, False] + [False] * 18 + +def game_control_data() -> bytes: + return GameControlDataStruct.build( + dict( + packet_number=1, + players_per_team=4, + competition_type=0, # COMPETITION_TYPE_SMALL + stopped=False, + game_phase=0, # GAME_PHASE_NORMAL + state=3, # STATE_PLAYING + set_play=0, # SET_PLAY_NONE + first_half=True, + kicking_team=1, + secs_remaining=300, + secondary_time=0, + teams=[ + dict( + team_number=6, + field_player_color=1, # "TEAM_COLOR_RED + goalkeeper_color=3, # "TEAM_COLOR_BLACK + goalkeeper=1, + score=1, + penalty_shot=0, + single_shots=0b0000000000000000, + message_budget=10, + players=[ + dict( + penalty=12, # PENALTY_SENT_OFF + secs_till_unpenalized=0, + cautions=2, + ), + dict( + penalty=2, # PENALTY_MOTION_IN_SET + secs_till_unpenalized=16, + cautions=1, + ), + ] + [ + dict( + penalty=0, # PENALTY_NONE + secs_till_unpenalized=0, + cautions=0, + ) for _ in range(18) + ] + ), + dict( + team_number=7, + field_player_color=0, # TEAM_COLOR_BLUE + goalkeeper_color=4, # TEAM_COLOR_WHITE + goalkeeper=1, + score=0, + penalty_shot=0, + single_shots=0b0000000000000000, + message_budget=10, + players=[ + dict( + penalty=0, # PENALTY_NONE + secs_till_unpenalized=0, + cautions=0, + ) for _ in range(20) + ], + ), + ], + ) + ) + +def player_state_response(player_pose_yaw) -> PlayerStateResponse: + return PlayerStateResponse( + fallen=True, + pose=PoseStamped( + pose=Pose( + position=Point(x=1.0, y=-2.0, z=0.0), + orientation=Quaternion(z=math.sin(player_pose_yaw / 2.0), w=math.cos(player_pose_yaw / 2.0)), + ) + ), + ball=PointStamped( + header=Header(stamp=TimeMsg(sec=10)), + point=Point(x=0.3, y=-0.4), + ), + ) + + +@pytest.fixture +def receiver() -> GameStateReceiver: + receiver = object.__new__(GameStateReceiver) + receiver.player_number = 2 + receiver.team_number = 6 + receiver._clock = Mock() + + return receiver diff --git a/src/lib/game_controller_hsl/game_controller_hsl_interfaces/CMakeLists.txt b/src/lib/game_controller_hsl/game_controller_hsl_interfaces/CMakeLists.txt index 0f527ede7..100166c4a 100644 --- a/src/lib/game_controller_hsl/game_controller_hsl_interfaces/CMakeLists.txt +++ b/src/lib/game_controller_hsl/game_controller_hsl_interfaces/CMakeLists.txt @@ -14,7 +14,7 @@ endif() rosidl_generate_interfaces(${PROJECT_NAME} "msg/GameState.msg" - "msg/PlayerStatusPose.msg" + "msg/PlayerStateResponse.msg" DEPENDENCIES std_msgs geometry_msgs diff --git a/src/lib/game_controller_hsl/game_controller_hsl_interfaces/msg/GameState.msg b/src/lib/game_controller_hsl/game_controller_hsl_interfaces/msg/GameState.msg index e6b9b43ae..8904b7839 100644 --- a/src/lib/game_controller_hsl/game_controller_hsl_interfaces/msg/GameState.msg +++ b/src/lib/game_controller_hsl/game_controller_hsl_interfaces/msg/GameState.msg @@ -1,10 +1,16 @@ -# This message provides all information from the game controller -# for additional information see documentation of the game controller -# https://github.com/bhuman/GameController - +# This message defines a GameState extracted from the RoboCupGameControlData packet received by the game controller. +# While the game controller provides info about both teams and all players this message represents the +# state for the currently running team/player combination. +# +# For the packet defition for RoboCupGameControlData see: +# https://github.com/RoboCup-HumanoidSoccerLeague/GameController/blob/master/game_controller_msgs/headers/RoboCupGameControlData.h +# +# For additional documentation of the underlying meaning of fields have a look at: +# https://github.com/RoboCup-HumanoidSoccerLeague/HSL-Rules std_msgs/Header header +##### Game State uint8 players_per_team uint8 COMPETITION_TYPE_SMALL = 0 @@ -34,21 +40,18 @@ uint8 SET_PLAY_GOAL_KICK = 5 uint8 SET_PLAY_CORNER_KICK = 6 uint8 set_play -uint8 kicking_team bool first_half bool stopped +uint8 kicking_team uint8 own_score uint8 rival_score # Seconds remaining for the game half -int16 secs_remaining -# Seconds remaining for things like kickoff +int16 seconds_remaining +# Seconds remaining for current game action (e.g. kickoff) int16 secondary_time -bool penalized -bool penalized_in_place -uint16 seconds_till_unpenalized -uint8 warnings -uint8 cautions + +##### TeamInfo uint8 TEAM_BLUE = 0 uint8 TEAM_RED = 1 @@ -70,5 +73,41 @@ uint8 penalty_shot # a binary pattern indicating the successful penalty shots (1 for successful, 0 for unsuccessful) uint16 single_shots -uint8 message_budget +# Number of remaining messages, that can be sent by the team +uint16 message_budget + +# Extra fields to make decisions based on number of active or cautioned players. +# The indexes represent the players in order. bool[] team_mates_with_penalty +bool[] team_mates_with_yellow_card +bool[] team_mates_with_red_card + +##### RobotInfo + +uint8 PENALTY_NONE = 0 +uint8 PENALTY_ILLEGAL_POSITIONING = 1 +uint8 PENALTY_MOTION_IN_SET = 2 +uint8 PENALTY_MOTION_IN_STOP = 3 +uint8 PENALTY_LOCAL_GAME_STUCK = 4 +uint8 PENALTY_INCAPABLE_ROBOT = 5 +uint8 PENALTY_PICK_UP = 6 +uint8 PENALTY_BALL_HOLDING = 7 +uint8 PENALTY_LEAVING_THE_FIELD = 8 +uint8 PENALTY_PLAYING_WITH_ARMS_HANDS = 9 +uint8 PENALTY_PUSHING = 10 +uint8 PENALTY_CAUTIONED = 11 +uint8 PENALTY_SENT_OFF = 12 +uint8 PENALTY_SUBSTITUTE = 13 +uint8 penalty + +bool penalized +# Extra fields to differentiate between penalties, which will have the robot +# removed from the game to the sideline and penalties that happen in place +# on the field (e.g. MOTION_IN_SET) +bool penalized_in_place +uint8 seconds_till_unpenalized + +# Number of yellow cards the player has received +uint8 cautions +bool has_yellow_card +bool has_red_card diff --git a/src/lib/game_controller_hsl/game_controller_hsl_interfaces/msg/PlayerStateResponse.msg b/src/lib/game_controller_hsl/game_controller_hsl_interfaces/msg/PlayerStateResponse.msg new file mode 100644 index 000000000..e914ea629 --- /dev/null +++ b/src/lib/game_controller_hsl/game_controller_hsl_interfaces/msg/PlayerStateResponse.msg @@ -0,0 +1,39 @@ +# This message defines a PlayerStateResponse to be converted and sent as a RoboCupGameControlReturnData packet to the game controller. +# It is used to verify a player is connected to game controller and for visualization purposes. +# +# For the packet defition for RoboCupGameControlReturnData see: +# https://github.com/RoboCup-HumanoidSoccerLeague/GameController/blob/master/game_controller_msgs/headers/RoboCupGameControlData.h + +# Time at which this response snapshot was assembled. frame_id is unused; +# pose and ball carry their own coordinate frames. +std_msgs/Header header + +bool fallen + +# This pose is based on the following field coordinate system: +# +# y +# ^ ______________________ +# | M | | | O +# | Y |_ -x, y | x, y _| P +# | G | | | | | P +# 0 + O | | ( ) | | G +# | A |_| | |_| O +# | L | -x,-y | x,-y | A +# | |__________|__________| L +# | +# +------------------+--------------> x +# 0 +# +# 0,0 is the center of the field. pose.header.stamp is the time of the pose +# estimate and pose.header.frame_id identifies the field coordinate frame. +# Position is expressed in metres. The orientation represents yaw about the +# +z axis; it is converted to an angle in radians for the return packet, with +# yaw 0 along the +x axis and increasing counter-clockwise. +geometry_msgs/PoseStamped pose + +# Ball position in metres, relative to the robot. ball.header.stamp is the time +# the ball was last observed and ball.header.frame_id identifies the applicable +# robot-relative coordinate frame, whose +x axis points forward and +y axis +# points left. +geometry_msgs/PointStamped ball diff --git a/src/lib/game_controller_hsl/game_controller_hsl_interfaces/msg/PlayerStatusPose.msg b/src/lib/game_controller_hsl/game_controller_hsl_interfaces/msg/PlayerStatusPose.msg deleted file mode 100644 index f2f6f218a..000000000 --- a/src/lib/game_controller_hsl/game_controller_hsl_interfaces/msg/PlayerStatusPose.msg +++ /dev/null @@ -1,22 +0,0 @@ -# This Pose message is based on the following field coordinate system: -# -# y -# ^ ______________________ -# | M | | | O -# | Y |_ -x, y | x, y _| P -# | G | | | | | P -# 0 + O | | ( ) | | G -# | A |_| | |_| O -# | L | -x,-y | x,-y | A -# | |__________|__________| L -# | -# +------------------+--------------> x -# 0 -# -# 0,0 is the center of the filed and we expect (x, y, theta) with theta -# being the euler angle in radians, 0 along the +x axis, -# increasing counter clockwise - -std_msgs/Header header - -float32[3] pose diff --git a/src/lib/game_controller_hsl/game_controller_hsl_interfaces/package.xml b/src/lib/game_controller_hsl/game_controller_hsl_interfaces/package.xml index 07ee19e59..f0d673f17 100644 --- a/src/lib/game_controller_hsl/game_controller_hsl_interfaces/package.xml +++ b/src/lib/game_controller_hsl/game_controller_hsl_interfaces/package.xml @@ -5,7 +5,6 @@ 0.0.1 RoboCup Humanoid League Game Controller Messages - Florian Vahl Hamburg Bit-Bots Apache License 2.0 @@ -15,8 +14,9 @@ ament_cmake rosidl_default_generators - std_msgs ament_lint_auto + geometry_msgs + std_msgs rosidl_default_generators rosidl_default_runtime