From e086aabf3562eca3957ef1887f0c983a170609ec Mon Sep 17 00:00:00 2001 From: texhnolyze Date: Sun, 15 Jan 2023 22:01:25 +0100 Subject: [PATCH 01/17] style(team_comm): apply google python styleguide --- .../humanoid_league_team_communication.py | 24 ++++---- .../scripts/show_team_comm.py | 56 +++++++++++-------- .../scripts/team_comm.py | 1 + .../scripts/team_comm_test_marker.py | 34 +++++------ .../scripts/test_team_comm.py | 1 + 5 files changed, 67 insertions(+), 49 deletions(-) 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..6a8e6fc 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,11 +1,11 @@ #!/usr/bin/env python3 + import math import socket import struct import threading from typing import Optional - import rclpy from rclpy.duration import Duration from rclpy.node import Node @@ -22,7 +22,9 @@ 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 @@ -319,6 +321,7 @@ def pose_proto_to_ros(robot, pose): self.pub_team_data.publish(team_data) 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] @@ -339,8 +342,7 @@ def covariance_ros_to_proto(ros_covariance, fmat3): 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 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 @@ -367,8 +369,7 @@ def covariance_ros_to_proto(ros_covariance, fmat3): 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): + 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 @@ -388,8 +389,7 @@ def covariance_ros_to_proto(ros_covariance, fmat3): 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): + 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): @@ -404,10 +404,10 @@ def covariance_ros_to_proto(ros_covariance, fmat3): 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) + 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): @@ -430,9 +430,11 @@ def covariance_ros_to_proto(ros_covariance, fmat3): 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__': main() diff --git a/humanoid_league_team_communication/scripts/show_team_comm.py b/humanoid_league_team_communication/scripts/show_team_comm.py index 30edb08..09d7fa5 100755 --- a/humanoid_league_team_communication/scripts/show_team_comm.py +++ b/humanoid_league_team_communication/scripts/show_team_comm.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 + import sys import rclpy @@ -34,7 +35,9 @@ 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) @@ -42,28 +45,36 @@ def __init__(self): 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" + } def data_cb(self, msg: TeamData): self.team_data[msg.robot_id] = msg @@ -119,6 +130,7 @@ def run(self): print(f"{line}") rate.sleep() + if __name__ == '__main__': rclpy.init(args=None) printer = TeamCommPrinter() 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..3d01541 100755 --- a/humanoid_league_team_communication/scripts/test_team_comm.py +++ b/humanoid_league_team_communication/scripts/test_team_comm.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 + """ This script publishes dummy values for ball, goalpost, position and obstacles for testing the team communication. """ From 5d0d0bb9eee079f9499b875210bc6690d49ccada Mon Sep 17 00:00:00 2001 From: texhnolyze Date: Sun, 15 Jan 2023 22:50:45 +0100 Subject: [PATCH 02/17] fix(team_comm): correctly pass args on thread setup --- .../humanoid_league_team_communication.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 6a8e6fc..988db85 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 @@ -115,7 +115,7 @@ def __init__(self): 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 = threading.Thread(target=rclpy.spin, args=[self.node], daemon=True) thread.start() self.node.create_timer(1 / self.rate, self.send_message) From c134c75aac53d4bd5811e0d3255b11c5d9566db3 Mon Sep 17 00:00:00 2001 From: texhnolyze Date: Sun, 15 Jan 2023 22:26:07 +0100 Subject: [PATCH 03/17] refactor(team_comm): extract UDP socket communication --- .../communication.py | 53 ++++++++++++++++ .../humanoid_league_team_communication.py | 63 +++++-------------- 2 files changed, 70 insertions(+), 46 deletions(-) create mode 100644 humanoid_league_team_communication/humanoid_league_team_communication/communication.py 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..c62ae26 --- /dev/null +++ b/humanoid_league_team_communication/humanoid_league_team_communication/communication.py @@ -0,0 +1,53 @@ +import socket + +from rclpy.node import Node + + +class SocketCommunication: + + def __init__(self, node: Node, logger, team_id, robot_id): + self.logger = logger + + self.socket: socket.socket = None + self.target_host = node.get_parameter('target_host').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 + local_target_ports = node.get_parameter('local_target_ports').get_parameter_value().integer_array_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 = node.get_parameter('target_port').get_parameter_value().integer_value + receive_port = node.get_parameter('receive_port').get_parameter_value().integer_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) -> bytes: + return self.socket.recv(1024) + + def send_message(self, message): + for port in self.target_ports: + self.logger.debug(f'Sending message to {port} on {self.target_host}') + self.socket.sendto(message, (self.target_host, port)) 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 988db85..5a03fb9 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 @@ -21,13 +21,13 @@ from ament_index_python.packages import get_package_share_directory from humanoid_league_team_communication import robocup_extension_pb2 +from humanoid_league_team_communication.communication import SocketCommunication 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() @@ -37,11 +37,8 @@ def __init__(self): 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.socket_communication = SocketCommunication(self.node, self.logger, self.team_id, self.player_id) + 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 @@ -49,14 +46,6 @@ def __init__(self): 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.create_publishers() self.create_subscribers() @@ -107,12 +96,7 @@ def __init__(self): self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self.node) - # 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() - self.node.get_clock().sleep_for(Duration(seconds=1)) - rclpy.spin_once(self.node) - self.socket.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) + self.try_to_establish_connection() # Necessary in ROS2, else we are forever stuck receiving messages thread = threading.Thread(target=rclpy.spin, args=[self.node], daemon=True) @@ -121,6 +105,13 @@ def __init__(self): self.node.create_timer(1 / self.rate, self.send_message) self.receive_forever() + def try_to_establish_connection(self): + # we will try multiple times till we manage to get a 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) + def create_publishers(self): self.pub_team_data = self.node.create_publisher(TeamData, self.topics['team_data_topic'], 1) @@ -136,20 +127,6 @@ def create_subscribers(self): 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.gamestate = msg @@ -199,20 +176,17 @@ def ball_cb(self, msg: PoseWithCovarianceStamped): 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) - def __del__(self): - self.close_connection() - 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) + if message: + self.handle_message(message) - def handle_message(self, msg): + def handle_message(self, string_message): 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, θ] @@ -240,7 +214,7 @@ def pose_proto_to_ros(robot, pose): covariance_proto_to_ros(robot.covariance, pose.covariance) message = robocup_extension_pb2.Message() - message.ParseFromString(msg) + message.ParseFromString(string_message) player_id = message.current_pose.player_id team_id = message.current_pose.team @@ -425,10 +399,7 @@ def covariance_ros_to_proto(ros_covariance, fmat3): 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)) + self.socket_communication.send_message(message.SerializeToString()) def main(): From 8480c2d84bf9e60146a0cd609cad9a1af191db4c Mon Sep 17 00:00:00 2001 From: texhnolyze Date: Sun, 15 Jan 2023 23:27:53 +0100 Subject: [PATCH 04/17] refactor(team_comm): add typing and helper methods --- .../humanoid_league_team_communication.py | 191 ++++++++++-------- 1 file changed, 105 insertions(+), 86 deletions(-) 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 5a03fb9..0b8ab09 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 @@ -4,7 +4,8 @@ import socket import struct import threading -from typing import Optional +from numpy import double +from typing import List, Optional, Tuple import rclpy from rclpy.duration import Duration @@ -14,14 +15,14 @@ import transforms3d from rclpy.time import Time from std_msgs.msg import Header, Float32 -from geometry_msgs.msg import Twist, PoseWithCovarianceStamped, TwistWithCovarianceStamped +from geometry_msgs.msg import PoseWithCovariance, PoseWithCovarianceStamped, Twist, TwistWithCovarianceStamped from humanoid_league_msgs.msg import GameState, TeamData, ObstacleRelativeArray, ObstacleRelative, Strategy 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 humanoid_league_team_communication import robocup_extension_pb2 from humanoid_league_team_communication.communication import SocketCommunication +from humanoid_league_team_communication.robocup_extension_pb2 import * class HumanoidLeagueTeamCommunication: @@ -49,61 +50,65 @@ def __init__(self): self.create_publishers() self.create_subscribers() - self.gamestate = None # type: GameState - self.pose = None # type: PoseWithCovarianceStamped - self.cmd_vel = None # type: Twist - 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.strategy_time = Time(nanoseconds=0, clock_type=self.node.get_clock().clock_type) - self.time_to_ball = 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 + self.set_state_defaults() # 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.team_mapping = ((Team.UNKNOWN_TEAM, ObstacleRelative.ROBOT_UNDEFINED), + (Team.BLUE, ObstacleRelative.ROBOT_CYAN), (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), + (Role.ROLE_UNDEFINED, Strategy.ROLE_UNDEFINED), + (Role.ROLE_IDLING, Strategy.ROLE_IDLING), + (Role.ROLE_OTHER, Strategy.ROLE_OTHER), + (Role.ROLE_STRIKER, Strategy.ROLE_STRIKER), + (Role.ROLE_SUPPORTER, Strategy.ROLE_SUPPORTER), + (Role.ROLE_DEFENDER, Strategy.ROLE_DEFENDER), + (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), + (Action.ACTION_UNDEFINED, Strategy.ACTION_UNDEFINED), + (Action.ACTION_POSITIONING, Strategy.ACTION_POSITIONING), + (Action.ACTION_GOING_TO_BALL, Strategy.ACTION_GOING_TO_BALL), + (Action.ACTION_TRYING_TO_SCORE, Strategy.ACTION_TRYING_TO_SCORE), + (Action.ACTION_WAITING, Strategy.ACTION_WAITING), + (Action.ACTION_KICKING, Strategy.ACTION_KICKING), + (Action.ACTION_SEARCHING, Strategy.ACTION_SEARCHING), + (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), + (OffensiveSide.SIDE_UNDEFINED, Strategy.SIDE_UNDEFINED), + (OffensiveSide.SIDE_LEFT, Strategy.SIDE_LEFT), + (OffensiveSide.SIDE_MIDDLE, Strategy.SIDE_MIDDLE), + (OffensiveSide.SIDE_RIGHT, Strategy.SIDE_RIGHT), ) self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.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() - self.node.create_timer(1 / self.rate, self.send_message) - self.receive_forever() + def set_state_defaults(self): + self.gamestate: GameState = None + self.pose: PoseWithCovarianceStamped = None + self.cmd_vel: Twist = None + self.cmd_vel_time = Time(nanoseconds=0, clock_type=self.node.get_clock().clock_type) + self.ball: Optional[PointStamped] = None + self.ball_velocity: Tuple[float, float, float] = (0, 0, 0) + self.ball_covariance: List[double] = None + self.strategy: Strategy = None + self.strategy_time = Time(nanoseconds=0, clock_type=self.node.get_clock().clock_type) + self.time_to_ball: float = None + self.time_to_ball_time = Time(nanoseconds=0, clock_type=self.node.get_clock().clock_type) + self.obstacles: ObstacleRelativeArray = None + self.move_base_goal: PoseStamped = None def try_to_establish_connection(self): # we will try multiple times till we manage to get a connection @@ -113,68 +118,70 @@ def try_to_establish_connection(self): rclpy.spin_once(self.node) 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(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(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 gamestate_cb(self, msg): + 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() - 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() - 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() - 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 obstacle_cb(self, msg: ObstacleRelativeArray): + + def transform_to_map(obstacle: ObstacleRelative): + obstacle_pose = PoseStamped(header=msg.header, pose=obstacle.pose.pose.pose) try: - obstacle_map = self.tf_buffer.transform(obstacle_pose, - self.map_frame, - timeout=Duration(nanoseconds=0.3e9)) + obstacle_map = self.tf_transform(obstacle_pose) obstacle.pose.pose.pose = obstacle_map.pose - self.obstacles.obstacles.append(obstacle) + return obstacle except tf2_ros.TransformException: self.logger.error("TeamComm: Could not transform obstacle to map frame") + self.obstacles = ObstacleRelativeArray(header=msg.header) + self.obstacles.header.frame_id = self.map_frame + self.obstacles.obstacles = list(map(transform_to_map, msg.obstacles)) + 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.tf_transform(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) + 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 tf_transform(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(): @@ -186,9 +193,9 @@ def receive_forever(self): if message: self.handle_message(message) - def handle_message(self, string_message): + def handle_message(self, string_message: bytes): - def covariance_proto_to_ros(fmat3, ros_covariance): + def covariance_proto_to_ros(fmat3: fmat3, ros_covariance: List[double]): # 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 @@ -200,7 +207,7 @@ def covariance_proto_to_ros(fmat3, ros_covariance): ros_covariance[31] = fmat3.y.z ros_covariance[35] = fmat3.z.z - def pose_proto_to_ros(robot, pose): + def pose_proto_to_ros(robot: Robot, pose: PoseWithCovariance): pose.pose.position.x = robot.position.x pose.pose.position.y = robot.position.y @@ -213,21 +220,17 @@ def pose_proto_to_ros(robot, pose): if pose.covariance: covariance_proto_to_ros(robot.covariance, pose.covariance) - message = robocup_extension_pb2.Message() + message = Message() message.ParseFromString(string_message) - player_id = message.current_pose.player_id - team_id = message.current_pose.team - - 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): 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.stamp = self.get_current_time() header.frame_id = self.map_frame # Handle timestamp @@ -236,7 +239,7 @@ def pose_proto_to_ros(robot, pose): # Handle robot ID ################# - team_data.robot_id = player_id + team_data.robot_id = message.current_pose.player_id # Handle state ############## @@ -292,11 +295,11 @@ def pose_proto_to_ros(robot, pose): offensive_side_mapping = dict(self.side_mapping) team_data.strategy.offensive_side = offensive_side_mapping[message.offensive_side] - self.pub_team_data.publish(team_data) + self.team_data_publisher.publish(team_data) def send_message(self): - def covariance_ros_to_proto(ros_covariance, fmat3): + def covariance_ros_to_proto(ros_covariance: List[double], fmat3: 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] @@ -308,8 +311,8 @@ def covariance_ros_to_proto(ros_covariance, fmat3): fmat3.y.z = ros_covariance[31] fmat3.z.z = ros_covariance[35] - message = robocup_extension_pb2.Message() - now = self.node.get_clock().now() + message = Message() + now = self.get_current_time() message.timestamp.seconds = now.seconds_nanoseconds()[0] message.timestamp.nanos = now.seconds_nanoseconds()[1] @@ -321,9 +324,9 @@ def covariance_ros_to_proto(ros_covariance, fmat3): # If we are penalized, we are not allowed to send team communication return else: - message.state = robocup_extension_pb2.State.UNPENALISED + message.state = State.UNPENALISED else: - message.state = robocup_extension_pb2.State.UNKNOWN_STATE + message.state = 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 @@ -364,10 +367,11 @@ def covariance_ros_to_proto(ros_covariance, fmat3): 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 + for obstacle in self.obstacles.obstacles: + obstacle: ObstacleRelative if obstacle.type in (ObstacleRelative.ROBOT_CYAN, ObstacleRelative.ROBOT_MAGENTA, ObstacleRelative.ROBOT_UNDEFINED): - robot = robocup_extension_pb2.Robot() + robot = Robot() robot.player_id = obstacle.playerNumber robot.position.x = obstacle.pose.pose.pose.position.x robot.position.y = obstacle.pose.pose.pose.position.y @@ -401,6 +405,21 @@ def covariance_ros_to_proto(ros_covariance, fmat3): self.socket_communication.send_message(message.SerializeToString()) + def should_message_be_discarded(self, message: Message) -> bool: + player_id = message.current_pose.player_id + team_id = message.current_pose.team + + isOwnMessage = player_id == self.player_id + isMessageFromOpositeTeam = team_id != self.team_id + + return isOwnMessage or isMessageFromOpositeTeam + + def is_robot_allowed_to_send_message(self) -> bool: + return self.gamestate and not self.gamestate.penalized + + def get_current_time(self) -> Time: + return self.node.get_clock().now() + def main(): rclpy.init(args=None) From c9698fd9215b14a4b6198c6cc29bfead062027d5 Mon Sep 17 00:00:00 2001 From: texhnolyze Date: Mon, 16 Jan 2023 02:09:52 +0100 Subject: [PATCH 05/17] fix(team_comm): checking for expired msg data as timestamps from headers now need to be converted as the times in messages are not `rclpy.time.Time` classes --- .../humanoid_league_team_communication.py | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) 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 0b8ab09..47adbc6 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 @@ -230,7 +230,7 @@ def pose_proto_to_ros(robot: Robot, pose: PoseWithCovariance): header = Header() # The robots' times can differ, therefore use our own time here - header.stamp = self.get_current_time() + header.stamp = self.get_current_time().to_msg() header.frame_id = self.map_frame # Handle timestamp @@ -319,7 +319,7 @@ def covariance_ros_to_proto(ros_covariance: List[double], fmat3: fmat3): 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 and now - Time.from_msg(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 @@ -328,7 +328,7 @@ def covariance_ros_to_proto(ros_covariance: List[double], fmat3: fmat3): else: message.state = State.UNKNOWN_STATE - if self.pose and now - self.pose.header.stamp < Duration(seconds=self.lifetime): + if self.pose and now - Time(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 @@ -346,13 +346,14 @@ def covariance_ros_to_proto(ros_covariance: List[double], fmat3: fmat3): 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): + if self.move_base_goal and now - Time.from_msg(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): + if self.ball and now - Time.from_msg(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 @@ -366,13 +367,13 @@ def covariance_ros_to_proto(ros_covariance: List[double], fmat3: fmat3): 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): + if self.obstacles and now - Time.from_msg(self.obstacles.header.stamp) < Duration(seconds=self.lifetime): for obstacle in self.obstacles.obstacles: obstacle: ObstacleRelative if obstacle.type in (ObstacleRelative.ROBOT_CYAN, ObstacleRelative.ROBOT_MAGENTA, ObstacleRelative.ROBOT_UNDEFINED): robot = Robot() - robot.player_id = obstacle.playerNumber + robot.player_id = obstacle.player_number robot.position.x = obstacle.pose.pose.pose.position.x robot.position.y = obstacle.pose.pose.pose.position.y q = obstacle.pose.pose.pose.orientation @@ -382,8 +383,8 @@ def covariance_ros_to_proto(ros_covariance: List[double], fmat3: fmat3): 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)): + if (self.ball and now - Time.from_msg(self.ball.header.stamp) < Duration(seconds=self.lifetime) and + self.pose and now - Time.from_msg(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 From 7f29d0506a9cffe60ad2e58b809b8210d9a03316 Mon Sep 17 00:00:00 2001 From: texhnolyze Date: Mon, 16 Jan 2023 14:57:26 +0100 Subject: [PATCH 06/17] refactor(team_comm): fix/improve scripts for ros2 usage --- .../scripts/show_team_comm.py | 15 +++- .../scripts/test_team_comm.py | 85 +++++++++++++------ 2 files changed, 73 insertions(+), 27 deletions(-) diff --git a/humanoid_league_team_communication/scripts/show_team_comm.py b/humanoid_league_team_communication/scripts/show_team_comm.py index 09d7fa5..3401290 100755 --- a/humanoid_league_team_communication/scripts/show_team_comm.py +++ b/humanoid_league_team_communication/scripts/show_team_comm.py @@ -1,10 +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 @@ -41,10 +42,12 @@ 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", @@ -76,6 +79,8 @@ def __init__(self): Strategy.SIDE_UNDEFINED: "Undefined" } + self.run_spin_in_thread() + def data_cb(self, msg: TeamData): self.team_data[msg.robot_id] = msg @@ -108,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 = [] @@ -128,8 +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/test_team_comm.py b/humanoid_league_team_communication/scripts/test_team_comm.py index 3d01541..33a5d88 100755 --- a/humanoid_league_team_communication/scripts/test_team_comm.py +++ b/humanoid_league_team_communication/scripts/test_team_comm.py @@ -1,44 +1,81 @@ #!/usr/bin/env python3 - """ This script publishes dummy values for ball, goalpost, position and obstacles for testing the team communication. """ 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 +from humanoid_league_msgs.msg import GameState, ObstacleRelativeArray, ObstacleRelative, Strategy + + +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 + 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) + + 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) 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 + + 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) obstacle_msg = ObstacleRelativeArray() + obstacle = ObstacleRelative() obstacle.pose.pose.pose.position.x = 4.0 - obstacle.type = 2 + obstacle.pose.pose.pose.position.y = 5.0 + obstacle.type = ObstacleRelative.ROBOT_CYAN obstacle_msg.obstacles.append(obstacle) + obstacle2 = ObstacleRelative() - obstacle2.pose.pose.pose.position.x = 2.0 - obstacle2.type = 2 + obstacle2.pose.pose.pose.position.x = 1.0 + obstacle2.pose.pose.pose.position.y = 2.0 + obstacle2.type = ObstacleRelative.ROBOT_MAGENTA 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) 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() - ball_msg.header.frame_id = "base_footprint" - ball_pub.publish(ball_msg) + now = node.get_clock().now().to_msg() + gamestate_msg.header.stamp = now + position_msg.header.stamp = now + obstacle_msg.header.stamp = now + ball_msg.header.stamp = now + + # @TODO: check if we should use another map frame + ball_msg.header.frame_id = "map" + obstacle_msg.header.frame_id = "map" + + 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) + obstacle_pub.publish(obstacle_msg) \ No newline at end of file From 80f40eb30f007a0a0bd20a6e6d13bbdce6858db9 Mon Sep 17 00:00:00 2001 From: texhnolyze Date: Mon, 16 Jan 2023 18:12:32 +0100 Subject: [PATCH 07/17] refactor(team_comm): extract conversion between ros/protobuf --- .../converter/__init__.py | 0 .../message_to_team_data_converter.py | 103 +++++++ .../converter/robocup_protocol_converter.py | 55 ++++ .../converter/state_to_message_converter.py | 173 +++++++++++ .../humanoid_league_team_communication.py | 275 +++--------------- 5 files changed, 366 insertions(+), 240 deletions(-) create mode 100644 humanoid_league_team_communication/humanoid_league_team_communication/converter/__init__.py create mode 100644 humanoid_league_team_communication/humanoid_league_team_communication/converter/message_to_team_data_converter.py create mode 100644 humanoid_league_team_communication/humanoid_league_team_communication/converter/robocup_protocol_converter.py create mode 100644 humanoid_league_team_communication/humanoid_league_team_communication/converter/state_to_message_converter.py 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..c29d353 --- /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 humanoid_league_team_communication.robocup_extension_pb2 import Ball, Message, Robot, fmat3 +from numpy import double + +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: 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) + + 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: 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: 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[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) + + # @TODO: are lists in python/protobuf always sorted and can confidences be empty? + 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: 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: 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: 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..0ad53fb --- /dev/null +++ b/humanoid_league_team_communication/humanoid_league_team_communication/converter/robocup_protocol_converter.py @@ -0,0 +1,55 @@ +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_team_communication.robocup_extension_pb2 import Action, OffensiveSide, Role, Team + +from humanoid_league_msgs.msg import ObstacleRelative, Strategy + + +class RobocupProtocolConverter: + + def __init__(self): + self.team_mapping = ((Team.UNKNOWN_TEAM, ObstacleRelative.ROBOT_UNDEFINED), + (Team.BLUE, ObstacleRelative.ROBOT_CYAN), (Team.RED, ObstacleRelative.ROBOT_MAGENTA)) + self.role_mapping = ( + (Role.ROLE_UNDEFINED, Strategy.ROLE_UNDEFINED), + (Role.ROLE_IDLING, Strategy.ROLE_IDLING), + (Role.ROLE_OTHER, Strategy.ROLE_OTHER), + (Role.ROLE_STRIKER, Strategy.ROLE_STRIKER), + (Role.ROLE_SUPPORTER, Strategy.ROLE_SUPPORTER), + (Role.ROLE_DEFENDER, Strategy.ROLE_DEFENDER), + (Role.ROLE_GOALIE, Strategy.ROLE_GOALIE), + ) + self.action_mapping = ( + (Action.ACTION_UNDEFINED, Strategy.ACTION_UNDEFINED), + (Action.ACTION_POSITIONING, Strategy.ACTION_POSITIONING), + (Action.ACTION_GOING_TO_BALL, Strategy.ACTION_GOING_TO_BALL), + (Action.ACTION_TRYING_TO_SCORE, Strategy.ACTION_TRYING_TO_SCORE), + (Action.ACTION_WAITING, Strategy.ACTION_WAITING), + (Action.ACTION_KICKING, Strategy.ACTION_KICKING), + (Action.ACTION_SEARCHING, Strategy.ACTION_SEARCHING), + (Action.ACTION_LOCALIZING, Strategy.ACTION_LOCALIZING), + ) + self.side_mapping = ( + (OffensiveSide.SIDE_UNDEFINED, Strategy.SIDE_UNDEFINED), + (OffensiveSide.SIDE_LEFT, Strategy.SIDE_LEFT), + (OffensiveSide.SIDE_MIDDLE, Strategy.SIDE_MIDDLE), + (OffensiveSide.SIDE_RIGHT, Strategy.SIDE_RIGHT), + ) + + mappings = { + "team_mapping": dict(self.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) + reverse_mappings = { + "team_mapping": reverse_mapping(self.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(**mappings).convert + self.convert_to_message = StateToMessageConverter(**reverse_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..0c9eb90 --- /dev/null +++ b/humanoid_league_team_communication/humanoid_league_team_communication/converter/state_to_message_converter.py @@ -0,0 +1,173 @@ +import math +import transforms3d +from numpy import double +from typing import Callable, List, Tuple + +from rclpy.time import Time +from geometry_msgs.msg import PointStamped, PoseStamped, PoseWithCovarianceStamped, Quaternion, Twist +from humanoid_league_msgs.msg import GameState, ObstacleRelative, ObstacleRelativeArray, Strategy + +from humanoid_league_team_communication.robocup_extension_pb2 import * + + +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: Message, is_still_valid_checker: Callable[[Time], bool]) -> Message: + + def convert_gamestate(gamestate: GameState, message: Message): + if gamestate and is_still_valid_checker(gamestate.header.stamp): + message.state = State.PENALISED if gamestate.penalized else State.UNPENALISED + else: + message.state = State.UNKNOWN_STATE + + return message + + def convert_current_pose(current_pose: PoseWithCovarianceStamped, message: Message): + if current_pose 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_rotation_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: Twist, walk_command_time: Time, message: Message): + if walk_command 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: PoseStamped, message): + if target_position 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_rotation_angle(pose.orientation) + + return message + + def convert_ball_position(ball_position: PointStamped, ball_velocity: Tuple[float, float, float], + ball_covariance: List[double], message): + if ball_position 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_obstacles_to_robots(relative_obstacles: ObstacleRelativeArray, message: Message): + # @TODO: should confidence be decreased in else case? + if relative_obstacles and is_still_valid_checker(relative_obstacles.header.stamp): + obstacle: ObstacleRelative + for obstacle in relative_obstacles.obstacles: + if obstacle.type in (ObstacleRelative.ROBOT_CYAN, ObstacleRelative.ROBOT_MAGENTA, + ObstacleRelative.ROBOT_UNDEFINED): + robot = Robot() + robot.player_id = obstacle.player_number + + pose = obstacle.pose.pose.pose + robot.team = self.team_mapping[obstacle.type] + robot.position.x = pose.position.x + robot.position.y = pose.position.y + robot.position.z = self.extract_orientation_rotation_angle(pose.orientation) + + message.others.append(robot) + message.other_robot_confidence.append(obstacle.pose.confidence) + + return message + + def convert_strategy(strategy: Strategy, strategy_time: Time, message: Message): + if strategy 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: PoseWithCovarianceStamped, + ball_position: PointStamped) -> bool: + return (ball_position and is_still_valid_checker(ball_position.header.stamp) and current_pose 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: float, time_to_ball_time: Time, ball_position: PointStamped, + current_pose: PoseWithCovarianceStamped, walking_speed: float, message: Message): + if time_to_ball 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_obstacles_to_robots(state.obstacles, 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_rotation_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: 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 47adbc6..73130ae 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,6 +1,5 @@ #!/usr/bin/env python3 -import math import socket import struct import threading @@ -8,21 +7,21 @@ from typing import List, Optional, Tuple import rclpy +import tf2_ros +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, ObstacleRelative, ObstacleRelativeArray, 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 PoseWithCovariance, PoseWithCovarianceStamped, Twist, TwistWithCovarianceStamped -from humanoid_league_msgs.msg import GameState, TeamData, ObstacleRelativeArray, ObstacleRelative, Strategy +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 humanoid_league_team_communication.communication import SocketCommunication -from humanoid_league_team_communication.robocup_extension_pb2 import * +from humanoid_league_team_communication.robocup_extension_pb2 import Message +from humanoid_league_team_communication.converter.robocup_protocol_converter import RobocupProtocolConverter class HumanoidLeagueTeamCommunication: @@ -31,6 +30,7 @@ def __init__(self): self._package_path = get_package_share_directory("humanoid_league_team_communication") self.node = Node("team_comm", automatically_declare_parameters_from_overrides=True) self.logger = self.node.get_logger() + self.protocol_converter = RobocupProtocolConverter() self.logger.info("Initializing humanoid_league_team_communication...") @@ -52,35 +52,6 @@ def __init__(self): self.set_state_defaults() - # Protobuf <-> ROS Message mappings - self.team_mapping = ((Team.UNKNOWN_TEAM, ObstacleRelative.ROBOT_UNDEFINED), - (Team.BLUE, ObstacleRelative.ROBOT_CYAN), (Team.RED, ObstacleRelative.ROBOT_MAGENTA)) - self.role_mapping = ( - (Role.ROLE_UNDEFINED, Strategy.ROLE_UNDEFINED), - (Role.ROLE_IDLING, Strategy.ROLE_IDLING), - (Role.ROLE_OTHER, Strategy.ROLE_OTHER), - (Role.ROLE_STRIKER, Strategy.ROLE_STRIKER), - (Role.ROLE_SUPPORTER, Strategy.ROLE_SUPPORTER), - (Role.ROLE_DEFENDER, Strategy.ROLE_DEFENDER), - (Role.ROLE_GOALIE, Strategy.ROLE_GOALIE), - ) - self.action_mapping = ( - (Action.ACTION_UNDEFINED, Strategy.ACTION_UNDEFINED), - (Action.ACTION_POSITIONING, Strategy.ACTION_POSITIONING), - (Action.ACTION_GOING_TO_BALL, Strategy.ACTION_GOING_TO_BALL), - (Action.ACTION_TRYING_TO_SCORE, Strategy.ACTION_TRYING_TO_SCORE), - (Action.ACTION_WAITING, Strategy.ACTION_WAITING), - (Action.ACTION_KICKING, Strategy.ACTION_KICKING), - (Action.ACTION_SEARCHING, Strategy.ACTION_SEARCHING), - (Action.ACTION_LOCALIZING, Strategy.ACTION_LOCALIZING), - ) - self.side_mapping = ( - (OffensiveSide.SIDE_UNDEFINED, Strategy.SIDE_UNDEFINED), - (OffensiveSide.SIDE_LEFT, Strategy.SIDE_LEFT), - (OffensiveSide.SIDE_MIDDLE, Strategy.SIDE_MIDDLE), - (OffensiveSide.SIDE_RIGHT, Strategy.SIDE_RIGHT), - ) - self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self.node) @@ -140,15 +111,15 @@ def pose_cb(self, msg: PoseWithCovarianceStamped): def cmd_vel_cb(self, msg: Twist): self.cmd_vel = msg - self.cmd_vel_time = self.get_current_time() + self.cmd_vel_time = self.get_current_time().to_msg() def strategy_cb(self, msg: Strategy): self.strategy = msg - self.strategy_time = self.get_current_time() + self.strategy_time = self.get_current_time().to_msg() def time_to_ball_cb(self, msg: float): self.time_to_ball = msg.data - self.time_to_ball_time = self.get_current_time() + self.time_to_ball_time = self.get_current_time().to_msg() def move_base_goal_cb(self, msg: PoseStamped): self.move_base_goal = msg @@ -194,217 +165,41 @@ def receive_forever(self): self.handle_message(message) def handle_message(self, string_message: bytes): - - def covariance_proto_to_ros(fmat3: fmat3, ros_covariance: List[double]): - # 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: Robot, pose: PoseWithCovariance): - 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 = Message() message.ParseFromString(string_message) if self.should_message_be_discarded(message): + self.logger.info("Discarding msg by player {} in team {} at {}".format(message.current_pose.player_id, + message.current_pose.team, + message.timestamp.seconds)) return - team_data = TeamData() - - header = Header() - # The robots' times can differ, therefore use our own time here - header.stamp = self.get_current_time().to_msg() - header.frame_id = self.map_frame - - # Handle timestamp - ################## - team_data.header = header - - # Handle robot ID - ################# - team_data.robot_id = message.current_pose.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) - - # 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) - - # Handle obstacles - ################## - obstacle_relative_array = ObstacleRelativeArray() - obstacle_relative_array.header = header - - for index, robot in enumerate(message.others): - obstacle = ObstacleRelative() - - # Obstacle type - team_to_obstacle_type = dict(self.team_mapping) - obstacle.type = team_to_obstacle_type[robot.team] - - obstacle.playerNumber = robot.player_id - - 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] - - team_data.obstacles.obstacles.append(obstacle) - - # Handle time to position at ball - ################################# - if hasattr(message, "time_to_ball"): - team_data.time_to_position_at_ball = message.time_to_ball - - # 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] - + team_data = self.protocol_converter.convert_from_message(message, self.create_team_data()) self.team_data_publisher.publish(team_data) def send_message(self): + if not self.is_robot_allowed_to_send_message(): + self.logger.info("Not allowed to send message") + return - def covariance_ros_to_proto(ros_covariance: List[double], fmat3: 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] + 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()) + def create_empty_message(self, now: Time) -> Message: message = Message() - now = self.get_current_time() - 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 - Time.from_msg(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 = State.UNPENALISED - else: - message.state = State.UNKNOWN_STATE - - if self.pose and now - Time(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 - Time.from_msg(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 - Time.from_msg(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 - Time.from_msg(self.obstacles.header.stamp) < Duration(seconds=self.lifetime): - for obstacle in self.obstacles.obstacles: - obstacle: ObstacleRelative - if obstacle.type in (ObstacleRelative.ROBOT_CYAN, ObstacleRelative.ROBOT_MAGENTA, - ObstacleRelative.ROBOT_UNDEFINED): - robot = Robot() - robot.player_id = obstacle.player_number - 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 - Time.from_msg(self.ball.header.stamp) < Duration(seconds=self.lifetime) and - self.pose and now - Time.from_msg(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 + seconds, nanoseconds = now.seconds_nanoseconds() + message.timestamp.seconds = seconds + message.timestamp.nanos = nanoseconds + return message - self.socket_communication.send_message(message.SerializeToString()) + def create_team_data(self) -> TeamData: + return TeamData(header=self.create_header_with_own_time()) + + def create_header_with_own_time(self) -> Header: + return Header(stamp=self.get_current_time().to_msg(), frame_id=self.map_frame) def should_message_be_discarded(self, message: Message) -> bool: player_id = message.current_pose.player_id From 42837fb1253d0cc855d6117e6cd49dc012d08760 Mon Sep 17 00:00:00 2001 From: texhnolyze Date: Mon, 16 Jan 2023 18:13:25 +0100 Subject: [PATCH 08/17] test(team_comm): unit and syrupy snapshot tests for conversion between ros state <-> protobuf <-> team data --- .../test/__init__.py | 0 .../test_message_to_team_data_converter.ambr | 52 +++ .../test_robocup_protocol_converter.ambr | 172 +++++++++ .../test_state_to_message_converter.ambr | 139 +++++++ .../test_message_to_team_data_converter.py | 138 +++++++ .../test_robocup_protocol_converter.py | 50 +++ .../test_state_to_message_converter.py | 346 ++++++++++++++++++ 7 files changed, 897 insertions(+) create mode 100644 humanoid_league_team_communication/test/__init__.py create mode 100644 humanoid_league_team_communication/test/converter/__snapshots__/test_message_to_team_data_converter.ambr create mode 100644 humanoid_league_team_communication/test/converter/__snapshots__/test_robocup_protocol_converter.ambr create mode 100644 humanoid_league_team_communication/test/converter/__snapshots__/test_state_to_message_converter.ambr create mode 100644 humanoid_league_team_communication/test/converter/test_message_to_team_data_converter.py create mode 100644 humanoid_league_team_communication/test/converter/test_robocup_protocol_converter.py create mode 100644 humanoid_league_team_communication/test/converter/test_state_to_message_converter.py 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..2fdd5e7 --- /dev/null +++ b/humanoid_league_team_communication/test/converter/__snapshots__/test_robocup_protocol_converter.ambr @@ -0,0 +1,172 @@ +# 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, + 1, + ), + tuple( + 1, + 3, + ), + tuple( + 2, + 2, + ), + ) +# --- +# 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, + ), + ) +# --- +# name: test_setup_of_mappings.2 + 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.3 + tuple( + tuple( + 0, + 0, + ), + tuple( + 1, + 1, + ), + tuple( + 2, + 2, + ), + tuple( + 3, + 3, + ), + ) +# --- +# 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({ + 1: 0, + 2: 2, + 3: 1, + }), + }) +# --- 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..d7391df --- /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_obstacles_to_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_obstacles_to_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..0e147b7 --- /dev/null +++ b/humanoid_league_team_communication/test/converter/test_message_to_team_data_converter.py @@ -0,0 +1,138 @@ +import pytest +from humanoid_league_team_communication.converter.robocup_protocol_converter import RobocupProtocolConverter +from humanoid_league_team_communication.robocup_extension_pb2 import * +from std_msgs.msg import Header + +from humanoid_league_msgs.msg import ObstacleRelative, Strategy, TeamData + +own_team_id = 1 + + +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: Message, team_data=TeamData()): + return RobocupProtocolConverter().convert_from_message(message, team_data) + + +@pytest.fixture +def message_with_strategy(message) -> Message: + message.role = Role.ROLE_STRIKER + message.action = Action.ACTION_KICKING + message.offensive_side = OffensiveSide.SIDE_RIGHT + + return message + + +@pytest.fixture +def message_with_other_robots(message) -> 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) -> Message: + set_position(message.ball.position) + set_covariance_matrix(message.ball.covariance) + + return message + + +@pytest.fixture +def message_with_current_pose(message) -> 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() -> Message: + return Message() + + +def robot_message(player_id, is_own_team): + robot = 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: fvec3): + position.x = 3.6 + position.y = 1.8 + position.z = 1.9 + + +def set_covariance_matrix(covariance: 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..12e7fa1 --- /dev/null +++ b/humanoid_league_team_communication/test/converter/test_robocup_protocol_converter.py @@ -0,0 +1,50 @@ +from unittest.mock import MagicMock + +import pytest +from humanoid_league_team_communication.converter.robocup_protocol_converter import RobocupProtocolConverter + + +def test_setup_of_mappings(snapshot): + converter = RobocupProtocolConverter() + assert converter.team_mapping == snapshot + assert converter.role_mapping == snapshot + assert converter.action_mapping == snapshot + assert converter.side_mapping == snapshot + + +def test_setup_of_to_message_converter(snapshot, to_message_mock): + RobocupProtocolConverter() + + 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): + RobocupProtocolConverter() + + 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 = RobocupProtocolConverter() + + assert converter.convert_to_message() == 'message' + assert converter.convert_from_message() == 'team_data' + + +@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..3320e7e --- /dev/null +++ b/humanoid_league_team_communication/test/converter/test_state_to_message_converter.py @@ -0,0 +1,346 @@ +from unittest.mock import Mock + +import numpy +import pytest +from std_msgs.msg import Header +from geometry_msgs.msg import (Point, PointStamped, Pose, PoseStamped, PoseWithCovariance, PoseWithCovarianceStamped, + Quaternion, Twist, Vector3) +from humanoid_league_team_communication.converter.robocup_protocol_converter import RobocupProtocolConverter +from humanoid_league_team_communication.humanoid_league_team_communication import HumanoidLeagueTeamCommunication +from humanoid_league_team_communication.robocup_extension_pb2 import * +from rclpy.time import Time + +from humanoid_league_msgs.msg import GameState, ObstacleRelative, ObstacleRelativeArray, PoseWithCertainty, Strategy + +own_team_id = 1 +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 == State.UNPENALISED + + gamestate.penalized = True + result = convert_to_message(state_with_gamestate) + assert result.state == 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 = Message() + message.state = State.UNPENALISED + + result = convert_to_message(state_with_gamestate, message, is_state_expired=True) + + assert result.state == 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 = 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 = 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 = 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_obstacles_to_robots(snapshot, state_with_obstacles): + result = convert_to_message(state_with_obstacles) + + assert str(result.others) == snapshot + assert result.other_robot_confidence == snapshot + + validity_checker_valid.assert_called_with(state_with_obstacles.obstacles.header.stamp) + + +def test_convert_obstacles_to_robots_expired_headers(state_with_obstacles): + robot = Robot() + robot.player_id = 2 + message = Message() + message.others.append(robot) + + result = convert_to_message(state_with_obstacles, 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 == Role.ROLE_STRIKER + assert result.action == Action.ACTION_KICKING + assert result.offensive_side == OffensiveSide.SIDE_RIGHT + + validity_checker_valid.assert_called_with(state_with_strategy.strategy_time) + + +def test_convert_strategy_expired_headers(state_with_strategy): + message = Message() + message.role = Role.ROLE_DEFENDER + + result = convert_to_message(state_with_strategy, message, is_state_expired=True) + + assert result.role == Role.ROLE_DEFENDER + assert result.action == Action.ACTION_UNDEFINED + assert result.offensive_side == 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 = 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: Message = None, is_state_expired=False): + message = message if message else Message() + validity_checker = validity_checker_expired if is_state_expired else validity_checker_valid + return RobocupProtocolConverter().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_obstacles(state): + state.obstacles = Mock(ObstacleRelativeArray) + state.obstacles.header = Header() + state.obstacles.obstacles = [ + relative_obstacle(ObstacleRelative.ROBOT_CYAN, 2), + relative_obstacle(ObstacleRelative.ROBOT_MAGENTA, 3), + relative_obstacle(ObstacleRelative.ROBOT_UNDEFINED, 4), + relative_obstacle(ObstacleRelative.HUMAN), + relative_obstacle(ObstacleRelative.POLE), + ] + + 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.obstacles = 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_obstacle(obstacle_type, player_number=None): + obstacle = Mock(ObstacleRelative) + obstacle.type = obstacle_type + obstacle.player_number = player_number + obstacle.pose = PoseWithCertainty(pose=pose_with_covariance(), confidence=0.8) + + return obstacle + + +def pose_with_covariance(): + 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) + pose = PoseWithCovariance(pose=Pose(position=point, orientation=quat)) + pose.covariance = covariance_list() + + return 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 \ No newline at end of file From dcf2176c453ae27ffebe0bf1af8221c422c6dadc Mon Sep 17 00:00:00 2001 From: texhnolyze Date: Tue, 31 Jan 2023 21:20:59 +0100 Subject: [PATCH 09/17] test(team_comm): add tmux testing script opening multiple panes and launching the TeamComm, while also listening and showing messages from other robots --- .../scripts/tmux_testing_setup.zsh | 38 +++++++++++++++++++ 1 file changed, 38 insertions(+) create mode 100755 humanoid_league_team_communication/scripts/tmux_testing_setup.zsh 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 From cbf04a22c1123a7bb7a84e799622cb79b53c5fed Mon Sep 17 00:00:00 2001 From: texhnolyze Date: Thu, 19 Jan 2023 17:21:22 +0100 Subject: [PATCH 10/17] fix(team_comm): switch to RobotArray msgs on `robots_relative` topic, instead of ObstaclRelativeArray on `obstacles_relative` topic, where we do not publish any msgs. Currently the vision produces RobotArray msgs on `robots_in_image`, which are then transformed relative to the `base_footprint` map frame to have relative coordinates based on the running robot's location. This is done in the ipm (inverse projection mapping), which in turn publishes `robots_relative`. For now we totally humans and other obstacles, which could be handled with ObstaclRelative msgs. Currently the TeamData msgs also still contains an ObstaclRelativeArray, because change it would most likely also require changes in the behavior module. This will be done at a later stage. Fixes: https://github.com/bit-bots/humanoid_league_misc/issues/125 --- .../config/team_communication_config.yaml | 2 +- .../message_to_team_data_converter.py | 17 +-- .../converter/robocup_protocol_converter.py | 77 +++++++----- .../converter/state_to_message_converter.py | 57 +++++---- .../humanoid_league_team_communication.py | 49 ++++---- .../scripts/test_team_comm.py | 40 ++++--- .../test_robocup_protocol_converter.ambr | 31 ++--- .../test_state_to_message_converter.ambr | 4 +- .../test_message_to_team_data_converter.py | 33 +++--- .../test_robocup_protocol_converter.py | 27 ++++- .../test_state_to_message_converter.py | 110 ++++++++++-------- 11 files changed, 244 insertions(+), 203 deletions(-) diff --git a/humanoid_league_team_communication/config/team_communication_config.yaml b/humanoid_league_team_communication/config/team_communication_config.yaml index 9c821df..4416686 100644 --- a/humanoid_league_team_communication/config/team_communication_config.yaml +++ b/humanoid_league_team_communication/config/team_communication_config.yaml @@ -30,7 +30,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/converter/message_to_team_data_converter.py b/humanoid_league_team_communication/humanoid_league_team_communication/converter/message_to_team_data_converter.py index c29d353..4ac805a 100644 --- 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 @@ -2,9 +2,9 @@ import transforms3d from geometry_msgs.msg import PoseWithCovariance -from humanoid_league_team_communication.robocup_extension_pb2 import Ball, Message, Robot, fmat3 from numpy import double +import humanoid_league_team_communication.robocup_extension_pb2 as Proto from humanoid_league_msgs.msg import ObstacleRelative, ObstacleRelativeArray, TeamData @@ -16,25 +16,26 @@ def __init__(self, team_mapping, role_mapping, action_mapping, side_mapping): self.action_mapping = action_mapping self.side_mapping = side_mapping - def convert(self, message: Message, team_data: TeamData) -> TeamData: + 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 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: Message, team_data: TeamData) -> TeamData: + 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: Message, team_data: TeamData) -> TeamData: + 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"): @@ -44,7 +45,7 @@ def convert_strategy(self, message: Message, team_data: TeamData) -> TeamData: return team_data - def convert_robots_to_obstacles(self, message_robots: List[Robot], + def convert_robots_to_obstacles(self, message_robots: List[Proto.Robot], message_robot_confidence: List[float]) -> ObstacleRelativeArray: relative_obstacles = ObstacleRelativeArray() @@ -60,7 +61,7 @@ def convert_robots_to_obstacles(self, message_robots: List[Robot], return relative_obstacles - def convert_ball_pose(self, message_ball_pose: Ball) -> PoseWithCovariance: + 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 @@ -71,7 +72,7 @@ def convert_ball_pose(self, message_ball_pose: Ball) -> PoseWithCovariance: return ball - def convert_robot_pose(self, message_robot_pose: Robot) -> PoseWithCovariance: + 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 @@ -89,7 +90,7 @@ def convert_robot_pose(self, message_robot_pose: Robot) -> PoseWithCovariance: 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: fmat3): + 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 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 index 0ad53fb..cf24c68 100644 --- 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 @@ -1,55 +1,74 @@ +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_team_communication.robocup_extension_pb2 import Action, OffensiveSide, Role, Team 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): - self.team_mapping = ((Team.UNKNOWN_TEAM, ObstacleRelative.ROBOT_UNDEFINED), - (Team.BLUE, ObstacleRelative.ROBOT_CYAN), (Team.RED, ObstacleRelative.ROBOT_MAGENTA)) + def __init__(self, own_team_color: TeamColor): self.role_mapping = ( - (Role.ROLE_UNDEFINED, Strategy.ROLE_UNDEFINED), - (Role.ROLE_IDLING, Strategy.ROLE_IDLING), - (Role.ROLE_OTHER, Strategy.ROLE_OTHER), - (Role.ROLE_STRIKER, Strategy.ROLE_STRIKER), - (Role.ROLE_SUPPORTER, Strategy.ROLE_SUPPORTER), - (Role.ROLE_DEFENDER, Strategy.ROLE_DEFENDER), - (Role.ROLE_GOALIE, Strategy.ROLE_GOALIE), + (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 = ( - (Action.ACTION_UNDEFINED, Strategy.ACTION_UNDEFINED), - (Action.ACTION_POSITIONING, Strategy.ACTION_POSITIONING), - (Action.ACTION_GOING_TO_BALL, Strategy.ACTION_GOING_TO_BALL), - (Action.ACTION_TRYING_TO_SCORE, Strategy.ACTION_TRYING_TO_SCORE), - (Action.ACTION_WAITING, Strategy.ACTION_WAITING), - (Action.ACTION_KICKING, Strategy.ACTION_KICKING), - (Action.ACTION_SEARCHING, Strategy.ACTION_SEARCHING), - (Action.ACTION_LOCALIZING, Strategy.ACTION_LOCALIZING), + (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 = ( - (OffensiveSide.SIDE_UNDEFINED, Strategy.SIDE_UNDEFINED), - (OffensiveSide.SIDE_LEFT, Strategy.SIDE_LEFT), - (OffensiveSide.SIDE_MIDDLE, Strategy.SIDE_MIDDLE), - (OffensiveSide.SIDE_RIGHT, Strategy.SIDE_RIGHT), + (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), ) - mappings = { - "team_mapping": dict(self.team_mapping), + 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 + } + # @TODO: check if RobotAttributes.TEAM_OWN is based on the robot seen or robot running + 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) - reverse_mappings = { - "team_mapping": reverse_mapping(self.team_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(**mappings).convert - self.convert_to_message = StateToMessageConverter(**reverse_mappings).convert + self.convert_from_message = MessageToTeamDataConverter(**proto_to_team_data_mappings).convert + self.convert_to_message = StateToMessageConverter(**state_to_proto_mappings).convert \ No newline at end of file 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 index 0c9eb90..a018892 100644 --- 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 @@ -5,9 +5,10 @@ from rclpy.time import Time from geometry_msgs.msg import PointStamped, PoseStamped, PoseWithCovarianceStamped, Quaternion, Twist -from humanoid_league_msgs.msg import GameState, ObstacleRelative, ObstacleRelativeArray, Strategy +from humanoid_league_msgs.msg import GameState, Strategy +from soccer_vision_3d_msgs.msg import Robot, RobotArray -from humanoid_league_team_communication.robocup_extension_pb2 import * +import humanoid_league_team_communication.robocup_extension_pb2 as Proto class StateToMessageConverter: @@ -18,17 +19,17 @@ def __init__(self, team_mapping, role_mapping, action_mapping, side_mapping): self.action_mapping = action_mapping self.side_mapping = side_mapping - def convert(self, state, message: Message, is_still_valid_checker: Callable[[Time], bool]) -> Message: + def convert(self, state, message: Proto.Message, is_still_valid_checker: Callable[[Time], bool]) -> Proto.Message: - def convert_gamestate(gamestate: GameState, message: Message): + def convert_gamestate(gamestate: GameState, message: Proto.Message): if gamestate and is_still_valid_checker(gamestate.header.stamp): - message.state = State.PENALISED if gamestate.penalized else State.UNPENALISED + message.state = Proto.State.PENALISED if gamestate.penalized else Proto.State.UNPENALISED else: - message.state = State.UNKNOWN_STATE + message.state = Proto.State.UNKNOWN_STATE return message - def convert_current_pose(current_pose: PoseWithCovarianceStamped, message: Message): + def convert_current_pose(current_pose: PoseWithCovarianceStamped, message: Proto.Message): if current_pose and is_still_valid_checker(current_pose.header.stamp): pose_with_covariance = current_pose.pose pose = pose_with_covariance.pose @@ -43,7 +44,7 @@ def convert_current_pose(current_pose: PoseWithCovarianceStamped, message: Messa return message - def convert_walk_command(walk_command: Twist, walk_command_time: Time, message: Message): + def convert_walk_command(walk_command: Twist, walk_command_time: Time, message: Proto.Message): if walk_command and is_still_valid_checker(walk_command_time): message.walk_command.x = walk_command.linear.x message.walk_command.y = walk_command.linear.y @@ -77,28 +78,26 @@ def convert_ball_position(ball_position: PointStamped, ball_velocity: Tuple[floa return message - def convert_obstacles_to_robots(relative_obstacles: ObstacleRelativeArray, message: Message): + def convert_seen_robots(seen_robots: RobotArray, message: Proto.Message): # @TODO: should confidence be decreased in else case? - if relative_obstacles and is_still_valid_checker(relative_obstacles.header.stamp): - obstacle: ObstacleRelative - for obstacle in relative_obstacles.obstacles: - if obstacle.type in (ObstacleRelative.ROBOT_CYAN, ObstacleRelative.ROBOT_MAGENTA, - ObstacleRelative.ROBOT_UNDEFINED): - robot = Robot() - robot.player_id = obstacle.player_number - - pose = obstacle.pose.pose.pose - robot.team = self.team_mapping[obstacle.type] - robot.position.x = pose.position.x - robot.position.y = pose.position.y - robot.position.z = self.extract_orientation_rotation_angle(pose.orientation) - - message.others.append(robot) - message.other_robot_confidence.append(obstacle.pose.confidence) + if seen_robots 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_rotation_angle(pose.orientation) + + message.others.append(robot) + message.other_robot_confidence.append(seen_robot.confidence.confidence) return message - def convert_strategy(strategy: Strategy, strategy_time: Time, message: Message): + def convert_strategy(strategy: Strategy, strategy_time: Time, message: Proto.Message): if strategy and is_still_valid_checker(strategy_time): message.role = self.role_mapping[strategy.role] message.action = self.action_mapping[strategy.action] @@ -120,7 +119,7 @@ def calculate_time_to_ball(current_pose: PoseWithCovarianceStamped, ball_positio return ball_distance / walking_speed def convert_time_to_ball(time_to_ball: float, time_to_ball_time: Time, ball_position: PointStamped, - current_pose: PoseWithCovarianceStamped, walking_speed: float, message: Message): + current_pose: PoseWithCovarianceStamped, walking_speed: float, message: Proto.Message): if time_to_ball 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): @@ -138,7 +137,7 @@ def convert_time_to_ball(time_to_ball: float, time_to_ball_time: Time, ball_posi 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_obstacles_to_robots(state.obstacles, 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) @@ -159,7 +158,7 @@ def extract_orientation_rotation_angle(self, quaternion: Quaternion): 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: fmat3, row_major_covariance: List[double]): + 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] 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 73130ae..010f6c6 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 @@ -11,18 +11,18 @@ 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, ObstacleRelative, ObstacleRelativeArray, Strategy, TeamData +from humanoid_league_msgs.msg import GameState, Strategy, TeamData from numpy import double from rclpy.duration import Duration from rclpy.node import Node from rclpy.time import Time +from soccer_vision_3d_msgs.msg import Robot, RobotArray from std_msgs.msg import Float32, Header from tf2_geometry_msgs import PointStamped, PoseStamped +import humanoid_league_team_communication.robocup_extension_pb2 as Proto from humanoid_league_team_communication.communication import SocketCommunication -from humanoid_league_team_communication.robocup_extension_pb2 import Message -from humanoid_league_team_communication.converter.robocup_protocol_converter import RobocupProtocolConverter - +from humanoid_league_team_communication.converter.robocup_protocol_converter import TeamColor, RobocupProtocolConverter class HumanoidLeagueTeamCommunication: @@ -30,14 +30,17 @@ def __init__(self): self._package_path = get_package_share_directory("humanoid_league_team_communication") self.node = Node("team_comm", automatically_declare_parameters_from_overrides=True) self.logger = self.node.get_logger() - self.protocol_converter = RobocupProtocolConverter() self.logger.info("Initializing humanoid_league_team_communication...") - - params_blackboard = get_parameters_from_other_node(self.node, "parameter_blackboard", ['bot_id', 'team_id']) + 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 = self.node.get_parameter('rate').get_parameter_value().integer_value @@ -78,7 +81,7 @@ def set_state_defaults(self): self.strategy_time = Time(nanoseconds=0, clock_type=self.node.get_clock().clock_type) self.time_to_ball: float = None self.time_to_ball_time = Time(nanoseconds=0, clock_type=self.node.get_clock().clock_type) - self.obstacles: ObstacleRelativeArray = None + self.seen_robots: RobotArray = None self.move_base_goal: PoseStamped = None def try_to_establish_connection(self): @@ -100,7 +103,7 @@ def create_subscribers(self): 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(ObstacleRelativeArray, self.topics['obstacle_topic'], self.obstacle_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): @@ -124,20 +127,20 @@ def time_to_ball_cb(self, msg: float): def move_base_goal_cb(self, msg: PoseStamped): self.move_base_goal = msg - def obstacle_cb(self, msg: ObstacleRelativeArray): + def robots_cb(self, msg: RobotArray): - def transform_to_map(obstacle: ObstacleRelative): - obstacle_pose = PoseStamped(header=msg.header, pose=obstacle.pose.pose.pose) + def transform_to_map(robot_relative: Robot): + robot_pose = PoseStamped(header=msg.header, pose=robot_relative.bb.center) try: - obstacle_map = self.tf_transform(obstacle_pose) - obstacle.pose.pose.pose = obstacle_map.pose - return obstacle + robot_map = self.tf_transform(robot_pose) + robot_relative.bb.center = robot_map.pose + return robot_relative except tf2_ros.TransformException: - self.logger.error("TeamComm: Could not transform obstacle to map frame") + self.logger.error("TeamComm: Could not transform robot to map frame") - self.obstacles = ObstacleRelativeArray(header=msg.header) - self.obstacles.header.frame_id = self.map_frame - self.obstacles.obstacles = list(map(transform_to_map, msg.obstacles)) + robots_on_map = list(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): ball_point = PointStamped(header=msg.header, point=msg.pose.pose.position) @@ -165,7 +168,7 @@ def receive_forever(self): self.handle_message(message) def handle_message(self, string_message: bytes): - message = Message() + message = Proto.Message() message.ParseFromString(string_message) if self.should_message_be_discarded(message): @@ -188,8 +191,8 @@ def send_message(self): message = self.protocol_converter.convert_to_message(self, msg, is_still_valid) self.socket_communication.send_message(message.SerializeToString()) - def create_empty_message(self, now: Time) -> Message: - message = Message() + 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 @@ -201,7 +204,7 @@ def create_team_data(self) -> TeamData: def create_header_with_own_time(self) -> Header: return Header(stamp=self.get_current_time().to_msg(), frame_id=self.map_frame) - def should_message_be_discarded(self, message: Message) -> bool: + def should_message_be_discarded(self, message: Proto.Message) -> bool: player_id = message.current_pose.player_id team_id = message.current_pose.team diff --git a/humanoid_league_team_communication/scripts/test_team_comm.py b/humanoid_league_team_communication/scripts/test_team_comm.py index 33a5d88..5ac09a6 100755 --- a/humanoid_league_team_communication/scripts/test_team_comm.py +++ b/humanoid_league_team_communication/scripts/test_team_comm.py @@ -6,7 +6,9 @@ import rclpy import numpy from geometry_msgs.msg import Point, Pose, PoseWithCovariance, PoseWithCovarianceStamped, Quaternion -from humanoid_league_msgs.msg import GameState, ObstacleRelativeArray, ObstacleRelative, Strategy +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 def pose_with_covariance(x, y, z=0.0): @@ -41,7 +43,7 @@ def covariance_list(): 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) - obstacle_pub = node.create_publisher(ObstacleRelativeArray, "obstacles_relative", 1) + robots_pub = node.create_publisher(RobotArray, "robots_relative", 1) gamestate_msg = GameState(penalized=False) strategy_msg = Strategy(role=Strategy.ROLE_DEFENDER, @@ -49,33 +51,35 @@ def covariance_list(): 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) - obstacle_msg = ObstacleRelativeArray() - - obstacle = ObstacleRelative() - obstacle.pose.pose.pose.position.x = 4.0 - obstacle.pose.pose.pose.position.y = 5.0 - obstacle.type = ObstacleRelative.ROBOT_CYAN - obstacle_msg.obstacles.append(obstacle) - - obstacle2 = ObstacleRelative() - obstacle2.pose.pose.pose.position.x = 1.0 - obstacle2.pose.pose.pose.position.y = 2.0 - obstacle2.type = ObstacleRelative.ROBOT_MAGENTA - obstacle_msg.obstacles.append(obstacle2) + 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(): now = node.get_clock().now().to_msg() gamestate_msg.header.stamp = now position_msg.header.stamp = now - obstacle_msg.header.stamp = now + robots_msg.header.stamp = now ball_msg.header.stamp = now # @TODO: check if we should use another map frame ball_msg.header.frame_id = "map" - obstacle_msg.header.frame_id = "map" + robots_msg.header.frame_id = "map" gamestate_pub.publish(gamestate_msg) strategy_pub.publish(strategy_msg) position_pub.publish(position_msg) ball_pub.publish(ball_msg) - obstacle_pub.publish(obstacle_msg) \ No newline at end of file + robots_pub.publish(robots_msg) 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 index 2fdd5e7..c3f131b 100644 --- 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 @@ -33,22 +33,6 @@ }) # --- # name: test_setup_of_mappings - tuple( - tuple( - 0, - 1, - ), - tuple( - 1, - 3, - ), - tuple( - 2, - 2, - ), - ) -# --- -# name: test_setup_of_mappings.1 tuple( tuple( 0, @@ -80,7 +64,7 @@ ), ) # --- -# name: test_setup_of_mappings.2 +# name: test_setup_of_mappings.1 tuple( tuple( 0, @@ -116,7 +100,7 @@ ), ) # --- -# name: test_setup_of_mappings.3 +# name: test_setup_of_mappings.2 tuple( tuple( 0, @@ -136,6 +120,13 @@ ), ) # --- +# 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({ @@ -164,9 +155,9 @@ 3: 3, }), 'team_mapping': dict({ - 1: 0, + 0: 0, + 1: 1, 2: 2, - 3: 1, }), }) # --- 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 index d7391df..751b673 100644 --- 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 @@ -92,7 +92,7 @@ ''' # --- -# name: test_convert_obstacles_to_robots +# name: test_convert_seen_robots ''' [player_id: 2 position { @@ -117,7 +117,7 @@ ] ''' # --- -# name: test_convert_obstacles_to_robots.1 +# name: test_convert_seen_robots.1 list([ 0.800000011920929, 0.800000011920929, 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 index 0e147b7..b0b945a 100644 --- 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 @@ -1,11 +1,12 @@ +import humanoid_league_team_communication.robocup_extension_pb2 as Proto import pytest -from humanoid_league_team_communication.converter.robocup_protocol_converter import RobocupProtocolConverter -from humanoid_league_team_communication.robocup_extension_pb2 import * +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): @@ -53,21 +54,21 @@ def test_convert_strategy(message_with_strategy): assert team_data.strategy.offensive_side == Strategy.SIDE_RIGHT -def convert_from_message(message: Message, team_data=TeamData()): - return RobocupProtocolConverter().convert_from_message(message, team_data) +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) -> Message: - message.role = Role.ROLE_STRIKER - message.action = Action.ACTION_KICKING - message.offensive_side = OffensiveSide.SIDE_RIGHT +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) -> Message: +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)) @@ -75,7 +76,7 @@ def message_with_other_robots(message) -> Message: @pytest.fixture -def message_with_ball(message) -> Message: +def message_with_ball(message) -> Proto.Message: set_position(message.ball.position) set_covariance_matrix(message.ball.covariance) @@ -83,7 +84,7 @@ def message_with_ball(message) -> Message: @pytest.fixture -def message_with_current_pose(message) -> Message: +def message_with_current_pose(message) -> Proto.Message: message.current_pose.player_id = 3 message.current_pose.team = own_team_id @@ -103,12 +104,12 @@ def team_data_with_header() -> TeamData: @pytest.fixture -def message() -> Message: - return Message() +def message() -> Proto.Message: + return Proto.Message() def robot_message(player_id, is_own_team): - robot = Robot() + robot = Proto.Robot() robot.player_id = player_id robot.team = own_team_id if not is_own_team: @@ -120,13 +121,13 @@ def robot_message(player_id, is_own_team): return robot -def set_position(position: fvec3): +def set_position(position: Proto.fvec3): position.x = 3.6 position.y = 1.8 position.z = 1.9 -def set_covariance_matrix(covariance: fmat3): +def set_covariance_matrix(covariance: Proto.fmat3): covariance.x.x = 1.2 covariance.x.y = 2.2 covariance.x.z = 3.2 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 index 12e7fa1..c8f3d64 100644 --- a/humanoid_league_team_communication/test/converter/test_robocup_protocol_converter.py +++ b/humanoid_league_team_communication/test/converter/test_robocup_protocol_converter.py @@ -1,19 +1,30 @@ 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 +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 = RobocupProtocolConverter() - assert converter.team_mapping == 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): - RobocupProtocolConverter() + protocol_converter() to_message_mock.assert_called_once() args = to_message_mock.mock_calls[0].kwargs @@ -21,7 +32,7 @@ def test_setup_of_to_message_converter(snapshot, to_message_mock): def test_setup_of_from_message_converter(snapshot, from_message_mock): - RobocupProtocolConverter() + protocol_converter() from_message_mock.assert_called_once() args = from_message_mock.mock_calls[0].kwargs @@ -32,12 +43,16 @@ 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 = RobocupProtocolConverter() + 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( 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 index 3320e7e..449c74c 100644 --- 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 @@ -1,21 +1,25 @@ from unittest.mock import Mock +import humanoid_league_team_communication.robocup_extension_pb2 as Proto import numpy import pytest -from std_msgs.msg import Header from geometry_msgs.msg import (Point, PointStamped, Pose, PoseStamped, PoseWithCovariance, PoseWithCovarianceStamped, Quaternion, Twist, Vector3) -from humanoid_league_team_communication.converter.robocup_protocol_converter import RobocupProtocolConverter +from humanoid_league_team_communication.converter.robocup_protocol_converter import TeamColor, RobocupProtocolConverter from humanoid_league_team_communication.humanoid_league_team_communication import HumanoidLeagueTeamCommunication -from humanoid_league_team_communication.robocup_extension_pb2 import * 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, ObstacleRelative, ObstacleRelativeArray, PoseWithCertainty, Strategy +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) @@ -29,23 +33,23 @@ def test_convert_gamestate(state_with_gamestate): gamestate.penalized = False result = convert_to_message(state_with_gamestate) - assert result.state == State.UNPENALISED + assert result.state == Proto.State.UNPENALISED gamestate.penalized = True result = convert_to_message(state_with_gamestate) - assert result.state == State.PENALISED + 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 = Message() - message.state = State.UNPENALISED + message = Proto.Message() + message.state = Proto.State.UNPENALISED result = convert_to_message(state_with_gamestate, message, is_state_expired=True) - assert result.state == State.UNKNOWN_STATE + assert result.state == Proto.State.UNKNOWN_STATE def test_convert_current_pose(snapshot, state_with_current_pose): @@ -58,7 +62,7 @@ def test_convert_current_pose(snapshot, state_with_current_pose): def test_convert_current_pose_expired_headers(state_with_current_pose): - message = Message() + message = Proto.Message() message.current_pose.position.x = 3.0 result = convert_to_message(state_with_current_pose, message, is_state_expired=True) @@ -80,7 +84,7 @@ def test_convert_walk_command(state_with_walk_command): def test_convert_walk_command_expired_headers(state_with_walk_command): - message = Message() + message = Proto.Message() message.walk_command.x = 3.0 result = convert_to_message(state_with_walk_command, message, is_state_expired=True) @@ -99,7 +103,7 @@ def test_convert_target_pose(snapshot, state_with_target_pose): def test_convert_target_pose_expired_headers(state_with_target_pose): - message = Message() + message = Proto.Message() message.target_pose.position.x = 3.0 result = convert_to_message(state_with_target_pose, message, is_state_expired=True) @@ -125,22 +129,22 @@ def test_convert_ball_position_expired_headers(state_with_ball_pose): assert result.ball.covariance.z.z == 100 -def test_convert_obstacles_to_robots(snapshot, state_with_obstacles): - result = convert_to_message(state_with_obstacles) +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_obstacles.obstacles.header.stamp) + validity_checker_valid.assert_called_with(state_with_seen_robots.seen_robots.header.stamp) -def test_convert_obstacles_to_robots_expired_headers(state_with_obstacles): - robot = Robot() +def test_convert_obstacles_to_robots_expired_headers(state_with_seen_robots): + robot = Proto.Robot() robot.player_id = 2 - message = Message() + message = Proto.Message() message.others.append(robot) - result = convert_to_message(state_with_obstacles, message, is_state_expired=True) + 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 @@ -149,22 +153,22 @@ def test_convert_obstacles_to_robots_expired_headers(state_with_obstacles): def test_convert_strategy(state_with_strategy): result = convert_to_message(state_with_strategy) - assert result.role == Role.ROLE_STRIKER - assert result.action == Action.ACTION_KICKING - assert result.offensive_side == OffensiveSide.SIDE_RIGHT + 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 = Message() - message.role = Role.ROLE_DEFENDER + message = Proto.Message() + message.role = Proto.Role.ROLE_DEFENDER result = convert_to_message(state_with_strategy, message, is_state_expired=True) - assert result.role == Role.ROLE_DEFENDER - assert result.action == Action.ACTION_UNDEFINED - assert result.offensive_side == OffensiveSide.SIDE_UNDEFINED + 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): @@ -185,7 +189,7 @@ def test_convert_time_to_ball_calculated(snapshot, state_with_ball_and_robot_pos def test_convert_time_to_ball_expired_headers(state_with_time_to_ball): - message = Message() + message = Proto.Message() message.time_to_ball = 16.5 result = convert_to_message(state_with_time_to_ball, message, is_state_expired=True) @@ -193,10 +197,10 @@ def test_convert_time_to_ball_expired_headers(state_with_time_to_ball): assert pytest.approx(result.time_to_ball) == 9999.0 -def convert_to_message(team_data_state, message: Message = None, is_state_expired=False): - message = message if message else Message() +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().convert_to_message(team_data_state, message, validity_checker) + return RobocupProtocolConverter(own_team_color).convert_to_message(team_data_state, message, validity_checker) @pytest.fixture @@ -225,15 +229,13 @@ def state_with_strategy(state): @pytest.fixture -def state_with_obstacles(state): - state.obstacles = Mock(ObstacleRelativeArray) - state.obstacles.header = Header() - state.obstacles.obstacles = [ - relative_obstacle(ObstacleRelative.ROBOT_CYAN, 2), - relative_obstacle(ObstacleRelative.ROBOT_MAGENTA, 3), - relative_obstacle(ObstacleRelative.ROBOT_UNDEFINED, 4), - relative_obstacle(ObstacleRelative.HUMAN), - relative_obstacle(ObstacleRelative.POLE), +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 @@ -303,7 +305,7 @@ def state(): state.ball = None state.ball_velocity = None state.ball_covariance = None - state.obstacles = None + state.seen_robots = None state.strategy = None state.strategy_time = None state.time_to_ball = None @@ -313,24 +315,30 @@ def state(): return state -def relative_obstacle(obstacle_type, player_number=None): - obstacle = Mock(ObstacleRelative) - obstacle.type = obstacle_type - obstacle.player_number = player_number - obstacle.pose = PoseWithCertainty(pose=pose_with_covariance(), confidence=0.8) +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 obstacle + return robot -def pose_with_covariance(): - 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) - pose = PoseWithCovariance(pose=Pose(position=point, orientation=quat)) +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 From 472c66ee07e932add27294963b4a4fdd67a677f9 Mon Sep 17 00:00:00 2001 From: texhnolyze Date: Tue, 31 Jan 2023 21:25:02 +0100 Subject: [PATCH 11/17] refactor(team_comm): communication code review intoducing handling of truncated messages if a protobuf message should be larger than 1024 bytes, we then double the buffer size for the next iteration --- .../config/team_communication_config.yaml | 8 ++-- .../communication.py | 41 ++++++++++++++----- 2 files changed, 35 insertions(+), 14 deletions(-) diff --git a/humanoid_league_team_communication/config/team_communication_config.yaml b/humanoid_league_team_communication/config/team_communication_config.yaml index 4416686..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 diff --git a/humanoid_league_team_communication/humanoid_league_team_communication/communication.py b/humanoid_league_team_communication/humanoid_league_team_communication/communication.py index c62ae26..ec8d93f 100644 --- a/humanoid_league_team_communication/humanoid_league_team_communication/communication.py +++ b/humanoid_league_team_communication/humanoid_league_team_communication/communication.py @@ -1,4 +1,6 @@ +from ipaddress import IPv4Address import socket +from typing import List, Optional from rclpy.node import Node @@ -8,17 +10,18 @@ class SocketCommunication: def __init__(self, node: Node, logger, team_id, robot_id): self.logger = logger - self.socket: socket.socket = None - self.target_host = node.get_parameter('target_host').get_parameter_value().string_value + 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_host == '127.0.0.1': - # local mode, bind to port depending on bot id and team id - local_target_ports = node.get_parameter('local_target_ports').get_parameter_value().integer_array_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 = node.get_parameter('target_port').get_parameter_value().integer_value - receive_port = node.get_parameter('receive_port').get_parameter_value().integer_value + 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 @@ -44,10 +47,26 @@ def close_connection(self): self.socket.close() self.logger.info("Connection closed.") - def receive_message(self) -> bytes: - return self.socket.recv(1024) + 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_host}') - self.socket.sendto(message, (self.target_host, port)) + 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" From 06c24320e6b14eaab9ec227385f68b86f257b9ec Mon Sep 17 00:00:00 2001 From: texhnolyze Date: Tue, 31 Jan 2023 21:27:38 +0100 Subject: [PATCH 12/17] refactor(team_comm): code review optional typings and other smaller fixes --- .../message_to_team_data_converter.py | 1 - .../converter/state_to_message_converter.py | 51 +++++++++---------- .../humanoid_league_team_communication.py | 34 ++++++------- 3 files changed, 42 insertions(+), 44 deletions(-) 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 index 4ac805a..9dbf7f7 100644 --- 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 @@ -48,7 +48,6 @@ def convert_strategy(self, message: Proto.Message, team_data: TeamData) -> TeamD 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) 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 index a018892..cd4c4ea 100644 --- 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 @@ -1,7 +1,7 @@ import math import transforms3d from numpy import double -from typing import Callable, List, Tuple +from typing import Callable, List, Optional, Tuple from rclpy.time import Time from geometry_msgs.msg import PointStamped, PoseStamped, PoseWithCovarianceStamped, Quaternion, Twist @@ -21,22 +21,22 @@ def __init__(self, team_mapping, role_mapping, action_mapping, side_mapping): def convert(self, state, message: Proto.Message, is_still_valid_checker: Callable[[Time], bool]) -> Proto.Message: - def convert_gamestate(gamestate: GameState, message: Proto.Message): - if gamestate and is_still_valid_checker(gamestate.header.stamp): + 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: PoseWithCovarianceStamped, message: Proto.Message): - if current_pose and is_still_valid_checker(current_pose.header.stamp): + 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_rotation_angle(pose.orientation) + 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: @@ -44,26 +44,26 @@ def convert_current_pose(current_pose: PoseWithCovarianceStamped, message: Proto return message - def convert_walk_command(walk_command: Twist, walk_command_time: Time, message: Proto.Message): - if walk_command and is_still_valid_checker(walk_command_time): + 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: PoseStamped, message): - if target_position and is_still_valid_checker(target_position.header.stamp): + 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_rotation_angle(pose.orientation) + message.target_pose.position.z = self.extract_orientation_yaw_angle(pose.orientation) return message - def convert_ball_position(ball_position: PointStamped, ball_velocity: Tuple[float, float, float], + def convert_ball_position(ball_position: Optional[PointStamped], ball_velocity: Tuple[float, float, float], ball_covariance: List[double], message): - if ball_position and is_still_valid_checker(ball_position.header.stamp): + 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 @@ -78,9 +78,8 @@ def convert_ball_position(ball_position: PointStamped, ball_velocity: Tuple[floa return message - def convert_seen_robots(seen_robots: RobotArray, message: Proto.Message): - # @TODO: should confidence be decreased in else case? - if seen_robots and is_still_valid_checker(seen_robots.header.stamp): + 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() @@ -90,25 +89,25 @@ def convert_seen_robots(seen_robots: RobotArray, message: Proto.Message): 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_rotation_angle(pose.orientation) + 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: Strategy, strategy_time: Time, message: Proto.Message): - if strategy and is_still_valid_checker(strategy_time): + 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: PoseWithCovarianceStamped, - ball_position: PointStamped) -> bool: - return (ball_position and is_still_valid_checker(ball_position.header.stamp) and current_pose and - is_still_valid_checker(current_pose.header.stamp)) + 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: @@ -118,9 +117,9 @@ def calculate_time_to_ball(current_pose: PoseWithCovarianceStamped, ball_positio return ball_distance / walking_speed - def convert_time_to_ball(time_to_ball: float, time_to_ball_time: Time, ball_position: PointStamped, + 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 and is_still_valid_checker(time_to_ball_time): + 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) @@ -150,7 +149,7 @@ def default_to_large_covariance(self, message_property, value=100): message_property.covariance.y.y = value message_property.covariance.z.z = value - def extract_orientation_rotation_angle(self, quaternion: Quaternion): + def extract_orientation_yaw_angle(self, quaternion: Quaternion): angles = self.convert_to_euler(quaternion) theta = angles[2] return theta 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 010f6c6..b42e768 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 @@ -24,6 +24,7 @@ from humanoid_league_team_communication.communication import SocketCommunication from humanoid_league_team_communication.converter.robocup_protocol_converter import TeamColor, RobocupProtocolConverter + class HumanoidLeagueTeamCommunication: def __init__(self): @@ -43,12 +44,12 @@ def __init__(self): 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 = 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.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 self.topics = get_parameter_dict(self.node, 'topics') - self.map_frame = self.node.get_parameter('map_frame').get_parameter_value().string_value + self.map_frame: str = self.node.get_parameter('map_frame').value self.create_publishers() self.create_subscribers() @@ -70,19 +71,19 @@ def run_spin_in_thread(self): thread.start() def set_state_defaults(self): - self.gamestate: GameState = None - self.pose: PoseWithCovarianceStamped = None - self.cmd_vel: Twist = None + 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: Optional[PointStamped] = None self.ball_velocity: Tuple[float, float, float] = (0, 0, 0) - self.ball_covariance: List[double] = None - self.strategy: Strategy = None + 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: float = 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.seen_robots: RobotArray = None - self.move_base_goal: PoseStamped = None + 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 @@ -172,9 +173,8 @@ def handle_message(self, string_message: bytes): message.ParseFromString(string_message) if self.should_message_be_discarded(message): - self.logger.info("Discarding msg by player {} in team {} at {}".format(message.current_pose.player_id, - message.current_pose.team, - message.timestamp.seconds)) + 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 = self.protocol_converter.convert_from_message(message, self.create_team_data()) @@ -182,7 +182,7 @@ def handle_message(self, string_message: bytes): def send_message(self): if not self.is_robot_allowed_to_send_message(): - self.logger.info("Not allowed to send message") + self.logger.debug("Not allowed to send message") return now = self.get_current_time() @@ -214,7 +214,7 @@ def should_message_be_discarded(self, message: Proto.Message) -> bool: return isOwnMessage or isMessageFromOpositeTeam def is_robot_allowed_to_send_message(self) -> bool: - return self.gamestate and not self.gamestate.penalized + return self.gamestate is not None and not self.gamestate.penalized def get_current_time(self) -> Time: return self.node.get_clock().now() From bee6fb34902f0860e3086ba69f2af5b0ee4162fa Mon Sep 17 00:00:00 2001 From: texhnolyze Date: Tue, 21 Feb 2023 19:27:28 +0100 Subject: [PATCH 13/17] fix(team_comm): transform to "map" frame in test script and improve error messages for transform errors --- .../message_to_team_data_converter.py | 1 - .../converter/robocup_protocol_converter.py | 3 +-- .../humanoid_league_team_communication.py | 26 +++++++++---------- .../scripts/test_team_comm.py | 22 ++++++++++++---- 4 files changed, 31 insertions(+), 21 deletions(-) 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 index 9dbf7f7..316caa0 100644 --- 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 @@ -52,7 +52,6 @@ def convert_robots_to_obstacles(self, message_robots: List[Proto.Robot], obstacle = ObstacleRelative(player_number=robot.player_id, type=self.team_mapping[robot.team]) obstacle.pose.pose = self.convert_robot_pose(robot) - # @TODO: are lists in python/protobuf always sorted and can confidences be empty? if index < len(message_robot_confidence): obstacle.pose.confidence = message_robot_confidence[index] 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 index cf24c68..7af091f 100644 --- 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 @@ -48,7 +48,6 @@ def __init__(self, own_team_color: TeamColor): Proto.Team.BLUE: ObstacleRelative.ROBOT_CYAN, Proto.Team.RED: ObstacleRelative.ROBOT_MAGENTA } - # @TODO: check if RobotAttributes.TEAM_OWN is based on the robot seen or robot running 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, @@ -71,4 +70,4 @@ def __init__(self, own_team_color: TeamColor): } self.convert_from_message = MessageToTeamDataConverter(**proto_to_team_data_mappings).convert - self.convert_to_message = StateToMessageConverter(**state_to_proto_mappings).convert \ No newline at end of file + self.convert_to_message = StateToMessageConverter(**state_to_proto_mappings).convert 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 b42e768..7b3ceec 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 @@ -7,7 +7,6 @@ from typing import List, Optional, Tuple import rclpy -import tf2_ros 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 @@ -19,6 +18,7 @@ from soccer_vision_3d_msgs.msg import Robot, RobotArray from std_msgs.msg import Float32, Header from tf2_geometry_msgs import PointStamped, PoseStamped +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 @@ -56,8 +56,8 @@ def __init__(self): self.set_state_defaults() - self.tf_buffer = tf2_ros.Buffer() - self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self.node) + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self.node) self.try_to_establish_connection() self.run_spin_in_thread() @@ -131,31 +131,31 @@ def move_base_goal_cb(self, msg: PoseStamped): 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: - robot_map = self.tf_transform(robot_pose) - robot_relative.bb.center = robot_map.pose + robot_on_map = self.transform_to_map_frame(robot_pose) + robot_relative.bb.center = robot_on_map.pose return robot_relative - except tf2_ros.TransformException: - self.logger.error("TeamComm: Could not transform robot to map frame") + except TransformException as err: + self.logger.error(f"Could not transform robot to map frame: {err}") - robots_on_map = list(map(transform_to_map, msg.robots)) + 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): ball_point = PointStamped(header=msg.header, point=msg.pose.pose.position) try: - self.ball = self.tf_transform(ball_point) + 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 + 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 tf_transform(self, field, timeout_in_ns=0.3e9): + 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): diff --git a/humanoid_league_team_communication/scripts/test_team_comm.py b/humanoid_league_team_communication/scripts/test_team_comm.py index 5ac09a6..0dc24a6 100755 --- a/humanoid_league_team_communication/scripts/test_team_comm.py +++ b/humanoid_league_team_communication/scripts/test_team_comm.py @@ -5,10 +5,11 @@ import rclpy import numpy -from geometry_msgs.msg import Point, Pose, PoseWithCovariance, PoseWithCovarianceStamped, Quaternion +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): @@ -35,9 +36,18 @@ def covariance_list(): 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") + tf_static_broadcaster = StaticTransformBroadcaster(node) gamestate_pub = node.create_publisher(GameState, "gamestate", 1) strategy_pub = node.create_publisher(Strategy, "strategy", 1) @@ -45,6 +55,7 @@ def covariance_list(): 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, @@ -69,17 +80,18 @@ def covariance_list(): while rclpy.ok(): 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 - # @TODO: check if we should use another map frame - ball_msg.header.frame_id = "map" - robots_msg.header.frame_id = "map" + ball_msg.header.frame_id = "base_footprint" + 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) ball_pub.publish(ball_msg) - robots_pub.publish(robots_msg) + robots_pub.publish(robots_msg) \ No newline at end of file From 6901e4a87e379aa7e6eae2450da6c886cc551c46 Mon Sep 17 00:00:00 2001 From: texhnolyze Date: Tue, 21 Feb 2023 19:29:19 +0100 Subject: [PATCH 14/17] chore(team_comm): depend on base.launch for startup --- humanoid_league_team_communication/launch/team_comm.launch | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) 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 @@ + - + + + + + From a5f3cf81566402404ec9e528457a2643c4527750 Mon Sep 17 00:00:00 2001 From: texhnolyze Date: Tue, 21 Feb 2023 19:57:34 +0100 Subject: [PATCH 15/17] chore(team_comm): update maintainer/version --- humanoid_league_team_communication/package.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/humanoid_league_team_communication/package.xml b/humanoid_league_team_communication/package.xml index d263ade..b170ff3 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 From 939f7a4e4e1a1fbe2822bdf84c049d2a7d71130e Mon Sep 17 00:00:00 2001 From: texhnolyze Date: Thu, 23 Feb 2023 16:52:42 +0100 Subject: [PATCH 16/17] style(team_comm): autoformat files with yapf --- .../communication.py | 3 +- .../message_to_team_data_converter.py | 1 + .../humanoid_league_team_communication.py | 52 +++++++++++-------- 3 files changed, 33 insertions(+), 23 deletions(-) diff --git a/humanoid_league_team_communication/humanoid_league_team_communication/communication.py b/humanoid_league_team_communication/humanoid_league_team_communication/communication.py index ec8d93f..8b58b54 100644 --- a/humanoid_league_team_communication/humanoid_league_team_communication/communication.py +++ b/humanoid_league_team_communication/humanoid_league_team_communication/communication.py @@ -58,7 +58,8 @@ def receive_message(self) -> Optional[bytes]: 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.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}") 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 index 316caa0..16d0370 100644 --- 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 @@ -24,6 +24,7 @@ def convert(self, message: Proto.Message, team_data: TeamData) -> TeamData: 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 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 7b3ceec..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 @@ -34,22 +34,22 @@ def __init__(self): 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'] + ["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 + 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 - self.topics = get_parameter_dict(self.node, 'topics') - self.map_frame: str = self.node.get_parameter('map_frame').value + 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() @@ -93,19 +93,23 @@ def try_to_establish_connection(self): rclpy.spin_once(self.node) def create_publishers(self): - self.team_data_publisher = 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_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) + 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 @@ -153,7 +157,11 @@ def ball_cb(self, msg: PoseWithCovarianceStamped): 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) + self.ball_velocity = ( + msg.twist.twist.linear.x, + msg.twist.twist.linear.y, + msg.twist.twist.angular.z, + ) 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)) @@ -225,5 +233,5 @@ def main(): HumanoidLeagueTeamCommunication() -if __name__ == '__main__': +if __name__ == "__main__": main() From 51ef3ad07aa1338635d9ab2db228dcd91160ae0c Mon Sep 17 00:00:00 2001 From: texhnolyze Date: Thu, 23 Feb 2023 16:53:09 +0100 Subject: [PATCH 17/17] build(team_comm): add pytest dependency --- humanoid_league_team_communication/package.xml | 2 ++ humanoid_league_team_communication/setup.py | 8 +++++--- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/humanoid_league_team_communication/package.xml b/humanoid_league_team_communication/package.xml index b170ff3..5ab762a 100644 --- a/humanoid_league_team_communication/package.xml +++ b/humanoid_league_team_communication/package.xml @@ -29,6 +29,8 @@ python3-protobuf python3-transforms3d + python3-pytest + unknown 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'], + } ) +