diff --git a/humanoid_league_team_communication/config/team_communication_config.yaml b/humanoid_league_team_communication/config/team_communication_config.yaml index 9c821df..ecb717d 100644 --- a/humanoid_league_team_communication/config/team_communication_config.yaml +++ b/humanoid_league_team_communication/config/team_communication_config.yaml @@ -1,11 +1,13 @@ team_comm: ros__parameters: - # Only used if local_mode is false - target_host: 127.0.0.1 + # Sets local mode if set to loopback (127.0.0.1) + target_ip: 127.0.0.1 + + # Only used in non local mode with specific target_ip target_port: 3737 receive_port: 3737 - # Only used when target_host is 127.0.0.1 (local mode) + # Only used in local mode on loopback # the team communication will bind to one of these ports and send to the other ports, depending on its bot_id local_target_ports: - 4001 @@ -30,7 +32,7 @@ team_comm: pose_topic: "pose_with_covariance" ball_topic: "ball_position_relative_filtered" ball_velocity_topic: "ball_relative_movement" - obstacle_topic: "obstacles_relative" + robots_topic: "robots_relative" cmd_vel_topic: "cmd_vel" gamestate_topic: "gamestate" move_base_goal_topic: "move_base/current_goal" diff --git a/humanoid_league_team_communication/humanoid_league_team_communication/communication.py b/humanoid_league_team_communication/humanoid_league_team_communication/communication.py new file mode 100644 index 0000000..8b58b54 --- /dev/null +++ b/humanoid_league_team_communication/humanoid_league_team_communication/communication.py @@ -0,0 +1,73 @@ +from ipaddress import IPv4Address +import socket +from typing import List, Optional + +from rclpy.node import Node + + +class SocketCommunication: + + def __init__(self, node: Node, logger, team_id, robot_id): + self.logger = logger + + self.buffer_size: int = 1024 + self.socket: Optional[socket.socket] = None + self.target_ip: IPv4Address = IPv4Address(node.get_parameter('target_ip').value) + + if self.target_ip.is_loopback: + # local mode on loopback device, bind to port depending on bot id and team id + local_target_ports: List[int] = node.get_parameter('local_target_ports').value + self.target_ports = [port + 10 * team_id for port in local_target_ports] + self.receive_port = self.target_ports[robot_id - 1] + else: + target_port: int = node.get_parameter('target_port').value + receive_port: int = node.get_parameter('receive_port').value + self.target_ports = [target_port] + self.receive_port = receive_port + + def __del__(self): + self.close_connection() + + def is_setup(self) -> bool: + return self.socket is not None + + def establish_connection(self): + if not self.is_setup(): + self.socket = self.get_connection() + + def get_connection(self) -> socket.socket: + self.logger.info(f"Binding to port {self.receive_port} to receive messages") + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) + sock.bind(('0.0.0.0', self.receive_port)) + return sock + + def close_connection(self): + if self.is_setup(): + self.socket.close() + self.logger.info("Connection closed.") + + def receive_message(self) -> Optional[bytes]: + self.assert_is_setup() + msg, _, flags, _ = self.socket.recvmsg(self.buffer_size) + is_message_trucated = flags & socket.MSG_TRUNC + if is_message_trucated: + self.handle_truncated_message() + return None + else: + return msg + + def handle_truncated_message(self): + self.logger.warn( + f"recvmsg flag {socket.MSG_TRUNC} signaling a packet larger than buffer size {self.buffer_size}") + self.buffer_size *= 2 + self.logger.info(f"doubled buffer size to {self.buffer_size}") + + def send_message(self, message): + self.assert_is_setup() + for port in self.target_ports: + self.logger.debug(f'Sending message to {port} on {self.target_ip}') + self.socket.sendto(message, (str(self.target_ip), port)) + + def assert_is_setup(self): + assert self.is_setup(), "Socket is not yet initialized" diff --git a/humanoid_league_team_communication/humanoid_league_team_communication/converter/__init__.py b/humanoid_league_team_communication/humanoid_league_team_communication/converter/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/humanoid_league_team_communication/humanoid_league_team_communication/converter/message_to_team_data_converter.py b/humanoid_league_team_communication/humanoid_league_team_communication/converter/message_to_team_data_converter.py new file mode 100644 index 0000000..16d0370 --- /dev/null +++ b/humanoid_league_team_communication/humanoid_league_team_communication/converter/message_to_team_data_converter.py @@ -0,0 +1,103 @@ +from typing import List, Tuple + +import transforms3d +from geometry_msgs.msg import PoseWithCovariance +from numpy import double + +import humanoid_league_team_communication.robocup_extension_pb2 as Proto +from humanoid_league_msgs.msg import ObstacleRelative, ObstacleRelativeArray, TeamData + + +class MessageToTeamDataConverter: + + def __init__(self, team_mapping, role_mapping, action_mapping, side_mapping): + self.team_mapping = team_mapping + self.role_mapping = role_mapping + self.action_mapping = action_mapping + self.side_mapping = side_mapping + + def convert(self, message: Proto.Message, team_data: TeamData) -> TeamData: + team_data.robot_id = message.current_pose.player_id + team_data.state = message.state + + team_data.robot_position = self.convert_robot_pose(message.current_pose) + team_data.ball_absolute = self.convert_ball_pose(message.ball) + + # @TODO: change TeamData field/type to robots + # see: https://github.com/bit-bots/humanoid_league_misc/issues/125 + team_data.obstacles = self.convert_robots_to_obstacles(message.others, message.other_robot_confidence) + team_data.obstacles.header = team_data.header + + return self.convert_optional_fields(message, team_data) + + def convert_optional_fields(self, message: Proto.Message, team_data: TeamData) -> TeamData: + if hasattr(message, "time_to_ball"): + team_data.time_to_position_at_ball = message.time_to_ball + + return self.convert_strategy(message, team_data) + + def convert_strategy(self, message: Proto.Message, team_data: TeamData) -> TeamData: + if hasattr(message, "role"): + team_data.strategy.role = self.role_mapping[message.role] + if hasattr(message, "action"): + team_data.strategy.action = self.action_mapping[message.action] + if hasattr(message, "offensive_side"): + team_data.strategy.offensive_side = self.side_mapping[message.offensive_side] + + return team_data + + def convert_robots_to_obstacles(self, message_robots: List[Proto.Robot], + message_robot_confidence: List[float]) -> ObstacleRelativeArray: + relative_obstacles = ObstacleRelativeArray() + for index, robot in enumerate(message_robots): + obstacle = ObstacleRelative(player_number=robot.player_id, type=self.team_mapping[robot.team]) + obstacle.pose.pose = self.convert_robot_pose(robot) + + if index < len(message_robot_confidence): + obstacle.pose.confidence = message_robot_confidence[index] + + relative_obstacles.obstacles.append(obstacle) + + return relative_obstacles + + def convert_ball_pose(self, message_ball_pose: Proto.Ball) -> PoseWithCovariance: + ball = PoseWithCovariance() + ball.pose.position.x = message_ball_pose.position.x + ball.pose.position.y = message_ball_pose.position.y + ball.pose.position.z = message_ball_pose.position.z + + if message_ball_pose.covariance: + self.convert_to_row_major_covariance(ball.covariance, message_ball_pose.covariance) + + return ball + + def convert_robot_pose(self, message_robot_pose: Proto.Robot) -> PoseWithCovariance: + robot = PoseWithCovariance() + robot.pose.position.x = message_robot_pose.position.x + robot.pose.position.y = message_robot_pose.position.y + + quaternion = self.convert_to_quat((0, 0, message_robot_pose.position.z)) + robot.pose.orientation.w = quaternion[0] + robot.pose.orientation.x = quaternion[1] + robot.pose.orientation.y = quaternion[2] + robot.pose.orientation.z = quaternion[3] + + self.convert_to_row_major_covariance(robot.covariance, message_robot_pose.covariance) + + return robot + + def convert_to_quat(self, euler_angles: Tuple[float, float, float]): + return transforms3d.euler.euler2quat(*euler_angles) + + def convert_to_row_major_covariance(self, row_major_covariance: List[double], covariance_matrix: Proto.fmat3): + # ROS covariance is row-major 36 x float, while protobuf covariance + # is column-major 9 x float [x, y, θ] + row_major_covariance[0] = covariance_matrix.x.x + row_major_covariance[1] = covariance_matrix.y.x + row_major_covariance[5] = covariance_matrix.z.x + row_major_covariance[6] = covariance_matrix.x.y + row_major_covariance[7] = covariance_matrix.y.y + row_major_covariance[11] = covariance_matrix.z.y + row_major_covariance[30] = covariance_matrix.x.z + row_major_covariance[31] = covariance_matrix.y.z + row_major_covariance[35] = covariance_matrix.z.z diff --git a/humanoid_league_team_communication/humanoid_league_team_communication/converter/robocup_protocol_converter.py b/humanoid_league_team_communication/humanoid_league_team_communication/converter/robocup_protocol_converter.py new file mode 100644 index 0000000..7af091f --- /dev/null +++ b/humanoid_league_team_communication/humanoid_league_team_communication/converter/robocup_protocol_converter.py @@ -0,0 +1,73 @@ +from enum import IntEnum + +from humanoid_league_team_communication.converter.message_to_team_data_converter import MessageToTeamDataConverter +from humanoid_league_team_communication.converter.state_to_message_converter import StateToMessageConverter + +from humanoid_league_msgs.msg import ObstacleRelative, Strategy +from soccer_vision_attribute_msgs.msg import Robot as RobotAttributes + +import humanoid_league_team_communication.robocup_extension_pb2 as Proto + + +class TeamColor(IntEnum): + BLUE = 0 + RED = 1 + + +class RobocupProtocolConverter: + + def __init__(self, own_team_color: TeamColor): + self.role_mapping = ( + (Proto.Role.ROLE_UNDEFINED, Strategy.ROLE_UNDEFINED), + (Proto.Role.ROLE_IDLING, Strategy.ROLE_IDLING), + (Proto.Role.ROLE_OTHER, Strategy.ROLE_OTHER), + (Proto.Role.ROLE_STRIKER, Strategy.ROLE_STRIKER), + (Proto.Role.ROLE_SUPPORTER, Strategy.ROLE_SUPPORTER), + (Proto.Role.ROLE_DEFENDER, Strategy.ROLE_DEFENDER), + (Proto.Role.ROLE_GOALIE, Strategy.ROLE_GOALIE), + ) + self.action_mapping = ( + (Proto.Action.ACTION_UNDEFINED, Strategy.ACTION_UNDEFINED), + (Proto.Action.ACTION_POSITIONING, Strategy.ACTION_POSITIONING), + (Proto.Action.ACTION_GOING_TO_BALL, Strategy.ACTION_GOING_TO_BALL), + (Proto.Action.ACTION_TRYING_TO_SCORE, Strategy.ACTION_TRYING_TO_SCORE), + (Proto.Action.ACTION_WAITING, Strategy.ACTION_WAITING), + (Proto.Action.ACTION_KICKING, Strategy.ACTION_KICKING), + (Proto.Action.ACTION_SEARCHING, Strategy.ACTION_SEARCHING), + (Proto.Action.ACTION_LOCALIZING, Strategy.ACTION_LOCALIZING), + ) + self.side_mapping = ( + (Proto.OffensiveSide.SIDE_UNDEFINED, Strategy.SIDE_UNDEFINED), + (Proto.OffensiveSide.SIDE_LEFT, Strategy.SIDE_LEFT), + (Proto.OffensiveSide.SIDE_MIDDLE, Strategy.SIDE_MIDDLE), + (Proto.OffensiveSide.SIDE_RIGHT, Strategy.SIDE_RIGHT), + ) + + self.proto_to_team_data_team_mapping = { + Proto.Team.UNKNOWN_TEAM: ObstacleRelative.ROBOT_UNDEFINED, + Proto.Team.BLUE: ObstacleRelative.ROBOT_CYAN, + Proto.Team.RED: ObstacleRelative.ROBOT_MAGENTA + } + self.state_to_proto_team_mapping = { + RobotAttributes.TEAM_OWN: Proto.Team.RED if own_team_color == TeamColor.RED else Proto.Team.BLUE, + RobotAttributes.TEAM_OPPONENT: Proto.Team.BLUE if own_team_color == TeamColor.RED else Proto.Team.RED, + RobotAttributes.TEAM_UNKNOWN: Proto.Team.UNKNOWN_TEAM, + } + + proto_to_team_data_mappings = { + "team_mapping": self.proto_to_team_data_team_mapping, + "role_mapping": dict(self.role_mapping), + "action_mapping": dict(self.action_mapping), + "side_mapping": dict(self.side_mapping), + } + + reverse_mapping = lambda mapping: dict((b, a) for a, b in mapping) + state_to_proto_mappings = { + "team_mapping": self.state_to_proto_team_mapping, + "role_mapping": reverse_mapping(self.role_mapping), + "action_mapping": reverse_mapping(self.action_mapping), + "side_mapping": reverse_mapping(self.side_mapping), + } + + self.convert_from_message = MessageToTeamDataConverter(**proto_to_team_data_mappings).convert + self.convert_to_message = StateToMessageConverter(**state_to_proto_mappings).convert diff --git a/humanoid_league_team_communication/humanoid_league_team_communication/converter/state_to_message_converter.py b/humanoid_league_team_communication/humanoid_league_team_communication/converter/state_to_message_converter.py new file mode 100644 index 0000000..cd4c4ea --- /dev/null +++ b/humanoid_league_team_communication/humanoid_league_team_communication/converter/state_to_message_converter.py @@ -0,0 +1,171 @@ +import math +import transforms3d +from numpy import double +from typing import Callable, List, Optional, Tuple + +from rclpy.time import Time +from geometry_msgs.msg import PointStamped, PoseStamped, PoseWithCovarianceStamped, Quaternion, Twist +from humanoid_league_msgs.msg import GameState, Strategy +from soccer_vision_3d_msgs.msg import Robot, RobotArray + +import humanoid_league_team_communication.robocup_extension_pb2 as Proto + + +class StateToMessageConverter: + + def __init__(self, team_mapping, role_mapping, action_mapping, side_mapping): + self.team_mapping = team_mapping + self.role_mapping = role_mapping + self.action_mapping = action_mapping + self.side_mapping = side_mapping + + def convert(self, state, message: Proto.Message, is_still_valid_checker: Callable[[Time], bool]) -> Proto.Message: + + def convert_gamestate(gamestate: Optional[GameState], message: Proto.Message): + if gamestate is not None and is_still_valid_checker(gamestate.header.stamp): + message.state = Proto.State.PENALISED if gamestate.penalized else Proto.State.UNPENALISED + else: + message.state = Proto.State.UNKNOWN_STATE + + return message + + def convert_current_pose(current_pose: Optional[PoseWithCovarianceStamped], message: Proto.Message): + if current_pose is not None and is_still_valid_checker(current_pose.header.stamp): + pose_with_covariance = current_pose.pose + pose = pose_with_covariance.pose + + message.current_pose.position.x = pose.position.x + message.current_pose.position.y = pose.position.y + message.current_pose.position.z = self.extract_orientation_yaw_angle(pose.orientation) + + self.convert_to_covariance_matrix(message.current_pose.covariance, pose_with_covariance.covariance) + else: + self.default_to_large_covariance(message.current_pose) + + return message + + def convert_walk_command(walk_command: Optional[Twist], walk_command_time: Time, message: Proto.Message): + if walk_command is not None and is_still_valid_checker(walk_command_time): + message.walk_command.x = walk_command.linear.x + message.walk_command.y = walk_command.linear.y + message.walk_command.z = walk_command.angular.z + + return message + + def convert_target_position(target_position: Optional[PoseStamped], message): + if target_position is not None and is_still_valid_checker(target_position.header.stamp): + pose = target_position.pose + message.target_pose.position.x = pose.position.x + message.target_pose.position.y = pose.position.y + message.target_pose.position.z = self.extract_orientation_yaw_angle(pose.orientation) + + return message + + def convert_ball_position(ball_position: Optional[PointStamped], ball_velocity: Tuple[float, float, float], + ball_covariance: List[double], message): + if ball_position is not None and is_still_valid_checker(ball_position.header.stamp): + message.ball.position.x = ball_position.point.x + message.ball.position.y = ball_position.point.y + message.ball.position.z = ball_position.point.z + + message.ball.velocity.x = ball_velocity[0] + message.ball.velocity.y = ball_velocity[1] + message.ball.velocity.z = ball_velocity[2] + + self.convert_to_covariance_matrix(message.ball.covariance, ball_covariance) + else: + self.default_to_large_covariance(message.ball) + + return message + + def convert_seen_robots(seen_robots: Optional[RobotArray], message: Proto.Message): + if seen_robots is not None and is_still_valid_checker(seen_robots.header.stamp): + seen_robot: Robot + for seen_robot in seen_robots.robots: + robot = Proto.Robot() + robot.player_id = seen_robot.attributes.player_number + + pose = seen_robot.bb.center + robot.team = self.team_mapping[seen_robot.attributes.team] + robot.position.x = pose.position.x + robot.position.y = pose.position.y + robot.position.z = self.extract_orientation_yaw_angle(pose.orientation) + + message.others.append(robot) + message.other_robot_confidence.append(seen_robot.confidence.confidence) + + return message + + def convert_strategy(strategy: Optional[Strategy], strategy_time: Time, message: Proto.Message): + if strategy is not None and is_still_valid_checker(strategy_time): + message.role = self.role_mapping[strategy.role] + message.action = self.action_mapping[strategy.action] + message.offensive_side = self.side_mapping[strategy.offensive_side] + + return message + + def are_robot_and_ball_position_valid(current_pose: Optional[PoseWithCovarianceStamped], + ball_position: Optional[PointStamped]) -> bool: + return (ball_position is not None and is_still_valid_checker(ball_position.header.stamp) and + current_pose is not None and is_still_valid_checker(current_pose.header.stamp)) + + def calculate_time_to_ball(current_pose: PoseWithCovarianceStamped, ball_position: PointStamped, + walking_speed: float) -> float: + pose = current_pose.pose.pose + ball_distance = math.sqrt((ball_position.point.x - pose.position.x)**2 + + (ball_position.point.y - pose.position.y)**2) + + return ball_distance / walking_speed + + def convert_time_to_ball(time_to_ball: Optional[float], time_to_ball_time: Time, ball_position: PointStamped, + current_pose: PoseWithCovarianceStamped, walking_speed: float, message: Proto.Message): + if time_to_ball is not None and is_still_valid_checker(time_to_ball_time): + message.time_to_ball = time_to_ball + elif are_robot_and_ball_position_valid(current_pose, ball_position): + message.time_to_ball = calculate_time_to_ball(current_pose, ball_position, walking_speed) + else: + message.time_to_ball = 9999.0 + + return message + + message.current_pose.player_id = state.player_id + message.current_pose.team = state.team_id + + message = convert_gamestate(state.gamestate, message) + message = convert_current_pose(state.pose, message) + message = convert_walk_command(state.cmd_vel, state.cmd_vel_time, message) + message = convert_target_position(state.move_base_goal, message) + message = convert_ball_position(state.ball, state.ball_velocity, state.ball_covariance, message) + message = convert_seen_robots(state.seen_robots, message) + message = convert_strategy(state.strategy, state.strategy_time, message) + message = convert_time_to_ball(state.time_to_ball, state.time_to_ball_time, state.ball, state.pose, + state.avg_walking_speed, message) + + return message + + def default_to_large_covariance(self, message_property, value=100): + # set high covariance to show that we have no clue + message_property.covariance.x.x = value + message_property.covariance.y.y = value + message_property.covariance.z.z = value + + def extract_orientation_yaw_angle(self, quaternion: Quaternion): + angles = self.convert_to_euler(quaternion) + theta = angles[2] + return theta + + def convert_to_euler(self, quaternion: Quaternion): + return transforms3d.euler.quat2euler([quaternion.w, quaternion.x, quaternion.y, quaternion.z]) + + def convert_to_covariance_matrix(self, covariance_matrix: Proto.fmat3, row_major_covariance: List[double]): + # ROS covariance is row-major 36 x float, while protobuf covariance + # is column-major 9 x float [x, y, θ] + covariance_matrix.x.x = row_major_covariance[0] + covariance_matrix.y.x = row_major_covariance[1] + covariance_matrix.z.x = row_major_covariance[5] + covariance_matrix.x.y = row_major_covariance[6] + covariance_matrix.y.y = row_major_covariance[7] + covariance_matrix.z.y = row_major_covariance[11] + covariance_matrix.x.z = row_major_covariance[30] + covariance_matrix.y.z = row_major_covariance[31] + covariance_matrix.z.z = row_major_covariance[35] diff --git a/humanoid_league_team_communication/humanoid_league_team_communication/humanoid_league_team_communication.py b/humanoid_league_team_communication/humanoid_league_team_communication/humanoid_league_team_communication.py index 3be366f..8f1a0a9 100755 --- a/humanoid_league_team_communication/humanoid_league_team_communication/humanoid_league_team_communication.py +++ b/humanoid_league_team_communication/humanoid_league_team_communication/humanoid_league_team_communication.py @@ -1,438 +1,237 @@ #!/usr/bin/env python3 -import math + import socket import struct import threading -from typing import Optional - +from numpy import double +from typing import List, Optional, Tuple import rclpy +from ament_index_python.packages import get_package_share_directory +from bitbots_utils.utils import get_parameter_dict, get_parameters_from_other_node +from geometry_msgs.msg import PoseWithCovarianceStamped, Twist, TwistWithCovarianceStamped +from humanoid_league_msgs.msg import GameState, Strategy, TeamData +from numpy import double from rclpy.duration import Duration from rclpy.node import Node - -import tf2_ros -import transforms3d from rclpy.time import Time -from std_msgs.msg import Header, Float32 -from geometry_msgs.msg import Twist, PoseWithCovarianceStamped, TwistWithCovarianceStamped -from humanoid_league_msgs.msg import GameState, TeamData, ObstacleRelativeArray, ObstacleRelative, Strategy +from soccer_vision_3d_msgs.msg import Robot, RobotArray +from std_msgs.msg import Float32, Header from tf2_geometry_msgs import PointStamped, PoseStamped -from bitbots_utils.utils import get_parameter_dict, get_parameters_from_other_node -from ament_index_python.packages import get_package_share_directory +from tf2_ros import Buffer, TransformListener, TransformException + +import humanoid_league_team_communication.robocup_extension_pb2 as Proto +from humanoid_league_team_communication.communication import SocketCommunication +from humanoid_league_team_communication.converter.robocup_protocol_converter import TeamColor, RobocupProtocolConverter -from humanoid_league_team_communication import robocup_extension_pb2 class HumanoidLeagueTeamCommunication: + def __init__(self): self._package_path = get_package_share_directory("humanoid_league_team_communication") - self.socket = None self.node = Node("team_comm", automatically_declare_parameters_from_overrides=True) self.logger = self.node.get_logger() self.logger.info("Initializing humanoid_league_team_communication...") + params_blackboard = get_parameters_from_other_node(self.node, "parameter_blackboard", + ["bot_id", "team_id", "team_color"]) + self.player_id = params_blackboard["bot_id"] + self.team_id = params_blackboard["team_id"] + self.team_color_id = params_blackboard["team_color"] + + self.protocol_converter = RobocupProtocolConverter(TeamColor(self.team_color_id)) + + self.logger.info(f"Starting for {self.player_id} in team {self.team_id}...") + self.socket_communication = SocketCommunication(self.node, self.logger, self.team_id, self.player_id) + + self.rate: int = self.node.get_parameter("rate").value + self.lifetime: int = self.node.get_parameter("lifetime").value + self.avg_walking_speed: float = self.node.get_parameter("avg_walking_speed").value - params_blackboard = get_parameters_from_other_node(self.node, "parameter_blackboard", ['bot_id', 'team_id']) - self.player_id = params_blackboard['bot_id'] - self.team_id = params_blackboard['team_id'] - - self.target_host = self.node.get_parameter('target_host').get_parameter_value().string_value - self.local_target_ports = self.node.get_parameter( - 'local_target_ports').get_parameter_value().integer_array_value - self.target_port = self.node.get_parameter('target_port').get_parameter_value().integer_value - self.receive_port = self.node.get_parameter('receive_port').get_parameter_value().integer_value - self.rate = self.node.get_parameter('rate').get_parameter_value().integer_value - self.lifetime = self.node.get_parameter('lifetime').get_parameter_value().integer_value - self.avg_walking_speed = self.node.get_parameter('avg_walking_speed').get_parameter_value().double_value - - self.topics = get_parameter_dict(self.node, 'topics') - self.map_frame = self.node.get_parameter('map_frame').get_parameter_value().string_value - - if self.target_host == '127.0.0.1': - # local mode, bind to port depending on bot id and team id - self.target_ports = [port + 10 * self.team_id for port in self.local_target_ports] - self.receive_port = self.target_ports[self.player_id - 1] - else: - self.target_ports = [self.target_port] - self.receive_port = self.receive_port + self.topics = get_parameter_dict(self.node, "topics") + self.map_frame: str = self.node.get_parameter("map_frame").value self.create_publishers() self.create_subscribers() - self.gamestate = None # type: GameState - self.pose = None # type: PoseWithCovarianceStamped - self.cmd_vel = None # type: Twist + self.set_state_defaults() + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self.node) + + self.try_to_establish_connection() + self.run_spin_in_thread() + + self.node.create_timer(1 / self.rate, self.send_message) + self.receive_forever() + + def run_spin_in_thread(self): + # Necessary in ROS2, else we are forever stuck receiving messages + thread = threading.Thread(target=rclpy.spin, args=[self.node], daemon=True) + thread.start() + + def set_state_defaults(self): + self.gamestate: Optional[GameState] = None + self.pose: Optional[PoseWithCovarianceStamped] = None + self.cmd_vel: Optional[Twist] = None self.cmd_vel_time = Time(nanoseconds=0, clock_type=self.node.get_clock().clock_type) - self.ball = None # type: Optional[PointStamped] - self.ball_vel = (0, 0, 0) - self.ball_covariance = None - self.strategy = None # type: Strategy + self.ball: Optional[PointStamped] = None + self.ball_velocity: Tuple[float, float, float] = (0, 0, 0) + self.ball_covariance: List[double] = [] + self.strategy: Optional[Strategy] = None self.strategy_time = Time(nanoseconds=0, clock_type=self.node.get_clock().clock_type) - self.time_to_ball = None + self.time_to_ball: Optional[float] = None self.time_to_ball_time = Time(nanoseconds=0, clock_type=self.node.get_clock().clock_type) - self.obstacles = None # type: ObstacleRelativeArray - self.move_base_goal = None # type: PoseStamped - - # Protobuf <-> ROS Message mappings - self.team_mapping = ((robocup_extension_pb2.Team.UNKNOWN_TEAM, ObstacleRelative.ROBOT_UNDEFINED), - (robocup_extension_pb2.Team.BLUE, ObstacleRelative.ROBOT_CYAN), - (robocup_extension_pb2.Team.RED, ObstacleRelative.ROBOT_MAGENTA)) - self.role_mapping = ( - (robocup_extension_pb2.Role.ROLE_UNDEFINED, Strategy.ROLE_UNDEFINED), - (robocup_extension_pb2.Role.ROLE_IDLING, Strategy.ROLE_IDLING), - (robocup_extension_pb2.Role.ROLE_OTHER, Strategy.ROLE_OTHER), - (robocup_extension_pb2.Role.ROLE_STRIKER, Strategy.ROLE_STRIKER), - (robocup_extension_pb2.Role.ROLE_SUPPORTER, Strategy.ROLE_SUPPORTER), - (robocup_extension_pb2.Role.ROLE_DEFENDER, Strategy.ROLE_DEFENDER), - (robocup_extension_pb2.Role.ROLE_GOALIE, Strategy.ROLE_GOALIE), - ) - self.action_mapping = ( - (robocup_extension_pb2.Action.ACTION_UNDEFINED, Strategy.ACTION_UNDEFINED), - (robocup_extension_pb2.Action.ACTION_POSITIONING, Strategy.ACTION_POSITIONING), - (robocup_extension_pb2.Action.ACTION_GOING_TO_BALL, Strategy.ACTION_GOING_TO_BALL), - (robocup_extension_pb2.Action.ACTION_TRYING_TO_SCORE, Strategy.ACTION_TRYING_TO_SCORE), - (robocup_extension_pb2.Action.ACTION_WAITING, Strategy.ACTION_WAITING), - (robocup_extension_pb2.Action.ACTION_KICKING, Strategy.ACTION_KICKING), - (robocup_extension_pb2.Action.ACTION_SEARCHING, Strategy.ACTION_SEARCHING), - (robocup_extension_pb2.Action.ACTION_LOCALIZING, Strategy.ACTION_LOCALIZING), - ) - self.side_mapping = ( - (robocup_extension_pb2.OffensiveSide.SIDE_UNDEFINED, Strategy.SIDE_UNDEFINED), - (robocup_extension_pb2.OffensiveSide.SIDE_LEFT, Strategy.SIDE_LEFT), - (robocup_extension_pb2.OffensiveSide.SIDE_MIDDLE, Strategy.SIDE_MIDDLE), - (robocup_extension_pb2.OffensiveSide.SIDE_RIGHT, Strategy.SIDE_RIGHT), - ) - - self.tf_buffer = tf2_ros.Buffer() - self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self.node) + self.seen_robots: Optional[RobotArray] = None + self.move_base_goal: Optional[PoseStamped] = None + def try_to_establish_connection(self): # we will try multiple times till we manage to get a connection - while rclpy.ok() and self.socket is None: - self.socket = self.get_connection() + while rclpy.ok() and not self.socket_communication.is_setup(): + self.socket_communication.establish_connection() self.node.get_clock().sleep_for(Duration(seconds=1)) rclpy.spin_once(self.node) - self.socket.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) - - # Necessary in ROS2, else we are forever stuck receiving messages - thread = threading.Thread(target=rclpy.spin, args=(self.node), daemon=True) - thread.start() - - self.node.create_timer(1 / self.rate, self.send_message) - self.receive_forever() def create_publishers(self): - self.pub_team_data = self.node.create_publisher(TeamData, self.topics['team_data_topic'], 1) + self.team_data_publisher = self.node.create_publisher(TeamData, self.topics["team_data_topic"], 1) def create_subscribers(self): - self.node.create_subscription(GameState, self.topics['gamestate_topic'], self.gamestate_cb, 1) - self.node.create_subscription(PoseWithCovarianceStamped, self.topics['pose_topic'], self.pose_cb, 1) - self.node.create_subscription(Twist, self.topics['cmd_vel_topic'], self.cmd_vel_cb, 1) - self.node.create_subscription(PoseWithCovarianceStamped, self.topics['ball_topic'], self.ball_cb, 1) - self.node.create_subscription(TwistWithCovarianceStamped, self.topics['ball_velocity_topic'], self.ball_vel_cb, - 1) - self.node.create_subscription(Strategy, self.topics['strategy_topic'], self.strategy_cb, 1) - self.node.create_subscription(Float32, self.topics['time_to_ball_topic'], self.time_to_ball_cb, 1) - self.node.create_subscription(ObstacleRelativeArray, self.topics['obstacle_topic'], self.obstacle_cb, 1) - self.node.create_subscription(PoseStamped, self.topics['move_base_goal_topic'], self.move_base_goal_cb, 1) - - def get_connection(self): - self.logger.info(f"Binding to port {self.receive_port}") - sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) - sock.bind(('0.0.0.0', self.receive_port)) - return sock - - def close_connection(self): - if self.socket: - self.socket.close() - self.logger.info("Connection closed.") - - def receive_msg(self): - return self.socket.recv(1024) - - def gamestate_cb(self, msg): + self.node.create_subscription(GameState, self.topics["gamestate_topic"], self.gamestate_cb, 1) + self.node.create_subscription(PoseWithCovarianceStamped, self.topics["pose_topic"], self.pose_cb, 1) + self.node.create_subscription(Twist, self.topics["cmd_vel_topic"], self.cmd_vel_cb, 1) + self.node.create_subscription(PoseWithCovarianceStamped, self.topics["ball_topic"], self.ball_cb, 1) + self.node.create_subscription( + TwistWithCovarianceStamped, + self.topics["ball_velocity_topic"], + self.ball_velocity_cb, + 1, + ) + self.node.create_subscription(Strategy, self.topics["strategy_topic"], self.strategy_cb, 1) + self.node.create_subscription(Float32, self.topics["time_to_ball_topic"], self.time_to_ball_cb, 1) + self.node.create_subscription(RobotArray, self.topics["robots_topic"], self.robots_cb, 1) + self.node.create_subscription(PoseStamped, self.topics["move_base_goal_topic"], self.move_base_goal_cb, 1) + + def gamestate_cb(self, msg: GameState): self.gamestate = msg - def pose_cb(self, msg): + def pose_cb(self, msg: PoseWithCovarianceStamped): self.pose = msg - def cmd_vel_cb(self, msg): + def cmd_vel_cb(self, msg: Twist): self.cmd_vel = msg - self.cmd_vel_time = self.node.get_clock().now() + self.cmd_vel_time = self.get_current_time().to_msg() - def strategy_cb(self, msg): + def strategy_cb(self, msg: Strategy): self.strategy = msg - self.strategy_time = self.node.get_clock().now() + self.strategy_time = self.get_current_time().to_msg() - def time_to_ball_cb(self, msg): + def time_to_ball_cb(self, msg: float): self.time_to_ball = msg.data - self.time_to_ball_time = self.node.get_clock().now() + self.time_to_ball_time = self.get_current_time().to_msg() - def move_base_goal_cb(self, msg): + def move_base_goal_cb(self, msg: PoseStamped): self.move_base_goal = msg - def obstacle_cb(self, msg): - self.obstacles = ObstacleRelativeArray(header=msg.header) - self.obstacles.header.frame_id = self.map_frame - for obstacle in msg.obstacles: - # Transform to map - obstacle_pose = PoseStamped(msg.header, obstacle.pose.pose.pose) + def robots_cb(self, msg: RobotArray): + + def transform_to_map(robot_relative: Robot): + # @TODO: check if this is not handled by the transform itself + robot_pose = PoseStamped(header=msg.header, pose=robot_relative.bb.center) try: - obstacle_map = self.tf_buffer.transform(obstacle_pose, - self.map_frame, - timeout=Duration(nanoseconds=0.3e9)) - obstacle.pose.pose.pose = obstacle_map.pose - self.obstacles.obstacles.append(obstacle) - except tf2_ros.TransformException: - self.logger.error("TeamComm: Could not transform obstacle to map frame") + robot_on_map = self.transform_to_map_frame(robot_pose) + robot_relative.bb.center = robot_on_map.pose + return robot_relative + except TransformException as err: + self.logger.error(f"Could not transform robot to map frame: {err}") + + robots_on_map = list(filter(None, map(transform_to_map, msg.robots))) + self.seen_robots = RobotArray(header=msg.header, robots=robots_on_map) + self.seen_robots.header.frame_id = self.map_frame def ball_cb(self, msg: PoseWithCovarianceStamped): - # Transform to map - ball_point = PointStamped(msg.header, msg.pose.pose.position) + ball_point = PointStamped(header=msg.header, point=msg.pose.pose.position) try: - self.ball = self.tf_buffer.transform(ball_point, self.map_frame, timeout=Duration(nanoseconds=0.3e9)) + self.ball = self.transform_to_map_frame(ball_point) self.ball_covariance = msg.pose.covariance - except tf2_ros.TransformException: - self.logger.error("TeamComm: Could not transform ball to map frame") - self.ball = None - - def ball_vel_cb(self, msg: TwistWithCovarianceStamped): - self.ball_vel = (msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.angular.z) + except TransformException as err: + self.logger.error(f"Could not transform ball to map frame: {err}") + + def ball_velocity_cb(self, msg: TwistWithCovarianceStamped): + self.ball_velocity = ( + msg.twist.twist.linear.x, + msg.twist.twist.linear.y, + msg.twist.twist.angular.z, + ) - def __del__(self): - self.close_connection() + def transform_to_map_frame(self, field, timeout_in_ns=0.3e9): + return self.tf_buffer.transform(field, self.map_frame, timeout=Duration(nanoseconds=timeout_in_ns)) def receive_forever(self): while rclpy.ok(): try: - msg = self.receive_msg() + message = self.socket_communication.receive_message() except (struct.error, socket.timeout): continue - if msg: # Not handle empty messages or None - self.handle_message(msg) - - def handle_message(self, msg): - - def covariance_proto_to_ros(fmat3, ros_covariance): - # ROS covariance is row-major 36 x float, protobuf covariance is column-major 9 x float [x, y, θ] - ros_covariance[0] = fmat3.x.x - ros_covariance[1] = fmat3.y.x - ros_covariance[5] = fmat3.z.x - ros_covariance[6] = fmat3.x.y - ros_covariance[7] = fmat3.y.y - ros_covariance[11] = fmat3.z.y - ros_covariance[30] = fmat3.x.z - ros_covariance[31] = fmat3.y.z - ros_covariance[35] = fmat3.z.z - - def pose_proto_to_ros(robot, pose): - pose.pose.position.x = robot.position.x - pose.pose.position.y = robot.position.y - - quat = transforms3d.euler.euler2quat(0.0, 0.0, robot.position.z) # wxyz -> ros: xyzw - pose.pose.orientation.x = quat[1] - pose.pose.orientation.y = quat[2] - pose.pose.orientation.z = quat[3] - pose.pose.orientation.w = quat[0] - - if pose.covariance: - covariance_proto_to_ros(robot.covariance, pose.covariance) - - message = robocup_extension_pb2.Message() - message.ParseFromString(msg) + if message: + self.handle_message(message) - player_id = message.current_pose.player_id - team_id = message.current_pose.team + def handle_message(self, string_message: bytes): + message = Proto.Message() + message.ParseFromString(string_message) - if player_id == self.player_id or team_id != self.team_id: - # Skip information from ourselves or from the other team + if self.should_message_be_discarded(message): + self.logger.debug(f"Discarding msg by player {message.current_pose.player_id} " + + f"in team {message.current_pose.team} at {message.timestamp.seconds}") return - team_data = TeamData() - - header = Header() - # The robots' times can differ, therefore use our own time here - header.stamp = self.node.get_clock().now() - header.frame_id = self.map_frame - - # Handle timestamp - ################## - team_data.header = header - - # Handle robot ID - ################# - team_data.robot_id = player_id - - # Handle state - ############## - team_data.state = message.state - - # Handle pose of current player - ############################### - pose_proto_to_ros(message.current_pose, team_data.robot_position) + team_data = self.protocol_converter.convert_from_message(message, self.create_team_data()) + self.team_data_publisher.publish(team_data) - # Handle ball - ############# - team_data.ball_absolute.pose.position.x = message.ball.position.x - team_data.ball_absolute.pose.position.y = message.ball.position.y - team_data.ball_absolute.pose.position.z = message.ball.position.z - - if message.ball.covariance: - covariance_proto_to_ros(message.ball.covariance, team_data.ball_absolute.covariance) + def send_message(self): + if not self.is_robot_allowed_to_send_message(): + self.logger.debug("Not allowed to send message") + return - # Handle obstacles - ################## - obstacle_relative_array = ObstacleRelativeArray() - obstacle_relative_array.header = header + now = self.get_current_time() + msg = self.create_empty_message(now) + is_still_valid = lambda time: now - Time.from_msg(time) < Duration(seconds=self.lifetime) + message = self.protocol_converter.convert_to_message(self, msg, is_still_valid) + self.socket_communication.send_message(message.SerializeToString()) - for index, robot in enumerate(message.others): - obstacle = ObstacleRelative() + def create_empty_message(self, now: Time) -> Proto.Message: + message = Proto.Message() + seconds, nanoseconds = now.seconds_nanoseconds() + message.timestamp.seconds = seconds + message.timestamp.nanos = nanoseconds + return message - # Obstacle type - team_to_obstacle_type = dict(self.team_mapping) - obstacle.type = team_to_obstacle_type[robot.team] + def create_team_data(self) -> TeamData: + return TeamData(header=self.create_header_with_own_time()) - obstacle.playerNumber = robot.player_id + def create_header_with_own_time(self) -> Header: + return Header(stamp=self.get_current_time().to_msg(), frame_id=self.map_frame) - pose_proto_to_ros(robot, obstacle.pose.pose) - if hasattr(message, "other_robot_confidence") and index < len(message.other_robot_confidence): - obstacle.pose.confidence = message.other_robot_confidence[index] + def should_message_be_discarded(self, message: Proto.Message) -> bool: + player_id = message.current_pose.player_id + team_id = message.current_pose.team - team_data.obstacles.obstacles.append(obstacle) + isOwnMessage = player_id == self.player_id + isMessageFromOpositeTeam = team_id != self.team_id - # Handle time to position at ball - ################################# - if hasattr(message, "time_to_ball"): - team_data.time_to_position_at_ball = message.time_to_ball + return isOwnMessage or isMessageFromOpositeTeam - # Handle strategy - ################# - if hasattr(message, "role"): - role_mapping = dict(self.role_mapping) - team_data.strategy.role = role_mapping[message.role] - if hasattr(message, "action"): - action_mapping = dict(self.action_mapping) - team_data.strategy.action = action_mapping[message.action] - if hasattr(message, "offensive_side"): - offensive_side_mapping = dict(self.side_mapping) - team_data.strategy.offensive_side = offensive_side_mapping[message.offensive_side] + def is_robot_allowed_to_send_message(self) -> bool: + return self.gamestate is not None and not self.gamestate.penalized - self.pub_team_data.publish(team_data) + def get_current_time(self) -> Time: + return self.node.get_clock().now() - def send_message(self): - def covariance_ros_to_proto(ros_covariance, fmat3): - # ROS covariance is row-major 36 x float, protobuf covariance is column-major 9 x float [x, y, θ] - fmat3.x.x = ros_covariance[0] - fmat3.y.x = ros_covariance[1] - fmat3.z.x = ros_covariance[5] - fmat3.x.y = ros_covariance[6] - fmat3.y.y = ros_covariance[7] - fmat3.z.y = ros_covariance[11] - fmat3.x.z = ros_covariance[30] - fmat3.y.z = ros_covariance[31] - fmat3.z.z = ros_covariance[35] - - message = robocup_extension_pb2.Message() - now = self.node.get_clock().now() - message.timestamp.seconds = now.seconds_nanoseconds()[0] - message.timestamp.nanos = now.seconds_nanoseconds()[1] - - message.current_pose.player_id = self.player_id - message.current_pose.team = self.team_id - - if self.gamestate and now - self.gamestate.header.stamp < Duration( - seconds=self.lifetime): - if self.gamestate.penalized: - # If we are penalized, we are not allowed to send team communication - return - else: - message.state = robocup_extension_pb2.State.UNPENALISED - else: - message.state = robocup_extension_pb2.State.UNKNOWN_STATE - - if self.pose and now - self.pose.header.stamp < Duration(seconds=self.lifetime): - message.current_pose.position.x = self.pose.pose.pose.position.x - message.current_pose.position.y = self.pose.pose.pose.position.y - q = self.pose.pose.pose.orientation - # z is theta - message.current_pose.position.z = transforms3d.euler.quat2euler([q.w, q.x, q.y, q.z])[2] - covariance_ros_to_proto(self.pose.pose.covariance, message.current_pose.covariance) - else: - # set high covariance to show that we have no clue - message.current_pose.covariance.x.x = 100 - message.current_pose.covariance.y.y = 100 - message.current_pose.covariance.z.z = 100 - - if self.cmd_vel and now - self.cmd_vel_time < Duration(seconds=self.lifetime): - message.walk_command.x = self.cmd_vel.linear.x - message.walk_command.y = self.cmd_vel.linear.y - message.walk_command.z = self.cmd_vel.angular.z - - if self.move_base_goal and now - self.move_base_goal.header.stamp < Duration( - seconds=self.lifetime): - message.target_pose.position.x = self.move_base_goal.pose.position.x - message.target_pose.position.y = self.move_base_goal.pose.position.y - q = self.move_base_goal.pose.orientation - message.target_pose.position.z = transforms3d.euler.quat2euler([q.w, q.x, q.y, q.z])[2] - - if self.ball and now - self.ball.header.stamp < Duration(seconds=self.lifetime): - message.ball.position.x = self.ball.point.x - message.ball.position.y = self.ball.point.y - message.ball.position.z = self.ball.point.z - message.ball.velocity.x = self.ball_vel[0] - message.ball.velocity.y = self.ball_vel[1] - message.ball.velocity.z = self.ball_vel[2] - covariance_ros_to_proto(self.ball_covariance, message.ball.covariance) - else: - # set high covariance to show that we have no clue - message.ball.covariance.x.x = 100 - message.ball.covariance.y.y = 100 - message.ball.covariance.z.z = 100 - - if self.obstacles and now - self.obstacles.header.stamp < Duration( - seconds=self.lifetime): - for obstacle in self.obstacles.obstacles: # type: ObstacleRelative - if obstacle.type in (ObstacleRelative.ROBOT_CYAN, ObstacleRelative.ROBOT_MAGENTA, - ObstacleRelative.ROBOT_UNDEFINED): - robot = robocup_extension_pb2.Robot() - robot.player_id = obstacle.playerNumber - robot.position.x = obstacle.pose.pose.pose.position.x - robot.position.y = obstacle.pose.pose.pose.position.y - q = obstacle.pose.pose.pose.orientation - robot.position.z = transforms3d.euler.quat2euler([q.w, q.x, q.y, q.z])[2] - team_mapping = dict(((b, a) for a, b in self.team_mapping)) - robot.team = team_mapping[obstacle.type] - message.others.append(robot) - message.other_robot_confidence.append(obstacle.pose.confidence) - - if (self.ball and now - self.ball.header.stamp < Duration(seconds=self.lifetime) and - self.pose and now - self.pose.header.stamp < Duration(seconds=self.lifetime)): - ball_distance = math.sqrt((self.ball.point.x - self.pose.pose.pose.position.x) ** 2 + - (self.ball.point.y - self.pose.pose.pose.position.y) ** 2) - message.time_to_ball = ball_distance / self.avg_walking_speed - - if self.strategy and now - self.strategy_time < Duration(seconds=self.lifetime): - role_mapping = dict(((b, a) for a, b in self.role_mapping)) - message.role = role_mapping[self.strategy.role] - - action_mapping = dict(((b, a) for a, b in self.action_mapping)) - message.action = action_mapping[self.strategy.action] - - side_mapping = dict(((b, a) for a, b in self.side_mapping)) - message.offensive_side = side_mapping[self.strategy.offensive_side] - - if self.time_to_ball and now - self.time_to_ball_time < Duration(seconds=self.lifetime): - message.time_to_ball = self.time_to_ball - else: - message.time_to_ball = 9999.0 - - msg = message.SerializeToString() - for port in self.target_ports: - self.logger.debug(f'Sending to {port} on {self.target_host}') - self.socket.sendto(msg, (self.target_host, port)) def main(): rclpy.init(args=None) HumanoidLeagueTeamCommunication() -if __name__ == '__main__': + +if __name__ == "__main__": main() diff --git a/humanoid_league_team_communication/launch/team_comm.launch b/humanoid_league_team_communication/launch/team_comm.launch index 4413b17..3d6fae1 100644 --- a/humanoid_league_team_communication/launch/team_comm.launch +++ b/humanoid_league_team_communication/launch/team_comm.launch @@ -1,5 +1,10 @@ + - + + + + + diff --git a/humanoid_league_team_communication/package.xml b/humanoid_league_team_communication/package.xml index d263ade..5ab762a 100644 --- a/humanoid_league_team_communication/package.xml +++ b/humanoid_league_team_communication/package.xml @@ -1,10 +1,11 @@ humanoid_league_team_communication - 2.0.0 + 3.0.0 The humanoid_league_team_communication package provides a ROS wrapper for the Protobuf RoboCup protocol. This is the standard communication protocol developed by the NUbots. + Joern Griepenburg Marc Bestmann Hamburg Bit-Bots @@ -28,6 +29,8 @@ python3-protobuf python3-transforms3d + python3-pytest + unknown diff --git a/humanoid_league_team_communication/scripts/show_team_comm.py b/humanoid_league_team_communication/scripts/show_team_comm.py index 30edb08..3401290 100755 --- a/humanoid_league_team_communication/scripts/show_team_comm.py +++ b/humanoid_league_team_communication/scripts/show_team_comm.py @@ -1,9 +1,11 @@ #!/usr/bin/env python3 + import sys +import threading import rclpy from rclpy.node import Node -from rclpy.time import Time +from rclpy.time import Time, Duration from rclpy.constants import S_TO_NS from humanoid_league_msgs.msg import TeamData, Strategy from transforms3d.euler import quat2euler @@ -34,36 +36,50 @@ SIDE: UNDEFINED """ + class TeamCommPrinter(Node): + def __init__(self): super().__init__("show_team_comm") self.subscriber = self.create_subscription(TeamData, "team_data", self.data_cb, 1) + self.team_data = {} for i in range(1, 5): self.team_data[i] = TeamData() self.team_data[i].robot_id = i - self.states = {TeamData.STATE_UNKNOWN: "Unknown", - TeamData.STATE_PENALIZED: "Penalized", - TeamData.STATE_UNPENALIZED: "Unpenalized"} - self.roles = {Strategy.ROLE_UNDEFINED: "Undefined", - Strategy.ROLE_GOALIE: "Goalie", - Strategy.ROLE_STRIKER: "Striker", - Strategy.ROLE_OTHER: "Other", - Strategy.ROLE_IDLING: "Idle", - Strategy.ROLE_DEFENDER: "Defender", - Strategy.ROLE_SUPPORTER: "Supporter"} - self.actions = {Strategy.ACTION_KICKING: "Kicking", - Strategy.ACTION_SEARCHING: "Searching", - Strategy.ACTION_LOCALIZING: "Localizing", - Strategy.ACTION_GOING_TO_BALL: "Going To Ball", - Strategy.ACTION_WAITING: "Waiting", - Strategy.ACTION_POSITIONING: "Positioning", - Strategy.ACTION_TRYING_TO_SCORE: "Trying to score", - Strategy.ACTION_UNDEFINED: "Undefined"} - self.sides = {Strategy.SIDE_LEFT: "Left", - Strategy.SIDE_RIGHT: "Right", - Strategy.SIDE_MIDDLE: "Middle", - Strategy.SIDE_UNDEFINED: "Undefined"} + + self.states = { + TeamData.STATE_UNKNOWN: "Unknown", + TeamData.STATE_PENALIZED: "Penalized", + TeamData.STATE_UNPENALIZED: "Unpenalized" + } + self.roles = { + Strategy.ROLE_UNDEFINED: "Undefined", + Strategy.ROLE_GOALIE: "Goalie", + Strategy.ROLE_STRIKER: "Striker", + Strategy.ROLE_OTHER: "Other", + Strategy.ROLE_IDLING: "Idle", + Strategy.ROLE_DEFENDER: "Defender", + Strategy.ROLE_SUPPORTER: "Supporter" + } + self.actions = { + Strategy.ACTION_KICKING: "Kicking", + Strategy.ACTION_SEARCHING: "Searching", + Strategy.ACTION_LOCALIZING: "Localizing", + Strategy.ACTION_GOING_TO_BALL: "Going To Ball", + Strategy.ACTION_WAITING: "Waiting", + Strategy.ACTION_POSITIONING: "Positioning", + Strategy.ACTION_TRYING_TO_SCORE: "Trying to score", + Strategy.ACTION_UNDEFINED: "Undefined" + } + self.sides = { + Strategy.SIDE_LEFT: "Left", + Strategy.SIDE_RIGHT: "Right", + Strategy.SIDE_MIDDLE: "Middle", + Strategy.SIDE_UNDEFINED: "Undefined" + } + + self.run_spin_in_thread() def data_cb(self, msg: TeamData): self.team_data[msg.robot_id] = msg @@ -97,7 +113,6 @@ def generate_string(self, data: TeamData): return lines def run(self): - rate = self.create_rate(1) first = True while rclpy.ok(): prints = [] @@ -117,7 +132,13 @@ def run(self): # append line with additional space to keep same length line = line + prints[j][i] + (max_line_length - len(prints[j][i])) * " " print(f"{line}") - rate.sleep() + + self.get_clock().sleep_for(Duration(seconds=1)) + + def run_spin_in_thread(self): + # Necessary in ROS2, else we are forever stuck + thread = threading.Thread(target=rclpy.spin, args=[self], daemon=True) + thread.start() if __name__ == '__main__': rclpy.init(args=None) diff --git a/humanoid_league_team_communication/scripts/team_comm.py b/humanoid_league_team_communication/scripts/team_comm.py index edf1980..e926827 100755 --- a/humanoid_league_team_communication/scripts/team_comm.py +++ b/humanoid_league_team_communication/scripts/team_comm.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 + import sys from humanoid_league_team_communication.humanoid_league_team_communication import main diff --git a/humanoid_league_team_communication/scripts/team_comm_test_marker.py b/humanoid_league_team_communication/scripts/team_comm_test_marker.py index afd5c12..b286395 100755 --- a/humanoid_league_team_communication/scripts/team_comm_test_marker.py +++ b/humanoid_league_team_communication/scripts/team_comm_test_marker.py @@ -29,6 +29,7 @@ class TeamCommMarker: + def __init__(self, server): self.server = server self.pose = Pose() @@ -97,6 +98,7 @@ def make_marker(self): class RobotMarker(TeamCommMarker): # todo change color based on active + def __init__(self, server): self.marker_name = "team_robot" self.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE @@ -141,6 +143,7 @@ def make_individual_markers(self, msg): class BallMarker(TeamCommMarker): + def __init__(self, server, id): self.marker_name = f"team_ball_{id}" self.interaction_mode = InteractiveMarkerControl.MOVE_PLANE @@ -163,6 +166,7 @@ def make_individual_markers(self, msg): class TeamMessage: + def __init__(self, robot, node): self.robot = robot self.node = node @@ -175,12 +179,12 @@ def publish(self): msg.header.frame_id = "map" msg.robot_id = self.robot.robot_id msg.robot_position.pose = self.robot.pose - msg.robot_position.covariance = [float(self.robot.covariance), 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, float(self.robot.covariance), 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, float(self.robot.covariance)] + msg.robot_position.covariance = [ + float(self.robot.covariance), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + float(self.robot.covariance), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + float(self.robot.covariance) + ] if self.robot.ball.active: try: @@ -194,8 +198,7 @@ def publish(self): ball_xyzw = self.robot.ball.pose.orientation mat_in_world = quat2mat((ball_xyzw.w, ball_xyzw.x, ball_xyzw.y, ball_xyzw.z)) trans_in_world = compose((self.robot.ball.pose.position.x, self.robot.ball.pose.position.y, - self.robot.ball.pose.position.z), - mat_in_world, [1, 1, 1]) + self.robot.ball.pose.position.z), mat_in_world, [1, 1, 1]) trans_in_robot = np.matmul(world_trans_in_robot, trans_in_world) pos_in_robot, mat_in_robot, _, _ = decompose(trans_in_robot) @@ -205,16 +208,15 @@ def publish(self): ball_absolute = PoseWithCovariance() ball_absolute.pose.position = self.robot.ball.pose.position ball_absolute.pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) - ball_absolute.covariance = [float(self.robot.ball.covariance), 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, float(self.robot.ball.covariance), 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, float(self.robot.ball.covariance)] + ball_absolute.covariance = [ + float(self.robot.ball.covariance), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + float(self.robot.ball.covariance), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + float(self.robot.ball.covariance) + ] msg.ball_absolute = ball_absolute - cartesian_distance = math.sqrt( - ball_relative.pose.position.x ** 2 + ball_relative.pose.position.y ** 2) + cartesian_distance = math.sqrt(ball_relative.pose.position.x**2 + ball_relative.pose.position.y**2) msg.time_to_position_at_ball = cartesian_distance / ROBOT_SPEED except tf2_ros.LookupException as ex: self.get_logger().warn(self.get_name() + ": " + str(ex), throttle_duration_sec=10.0) diff --git a/humanoid_league_team_communication/scripts/test_team_comm.py b/humanoid_league_team_communication/scripts/test_team_comm.py index 8a276dc..0dc24a6 100755 --- a/humanoid_league_team_communication/scripts/test_team_comm.py +++ b/humanoid_league_team_communication/scripts/test_team_comm.py @@ -4,40 +4,94 @@ """ import rclpy -from geometry_msgs.msg import PoseWithCovariance -from humanoid_league_msgs.msg import ObstacleRelativeArray, ObstacleRelative, PoseWithCertaintyArray, PoseWithCertainty +import numpy +from geometry_msgs.msg import Point, Pose, PoseWithCovariance, PoseWithCovarianceStamped, Quaternion, TransformStamped +from humanoid_league_msgs.msg import GameState, Strategy +from soccer_vision_3d_msgs.msg import Robot, RobotArray +from soccer_vision_attribute_msgs.msg import Robot as RobotAttributes +from tf2_ros import StaticTransformBroadcaster + + +def pose_with_covariance(x, y, z=0.0): + point = Point(x=x, y=y, z=z) + quat = Quaternion(x=0.2, y=0.3, z=0.5, w=0.8) + pose = PoseWithCovariance(pose=Pose(position=point, orientation=quat)) + pose.covariance = covariance_list() + + return PoseWithCovarianceStamped(pose=pose) + + +def covariance_list(): + covariance = numpy.empty(36, dtype=numpy.float64) + covariance[0] = 1.0 + covariance[6] = 2.0 + covariance[30] = 3.0 + covariance[1] = 4.0 + covariance[7] = 5.0 + covariance[31] = 6.0 + covariance[5] = 7.0 + covariance[11] = 8.0 + covariance[35] = 9.0 + + return covariance + + +def base_footprint_transform(): + transform = TransformStamped() + transform.header.frame_id = 'base_footprint' + transform.child_frame_id = 'map' + + return transform + if __name__ == '__main__': rclpy.init(args=None) node = rclpy.create_node("test_team_comm") - ball_pub = node.create_publisher(PoseWithCertaintyArray, "balls_relative", 1) - position_pub = node.create_publisher(PoseWithCertainty, "pose_with_certainty", 1) - obstacle_pub = node.create_publisher(ObstacleRelativeArray, "obstacles_relative", 1) - position_msg = PoseWithCertainty() - position_msg.pose.pose.position.x = 2.0 - position_msg.confidence = 0.7 - obstacle_msg = ObstacleRelativeArray() - obstacle = ObstacleRelative() - obstacle.pose.pose.pose.position.x = 4.0 - obstacle.type = 2 - obstacle_msg.obstacles.append(obstacle) - obstacle2 = ObstacleRelative() - obstacle2.pose.pose.pose.position.x = 2.0 - obstacle2.type = 2 - obstacle_msg.obstacles.append(obstacle2) - ball_msg = PoseWithCertaintyArray() - ball = PoseWithCertainty() - ball.confidence = 1.0 - ball_position = PoseWithCovariance() - ball_position.pose.position.x = 3.0 - ball.pose = ball_position - ball_msg.poses.append(ball) + tf_static_broadcaster = StaticTransformBroadcaster(node) + + gamestate_pub = node.create_publisher(GameState, "gamestate", 1) + strategy_pub = node.create_publisher(Strategy, "strategy", 1) + ball_pub = node.create_publisher(PoseWithCovarianceStamped, "ball_position_relative_filtered", 1) + position_pub = node.create_publisher(PoseWithCovarianceStamped, "pose_with_covariance", 1) + robots_pub = node.create_publisher(RobotArray, "robots_relative", 1) + + transform = base_footprint_transform() + gamestate_msg = GameState(penalized=False) + strategy_msg = Strategy(role=Strategy.ROLE_DEFENDER, + action=Strategy.ACTION_GOING_TO_BALL, + offensive_side=Strategy.SIDE_LEFT) + position_msg = pose_with_covariance(x=2.0, y=3.0) + ball_msg = pose_with_covariance(x=8.0, y=9.0) + robots_msg = RobotArray() + + robot = Robot() + robot.attributes.team = RobotAttributes.TEAM_OWN + robot.attributes.player_number = 2 + robot.bb.center.position.x = 4.0 + robot.bb.center.position.y = 5.0 + robots_msg.robots.append(robot) + + robot2 = Robot() + robot2.attributes.team = RobotAttributes.TEAM_OPPONENT + robot2.attributes.player_number = 3 + robot2.bb.center.position.x = 1.0 + robot2.bb.center.position.y = 2.0 + robots_msg.robots.append(robot2) while rclpy.ok(): - obstacle_msg.header.stamp = node.get_clock().now().to_msg() - obstacle_msg.header.frame_id = "base_footprint" - ball_msg.header.stamp = node.get_clock().now().to_msg() + now = node.get_clock().now().to_msg() + transform.header.stamp = now + gamestate_msg.header.stamp = now + position_msg.header.stamp = now + robots_msg.header.stamp = now + ball_msg.header.stamp = now + ball_msg.header.frame_id = "base_footprint" - ball_pub.publish(ball_msg) + robots_msg.header.frame_id = "base_footprint" + + tf_static_broadcaster.sendTransform(transform) + gamestate_pub.publish(gamestate_msg) + strategy_pub.publish(strategy_msg) position_pub.publish(position_msg) - obstacle_pub.publish(obstacle_msg) + ball_pub.publish(ball_msg) + robots_pub.publish(robots_msg) \ No newline at end of file diff --git a/humanoid_league_team_communication/scripts/tmux_testing_setup.zsh b/humanoid_league_team_communication/scripts/tmux_testing_setup.zsh new file mode 100755 index 0000000..a1725c0 --- /dev/null +++ b/humanoid_league_team_communication/scripts/tmux_testing_setup.zsh @@ -0,0 +1,38 @@ +#!/usr/bin/env zsh + +session="TeamComm" +pkg="humanoid_league_team_communication" + +run_tmux_session() { + session_running="$(tmux list-sessions | grep $session)" + + if [[ -z "$session_running" ]]; then + tmux new-session -d -s "$session" + + # window/pane setup + tmux rename-window -t "$session:1" "Launch" + tmux new-window -t "$session:2" -n "Test" + tmux split-window -h + tmux new-window -t "$session:3" -n "Base" + + # run required launch files in order + tmux send-keys -t "$session:Base" "rl bitbots_utils base.launch" Enter + tmux send-keys -t "$session:Launch" "rl humanoid_league_team_communication team_comm.launch" Enter + + # start test publisher/subscriber + tmux send-keys -t "$session:Test.top" "rr humanoid_league_team_communication test_team_comm.py" Enter + tmux send-keys -t "$session:Test.bottom" "rr humanoid_league_team_communication show_team_comm.py" Enter + fi + + tmux attach-session -t "$session:Test" +} + +kill_tmux_session() { + tmux kill_session "$session" +} + +trap kill_session INT + +cd "$COLCON_WS" +colcon build --symlink-install --packages-up-to "$pkg" +run_tmux_session diff --git a/humanoid_league_team_communication/setup.py b/humanoid_league_team_communication/setup.py index 248f4fe..d7628f8 100644 --- a/humanoid_league_team_communication/setup.py +++ b/humanoid_league_team_communication/setup.py @@ -1,13 +1,15 @@ -import glob - from setuptools import setup, find_packages package_name = 'humanoid_league_team_communication' setup( name=package_name, - packages=find_packages(), + packages=find_packages(exclude=['test']), install_requires=[ 'setuptools', ], + extras_require={ + 'dev': ['pytest', 'syrupy'], + } ) + diff --git a/humanoid_league_team_communication/test/__init__.py b/humanoid_league_team_communication/test/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/humanoid_league_team_communication/test/converter/__snapshots__/test_message_to_team_data_converter.ambr b/humanoid_league_team_communication/test/converter/__snapshots__/test_message_to_team_data_converter.ambr new file mode 100644 index 0000000..bbbe544 --- /dev/null +++ b/humanoid_league_team_communication/test/converter/__snapshots__/test_message_to_team_data_converter.ambr @@ -0,0 +1,52 @@ +# name: test_convert_ball_pose + ''' + geometry_msgs.msg.PoseWithCovariance(pose=geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=3.5999999046325684, y=1.7999999523162842, z=1.899999976158142), orientation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)), covariance=array([1.20000005, 4.19999981, 0. , 0. , 0. , + 7.19999981, 2.20000005, 5.19999981, 0. , 0. , + 0. , 8.19999981, 0. , 0. , 0. , + 0. , 0. , 0. , 0. , 0. , + 0. , 0. , 0. , 0. , 0. , + 0. , 0. , 0. , 0. , 0. , + 3.20000005, 6.19999981, 0. , 0. , 0. , + 9.19999981])) + ''' +# --- +# name: test_convert_current_pose + ''' + geometry_msgs.msg.PoseWithCovariance(pose=geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=3.5999999046325684, y=1.7999999523162842, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.8134154978551709, w=0.5816830991605519)), covariance=array([1.20000005, 4.19999981, 0. , 0. , 0. , + 7.19999981, 2.20000005, 5.19999981, 0. , 0. , + 0. , 8.19999981, 0. , 0. , 0. , + 0. , 0. , 0. , 0. , 0. , + 0. , 0. , 0. , 0. , 0. , + 0. , 0. , 0. , 0. , 0. , + 3.20000005, 6.19999981, 0. , 0. , 0. , + 9.19999981])) + ''' +# --- +# name: test_convert_empty_message + ''' + humanoid_league_msgs.msg.TeamData(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), robot_id=0, state=0, robot_position=geometry_msgs.msg.PoseWithCovariance(pose=geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=0.0, y=0.0, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)), covariance=array([0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., + 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., + 0., 0.])), ball_absolute=geometry_msgs.msg.PoseWithCovariance(pose=geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=0.0, y=0.0, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)), covariance=array([0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., + 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., + 0., 0.])), obstacles=humanoid_league_msgs.msg.ObstacleRelativeArray(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), obstacles=[]), time_to_position_at_ball=0.0, strategy=humanoid_league_msgs.msg.Strategy(role=0, action=0, offensive_side=0)) + ''' +# --- +# name: test_convert_robots_to_obstacles + ''' + humanoid_league_msgs.msg.ObstacleRelativeArray(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=1000, nanosec=0), frame_id=''), obstacles=[humanoid_league_msgs.msg.ObstacleRelative(type=3, player_number=4, pose=humanoid_league_msgs.msg.PoseWithCertainty(pose=geometry_msgs.msg.PoseWithCovariance(pose=geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=3.5999999046325684, y=1.7999999523162842, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.8134154978551709, w=0.5816830991605519)), covariance=array([1.20000005, 4.19999981, 0. , 0. , 0. , + 7.19999981, 2.20000005, 5.19999981, 0. , 0. , + 0. , 8.19999981, 0. , 0. , 0. , + 0. , 0. , 0. , 0. , 0. , + 0. , 0. , 0. , 0. , 0. , + 0. , 0. , 0. , 0. , 0. , + 3.20000005, 6.19999981, 0. , 0. , 0. , + 9.19999981])), confidence=0.0), width=0.0, height=0.0), humanoid_league_msgs.msg.ObstacleRelative(type=2, player_number=2, pose=humanoid_league_msgs.msg.PoseWithCertainty(pose=geometry_msgs.msg.PoseWithCovariance(pose=geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=3.5999999046325684, y=1.7999999523162842, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.8134154978551709, w=0.5816830991605519)), covariance=array([1.20000005, 4.19999981, 0. , 0. , 0. , + 7.19999981, 2.20000005, 5.19999981, 0. , 0. , + 0. , 8.19999981, 0. , 0. , 0. , + 0. , 0. , 0. , 0. , 0. , + 0. , 0. , 0. , 0. , 0. , + 0. , 0. , 0. , 0. , 0. , + 3.20000005, 6.19999981, 0. , 0. , 0. , + 9.19999981])), confidence=0.0), width=0.0, height=0.0)]) + ''' +# --- diff --git a/humanoid_league_team_communication/test/converter/__snapshots__/test_robocup_protocol_converter.ambr b/humanoid_league_team_communication/test/converter/__snapshots__/test_robocup_protocol_converter.ambr new file mode 100644 index 0000000..c3f131b --- /dev/null +++ b/humanoid_league_team_communication/test/converter/__snapshots__/test_robocup_protocol_converter.ambr @@ -0,0 +1,163 @@ +# name: test_setup_of_from_message_converter + dict({ + 'action_mapping': dict({ + 0: 0, + 1: 1, + 2: 2, + 3: 3, + 4: 4, + 5: 5, + 6: 6, + 7: 7, + }), + 'role_mapping': dict({ + 0: 0, + 1: 1, + 2: 2, + 3: 3, + 4: 4, + 5: 5, + 6: 6, + }), + 'side_mapping': dict({ + 0: 0, + 1: 1, + 2: 2, + 3: 3, + }), + 'team_mapping': dict({ + 0: 1, + 1: 3, + 2: 2, + }), + }) +# --- +# name: test_setup_of_mappings + tuple( + tuple( + 0, + 0, + ), + tuple( + 1, + 1, + ), + tuple( + 2, + 2, + ), + tuple( + 3, + 3, + ), + tuple( + 4, + 4, + ), + tuple( + 5, + 5, + ), + tuple( + 6, + 6, + ), + ) +# --- +# name: test_setup_of_mappings.1 + tuple( + tuple( + 0, + 0, + ), + tuple( + 1, + 1, + ), + tuple( + 2, + 2, + ), + tuple( + 3, + 3, + ), + tuple( + 4, + 4, + ), + tuple( + 5, + 5, + ), + tuple( + 6, + 6, + ), + tuple( + 7, + 7, + ), + ) +# --- +# name: test_setup_of_mappings.2 + tuple( + tuple( + 0, + 0, + ), + tuple( + 1, + 1, + ), + tuple( + 2, + 2, + ), + tuple( + 3, + 3, + ), + ) +# --- +# name: test_setup_of_team_color_mapping + dict({ + 0: 1, + 1: 3, + 2: 2, + }) +# --- +# name: test_setup_of_to_message_converter + dict({ + 'action_mapping': dict({ + 0: 0, + 1: 1, + 2: 2, + 3: 3, + 4: 4, + 5: 5, + 6: 6, + 7: 7, + }), + 'role_mapping': dict({ + 0: 0, + 1: 1, + 2: 2, + 3: 3, + 4: 4, + 5: 5, + 6: 6, + }), + 'side_mapping': dict({ + 0: 0, + 1: 1, + 2: 2, + 3: 3, + }), + 'team_mapping': dict({ + 0: 0, + 1: 1, + 2: 2, + }), + }) +# --- diff --git a/humanoid_league_team_communication/test/converter/__snapshots__/test_state_to_message_converter.ambr b/humanoid_league_team_communication/test/converter/__snapshots__/test_state_to_message_converter.ambr new file mode 100644 index 0000000..751b673 --- /dev/null +++ b/humanoid_league_team_communication/test/converter/__snapshots__/test_state_to_message_converter.ambr @@ -0,0 +1,139 @@ +# name: test_convert_ball_position + ''' + position { + x: 4.599999904632568 + y: 2.799999952316284 + z: 2.9000000953674316 + } + velocity { + x: 1.100000023841858 + y: 0.20000000298023224 + z: 0.30000001192092896 + } + covariance { + x { + x: 1.0 + y: 2.0 + z: 3.0 + } + y { + x: 4.0 + y: 5.0 + z: 6.0 + } + z { + x: 7.0 + y: 8.0 + z: 9.0 + } + } + + ''' +# --- +# name: test_convert_current_pose + ''' + x: 3.5999999046325684 + y: 1.7999999523162842 + z: 1.2167989015579224 + + ''' +# --- +# name: test_convert_current_pose.1 + ''' + x { + x: 1.0 + y: 2.0 + z: 3.0 + } + y { + x: 4.0 + y: 5.0 + z: 6.0 + } + z { + x: 7.0 + y: 8.0 + z: 9.0 + } + + ''' +# --- +# name: test_convert_empty_state + ''' + current_pose { + player_id: 2 + covariance { + x { + x: 100.0 + } + y { + y: 100.0 + } + z { + z: 100.0 + } + } + team: BLUE + } + ball { + covariance { + x { + x: 100.0 + } + y { + y: 100.0 + } + z { + z: 100.0 + } + } + } + time_to_ball: 9999.0 + + ''' +# --- +# name: test_convert_seen_robots + ''' + [player_id: 2 + position { + x: 3.5999999046325684 + y: 1.7999999523162842 + z: 1.2167989015579224 + } + team: BLUE + , player_id: 3 + position { + x: 3.5999999046325684 + y: 1.7999999523162842 + z: 1.2167989015579224 + } + team: RED + , player_id: 4 + position { + x: 3.5999999046325684 + y: 1.7999999523162842 + z: 1.2167989015579224 + } + ] + ''' +# --- +# name: test_convert_seen_robots.1 + list([ + 0.800000011920929, + 0.800000011920929, + 0.800000011920929, + ]) +# --- +# name: test_convert_target_pose + ''' + position { + x: 2.5999999046325684 + y: 0.800000011920929 + z: 1.2167989015579224 + } + + ''' +# --- +# name: test_convert_time_to_ball_calculated + 1.2856487035751343 +# --- diff --git a/humanoid_league_team_communication/test/converter/test_message_to_team_data_converter.py b/humanoid_league_team_communication/test/converter/test_message_to_team_data_converter.py new file mode 100644 index 0000000..b0b945a --- /dev/null +++ b/humanoid_league_team_communication/test/converter/test_message_to_team_data_converter.py @@ -0,0 +1,139 @@ +import humanoid_league_team_communication.robocup_extension_pb2 as Proto +import pytest +from humanoid_league_team_communication.converter.robocup_protocol_converter import RobocupProtocolConverter, TeamColor +from std_msgs.msg import Header + +from humanoid_league_msgs.msg import ObstacleRelative, Strategy, TeamData + +own_team_id = 1 +own_team_color = TeamColor(own_team_id) + + +def test_convert_empty_message(snapshot, message): + result = convert_from_message(message) + + assert str(result) == snapshot + + +def test_convert_current_pose(snapshot, message_with_current_pose): + team_data = convert_from_message(message_with_current_pose) + + assert team_data.robot_id == 3 + assert str(team_data.robot_position) == snapshot + + +def test_convert_ball_pose(snapshot, message_with_ball): + team_data = convert_from_message(message_with_ball) + + assert str(team_data.ball_absolute) == snapshot + + +def test_convert_robots_to_obstacles(snapshot, message_with_other_robots, team_data_with_header): + team_data = convert_from_message(message_with_other_robots, team_data_with_header) + + assert team_data.obstacles.header == team_data_with_header.header + assert team_data.obstacles.obstacles[0].type == ObstacleRelative.ROBOT_CYAN + assert team_data.obstacles.obstacles[1].type == ObstacleRelative.ROBOT_MAGENTA + + assert str(team_data.obstacles) == snapshot + + +def test_convert_time_to_ball(message): + message.time_to_ball = 16.84 + + team_data = convert_from_message(message) + + assert team_data.time_to_position_at_ball == pytest.approx(16.84) + + +def test_convert_strategy(message_with_strategy): + team_data = convert_from_message(message_with_strategy) + + assert team_data.strategy.role == Strategy.ROLE_STRIKER + assert team_data.strategy.action == Strategy.ACTION_KICKING + assert team_data.strategy.offensive_side == Strategy.SIDE_RIGHT + + +def convert_from_message(message: Proto.Message, team_data=TeamData()): + return RobocupProtocolConverter(own_team_color).convert_from_message(message, team_data) + + +@pytest.fixture +def message_with_strategy(message) -> Proto.Message: + message.role = Proto.Role.ROLE_STRIKER + message.action = Proto.Action.ACTION_KICKING + message.offensive_side = Proto.OffensiveSide.SIDE_RIGHT + + return message + + +@pytest.fixture +def message_with_other_robots(message) -> Proto.Message: + message.others.append(robot_message(player_id=4, is_own_team=True)) + message.others.append(robot_message(player_id=2, is_own_team=False)) + + return message + + +@pytest.fixture +def message_with_ball(message) -> Proto.Message: + set_position(message.ball.position) + set_covariance_matrix(message.ball.covariance) + + return message + + +@pytest.fixture +def message_with_current_pose(message) -> Proto.Message: + message.current_pose.player_id = 3 + message.current_pose.team = own_team_id + + set_position(message.current_pose.position) + set_covariance_matrix(message.current_pose.covariance) + + return message + + +@pytest.fixture +def team_data_with_header() -> TeamData: + team_data = TeamData() + team_data.header = Header() + team_data.header.stamp.sec = 1000 + + return team_data + + +@pytest.fixture +def message() -> Proto.Message: + return Proto.Message() + + +def robot_message(player_id, is_own_team): + robot = Proto.Robot() + robot.player_id = player_id + robot.team = own_team_id + if not is_own_team: + robot.team = own_team_id + 1 + + set_position(robot.position) + set_covariance_matrix(robot.covariance) + + return robot + + +def set_position(position: Proto.fvec3): + position.x = 3.6 + position.y = 1.8 + position.z = 1.9 + + +def set_covariance_matrix(covariance: Proto.fmat3): + covariance.x.x = 1.2 + covariance.x.y = 2.2 + covariance.x.z = 3.2 + covariance.y.x = 4.2 + covariance.y.y = 5.2 + covariance.y.z = 6.2 + covariance.z.x = 7.2 + covariance.z.y = 8.2 + covariance.z.z = 9.2 \ No newline at end of file diff --git a/humanoid_league_team_communication/test/converter/test_robocup_protocol_converter.py b/humanoid_league_team_communication/test/converter/test_robocup_protocol_converter.py new file mode 100644 index 0000000..c8f3d64 --- /dev/null +++ b/humanoid_league_team_communication/test/converter/test_robocup_protocol_converter.py @@ -0,0 +1,65 @@ +from unittest.mock import MagicMock + +import humanoid_league_team_communication.robocup_extension_pb2 as Proto +import pytest +from humanoid_league_team_communication.converter.robocup_protocol_converter import RobocupProtocolConverter, TeamColor +from soccer_vision_attribute_msgs.msg import Robot as RobotAttributes + +own_team_color = TeamColor.BLUE + + +def test_setup_of_mappings(snapshot): + converter = protocol_converter() + assert converter.role_mapping == snapshot + assert converter.action_mapping == snapshot + assert converter.side_mapping == snapshot + + +def test_setup_of_team_color_mapping(snapshot): + converter = protocol_converter() + assert converter.state_to_proto_team_mapping[RobotAttributes.TEAM_OWN] == Proto.Team.BLUE + assert converter.state_to_proto_team_mapping[RobotAttributes.TEAM_OPPONENT] == Proto.Team.RED + assert converter.state_to_proto_team_mapping[RobotAttributes.TEAM_UNKNOWN] == Proto.Team.UNKNOWN_TEAM + assert converter.proto_to_team_data_team_mapping == snapshot + + +def test_setup_of_to_message_converter(snapshot, to_message_mock): + protocol_converter() + + to_message_mock.assert_called_once() + args = to_message_mock.mock_calls[0].kwargs + assert args == snapshot + + +def test_setup_of_from_message_converter(snapshot, from_message_mock): + protocol_converter() + + from_message_mock.assert_called_once() + args = from_message_mock.mock_calls[0].kwargs + assert args == snapshot + + +def test_maps_convert_functions(to_message_mock, from_message_mock): + to_message_mock.return_value.convert.return_value = 'message' + from_message_mock.return_value.convert.return_value = 'team_data' + + converter = protocol_converter() + + assert converter.convert_to_message() == 'message' + assert converter.convert_from_message() == 'team_data' + + +def protocol_converter() -> RobocupProtocolConverter: + return RobocupProtocolConverter(own_team_color) + + +@pytest.fixture +def to_message_mock(mocker) -> MagicMock: + return mocker.patch( + "humanoid_league_team_communication.converter.robocup_protocol_converter.StateToMessageConverter") + + +@pytest.fixture +def from_message_mock(mocker) -> MagicMock: + return mocker.patch( + "humanoid_league_team_communication.converter.robocup_protocol_converter.MessageToTeamDataConverter") diff --git a/humanoid_league_team_communication/test/converter/test_state_to_message_converter.py b/humanoid_league_team_communication/test/converter/test_state_to_message_converter.py new file mode 100644 index 0000000..449c74c --- /dev/null +++ b/humanoid_league_team_communication/test/converter/test_state_to_message_converter.py @@ -0,0 +1,354 @@ +from unittest.mock import Mock + +import humanoid_league_team_communication.robocup_extension_pb2 as Proto +import numpy +import pytest +from geometry_msgs.msg import (Point, PointStamped, Pose, PoseStamped, PoseWithCovariance, PoseWithCovarianceStamped, + Quaternion, Twist, Vector3) +from humanoid_league_team_communication.converter.robocup_protocol_converter import TeamColor, RobocupProtocolConverter +from humanoid_league_team_communication.humanoid_league_team_communication import HumanoidLeagueTeamCommunication +from rclpy.time import Time +from soccer_vision_3d_msgs.msg import Robot, RobotArray +from soccer_vision_attribute_msgs.msg import Confidence, Robot as RobotAttributes +from std_msgs.msg import Header + +from humanoid_league_msgs.msg import GameState, Strategy + +own_team_id = 1 +own_team_color = TeamColor(own_team_id) +validity_checker_valid = Mock(return_value=True) +validity_checker_expired = Mock(return_value=False) + + +def test_convert_empty_state(snapshot, state): + result = convert_to_message(state) + + assert result.current_pose.player_id == state.player_id + assert result.current_pose.team == state.team_id + assert str(result) == snapshot + + +def test_convert_gamestate(state_with_gamestate): + gamestate = state_with_gamestate.gamestate + + gamestate.penalized = False + result = convert_to_message(state_with_gamestate) + assert result.state == Proto.State.UNPENALISED + + gamestate.penalized = True + result = convert_to_message(state_with_gamestate) + assert result.state == Proto.State.PENALISED + + assert validity_checker_valid.call_count == 2 + validity_checker_valid.assert_called_with(gamestate.header.stamp) + + +def test_convert_gamestate_expired_headers(state_with_gamestate): + message = Proto.Message() + message.state = Proto.State.UNPENALISED + + result = convert_to_message(state_with_gamestate, message, is_state_expired=True) + + assert result.state == Proto.State.UNKNOWN_STATE + + +def test_convert_current_pose(snapshot, state_with_current_pose): + result = convert_to_message(state_with_current_pose) + + assert str(result.current_pose.position) == snapshot + assert str(result.current_pose.covariance) == snapshot + + validity_checker_valid.assert_called_with(state_with_current_pose.pose.header.stamp) + + +def test_convert_current_pose_expired_headers(state_with_current_pose): + message = Proto.Message() + message.current_pose.position.x = 3.0 + + result = convert_to_message(state_with_current_pose, message, is_state_expired=True) + + assert result.current_pose.position.x == 3.0 + assert result.current_pose.covariance.x.x == 100 + assert result.current_pose.covariance.y.y == 100 + assert result.current_pose.covariance.z.z == 100 + + +def test_convert_walk_command(state_with_walk_command): + result = convert_to_message(state_with_walk_command) + + assert result.walk_command.x == state_with_walk_command.cmd_vel.linear.x + assert result.walk_command.y == state_with_walk_command.cmd_vel.linear.y + assert result.walk_command.z == state_with_walk_command.cmd_vel.angular.z + + validity_checker_valid.assert_called_with(state_with_walk_command.cmd_vel_time) + + +def test_convert_walk_command_expired_headers(state_with_walk_command): + message = Proto.Message() + message.walk_command.x = 3.0 + + result = convert_to_message(state_with_walk_command, message, is_state_expired=True) + + assert result.walk_command.x == 3.0 + assert result.walk_command.y == 0 + assert result.walk_command.z == 0 + + +def test_convert_target_pose(snapshot, state_with_target_pose): + result = convert_to_message(state_with_target_pose) + + assert str(result.target_pose) == snapshot + + validity_checker_valid.assert_called_with(state_with_target_pose.move_base_goal.header.stamp) + + +def test_convert_target_pose_expired_headers(state_with_target_pose): + message = Proto.Message() + message.target_pose.position.x = 3.0 + + result = convert_to_message(state_with_target_pose, message, is_state_expired=True) + + assert result.target_pose.position.x == 3.0 + assert result.target_pose.position.y == 0 + assert result.target_pose.position.z == 0 + + +def test_convert_ball_position(snapshot, state_with_ball_pose): + result = convert_to_message(state_with_ball_pose) + + assert str(result.ball) == snapshot + + validity_checker_valid.assert_called_with(state_with_ball_pose.ball.header.stamp) + + +def test_convert_ball_position_expired_headers(state_with_ball_pose): + result = convert_to_message(state_with_ball_pose, is_state_expired=True) + + assert result.ball.covariance.x.x == 100 + assert result.ball.covariance.y.y == 100 + assert result.ball.covariance.z.z == 100 + + +def test_convert_seen_robots(snapshot, state_with_seen_robots): + result = convert_to_message(state_with_seen_robots) + + assert str(result.others) == snapshot + assert result.other_robot_confidence == snapshot + + validity_checker_valid.assert_called_with(state_with_seen_robots.seen_robots.header.stamp) + + +def test_convert_obstacles_to_robots_expired_headers(state_with_seen_robots): + robot = Proto.Robot() + robot.player_id = 2 + message = Proto.Message() + message.others.append(robot) + + result = convert_to_message(state_with_seen_robots, message, is_state_expired=True) + + assert result.others[0].player_id == robot.player_id + assert len(result.others) == 1 + + +def test_convert_strategy(state_with_strategy): + result = convert_to_message(state_with_strategy) + + assert result.role == Proto.Role.ROLE_STRIKER + assert result.action == Proto.Action.ACTION_KICKING + assert result.offensive_side == Proto.OffensiveSide.SIDE_RIGHT + + validity_checker_valid.assert_called_with(state_with_strategy.strategy_time) + + +def test_convert_strategy_expired_headers(state_with_strategy): + message = Proto.Message() + message.role = Proto.Role.ROLE_DEFENDER + + result = convert_to_message(state_with_strategy, message, is_state_expired=True) + + assert result.role == Proto.Role.ROLE_DEFENDER + assert result.action == Proto.Action.ACTION_UNDEFINED + assert result.offensive_side == Proto.OffensiveSide.SIDE_UNDEFINED + + +def test_convert_time_to_ball(state_with_time_to_ball): + result = convert_to_message(state_with_time_to_ball) + + assert pytest.approx(result.time_to_ball) == state_with_time_to_ball.time_to_ball + + validity_checker_valid.assert_called_with(state_with_time_to_ball.time_to_ball_time) + + +def test_convert_time_to_ball_calculated(snapshot, state_with_ball_and_robot_pose): + result = convert_to_message(state_with_ball_and_robot_pose) + + assert result.time_to_ball == snapshot + + validity_checker_valid.assert_called_with(state_with_ball_and_robot_pose.pose.header.stamp) + validity_checker_valid.assert_called_with(state_with_ball_and_robot_pose.ball.header.stamp) + + +def test_convert_time_to_ball_expired_headers(state_with_time_to_ball): + message = Proto.Message() + message.time_to_ball = 16.5 + + result = convert_to_message(state_with_time_to_ball, message, is_state_expired=True) + + assert pytest.approx(result.time_to_ball) == 9999.0 + + +def convert_to_message(team_data_state, message: Proto.Message = None, is_state_expired=False): + message = message if message else Proto.Message() + validity_checker = validity_checker_expired if is_state_expired else validity_checker_valid + return RobocupProtocolConverter(own_team_color).convert_to_message(team_data_state, message, validity_checker) + + +@pytest.fixture +def state_with_ball_and_robot_pose(state_with_current_pose, state_with_ball_pose): + state_with_current_pose.ball = state_with_ball_pose.ball + return state_with_current_pose + + +@pytest.fixture +def state_with_time_to_ball(state): + state.time_to_ball = 6.4 + state.time_to_ball_time = Mock(Time) + + return state + + +@pytest.fixture +def state_with_strategy(state): + state.strategy = Strategy() + state.strategy.role = Strategy.ROLE_STRIKER + state.strategy.action = Strategy.ACTION_KICKING + state.strategy.offensive_side = Strategy.SIDE_RIGHT + state.strategy_time = Mock(Time) + + return state + + +@pytest.fixture +def state_with_seen_robots(state): + state.seen_robots = Mock(RobotArray) + state.seen_robots.header = Header() + state.seen_robots.robots = [ + relative_robot(RobotAttributes.TEAM_OPPONENT, 2), + relative_robot(RobotAttributes.TEAM_OWN, 3), + relative_robot(RobotAttributes.TEAM_UNKNOWN, 4), + ] + + return state + + +@pytest.fixture +def state_with_ball_pose(state): + point = Point(x=4.6, y=2.8, z=2.9) + state.ball = PointStamped(point=point) + state.ball.header = Header() + state.ball_velocity = (1.1, 0.2, 0.3) + state.ball_covariance = covariance_list() + + return state + + +@pytest.fixture +def state_with_walk_command(state): + state.cmd_vel = Mock(Twist) + state.cmd_vel.linear = Vector3(x=1.0, y=2.0) + state.cmd_vel.angular = Vector3(z=0.5) + + state.cmd_vel_time = Mock(Time) + + return state + + +@pytest.fixture +def state_with_target_pose(state): + state.move_base_goal = Mock(PoseStamped) + state.move_base_goal.header = Header() + point = Point(x=2.6, y=0.8, z=0.9) + quat = Quaternion(x=0.2, y=0.3, z=0.5, w=0.8) + state.move_base_goal.pose = Pose(position=point, orientation=quat) + + return state + + +@pytest.fixture +def state_with_current_pose(state): + state.pose = Mock(PoseWithCovarianceStamped) + state.pose.header = Header() + state.pose.pose = pose_with_covariance() + + return state + + +@pytest.fixture +def state_with_gamestate(state): + state.gamestate = Mock(GameState) + state.gamestate.header = Header() + state.gamestate.penalized = False + + return state + + +@pytest.fixture +def state(): + state = Mock(HumanoidLeagueTeamCommunication) + state.player_id = 2 + state.team_id = own_team_id + state.gamestate = None + state.pose = None + state.cmd_vel = None + state.cmd_vel_time = None + state.move_base_goal = None + state.ball = None + state.ball_velocity = None + state.ball_covariance = None + state.seen_robots = None + state.strategy = None + state.strategy_time = None + state.time_to_ball = None + state.time_to_ball_time = None + state.avg_walking_speed = 1.1 + + return state + + +def relative_robot(team, player_number) -> Robot: + robot = Robot() + robot.attributes.team = team + robot.attributes.player_number = player_number + robot.bb.center = base_pose() + robot.confidence = Confidence(confidence=0.8) + + return robot + + +def pose_with_covariance() -> PoseWithCovariance: + pose = PoseWithCovariance(pose=base_pose()) + pose.covariance = covariance_list() + + return pose + + +def base_pose() -> Pose: + point = Point(x=3.6, y=1.8, z=1.9) + quat = Quaternion(x=0.2, y=0.3, z=0.5, w=0.8) + + return Pose(position=point, orientation=quat) + + +def covariance_list(): + covariance = numpy.empty(36, dtype=numpy.float64) + covariance[0] = 1.0 + covariance[6] = 2.0 + covariance[30] = 3.0 + covariance[1] = 4.0 + covariance[7] = 5.0 + covariance[31] = 6.0 + covariance[5] = 7.0 + covariance[11] = 8.0 + covariance[35] = 9.0 + + return covariance \ No newline at end of file