diff --git a/consai2r2_msgs/CMakeLists.txt b/consai2r2_msgs/CMakeLists.txt index eefb16b..330d72f 100644 --- a/consai2r2_msgs/CMakeLists.txt +++ b/consai2r2_msgs/CMakeLists.txt @@ -29,6 +29,10 @@ set(msg_files "msg/DetectionRobot.msg" "msg/DetectionFrame.msg" "msg/VisionDetections.msg" + "msg/RobotInfo.msg" + "msg/BallInfo.msg" + "msg/DecodedReferee.msg" + "msg/ReplaceBall.msg" "msg/RobotCommand.msg" "msg/RobotCommands.msg" ) diff --git a/consai2r2_msgs/msg/BallInfo.msg b/consai2r2_msgs/msg/BallInfo.msg new file mode 100755 index 0000000..6a268cd --- /dev/null +++ b/consai2r2_msgs/msg/BallInfo.msg @@ -0,0 +1,10 @@ +# ball information for consai2_game + +geometry_msgs/Pose2D pose +geometry_msgs/Pose2D velocity +geometry_msgs/Twist velocity_twist +bool detected +builtin_interfaces/Time detection_stamp + +geometry_msgs/Pose2D last_detection_pose +bool disappeared diff --git a/consai2r2_msgs/msg/DecodedReferee.msg b/consai2r2_msgs/msg/DecodedReferee.msg new file mode 100755 index 0000000..b8002dd --- /dev/null +++ b/consai2r2_msgs/msg/DecodedReferee.msg @@ -0,0 +1,39 @@ +# game status for consai2_game + +# Declare of the constants of referee_id +uint8 ID_HALT = 0 +uint8 ID_STOP = 1 +uint8 ID_FORCE_START = 3 +uint8 ID_OUR_KICKOFF_PREPARATION = 11 +uint8 ID_OUR_KICKOFF_START = 12 +uint8 ID_OUR_PENALTY_PREPARATION = 13 +uint8 ID_OUR_PENALTY_START = 14 +uint8 ID_OUR_DIRECT_FREE = 15 +uint8 ID_OUR_INDIRECT_FREE = 16 +uint8 ID_OUR_TIMEOUT = 17 +uint8 ID_OUR_GOAL = 18 +uint8 ID_OUR_BALL_PLACEMENT = 19 +uint8 ID_THEIR_KICKOFF_PREPARATION = 21 +uint8 ID_THEIR_KICKOFF_START = 22 +uint8 ID_THEIR_PENALTY_PREPARATION = 23 +uint8 ID_THEIR_PENALTY_START = 24 +uint8 ID_THEIR_DIRECT_FREE = 25 +uint8 ID_THEIR_INDIRECT_FREE = 26 +uint8 ID_THEIR_TIMEOUT = 27 +uint8 ID_THEIR_GOAL = 28 +uint8 ID_THEIR_BALL_PLACEMENT = 29 + + +uint8 referee_id +string referee_text + +bool can_move_robot +float64 speed_limit_of_robot # meter/sec +bool can_kick_ball +bool can_enter_their_side +bool can_enter_center_circle +bool is_inplay +float64 keep_out_radius_from_ball # meter +float64 keep_out_distance_from_their_defense_area # meter + +geometry_msgs/Point placement_position diff --git a/consai2r2_msgs/msg/ReplaceBall.msg b/consai2r2_msgs/msg/ReplaceBall.msg new file mode 100755 index 0000000..546df12 --- /dev/null +++ b/consai2r2_msgs/msg/ReplaceBall.msg @@ -0,0 +1,8 @@ +# This msg file is copied from 'grSim_Replacement.proto' +# +float64 x +float64 y +float64 vx +float64 vy + +bool is_enabled diff --git a/consai2r2_msgs/msg/RobotInfo.msg b/consai2r2_msgs/msg/RobotInfo.msg new file mode 100755 index 0000000..fa1b49f --- /dev/null +++ b/consai2r2_msgs/msg/RobotInfo.msg @@ -0,0 +1,29 @@ +# robot information for consai2_game + +# robot id +uint8 robot_id + +# Robot 2D pose +# This field will be replaced to geometry_msgs/Pose +geometry_msgs/Pose2D pose + +# Robot Velocity +# [Deprecated] This field will be removed. +geometry_msgs/Pose2D velocity + +# Robot Velocity +geometry_msgs/Twist velocity_twist + +# If robot is detected by vision in this frame, this field is set to true +bool detected + +# Latest vision detection time +# [Deprecated] This field will be removed. +builtin_interfaces/Time detection_stamp + +# Latest detected pose. +# This field is NOT filtered. This is a raw vision value. +geometry_msgs/Pose2D last_detection_pose + +# If a robot disappear from field, this field is set to true +bool disappeared diff --git a/consai2r2_referee_wrapper/consai2r2_referee_wrapper/__init__.py b/consai2r2_referee_wrapper/consai2r2_referee_wrapper/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/consai2r2_referee_wrapper/consai2r2_referee_wrapper/referee_wrapper.py b/consai2r2_referee_wrapper/consai2r2_referee_wrapper/referee_wrapper.py new file mode 100755 index 0000000..834d81a --- /dev/null +++ b/consai2r2_referee_wrapper/consai2r2_referee_wrapper/referee_wrapper.py @@ -0,0 +1,392 @@ +# Copyright (c) 2019 SSL-Roots +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import math + +from consai2r2_description import consai2r2_parameters +from consai2r2_msgs.msg import BallInfo, DecodedReferee, Referee +from geometry_msgs.msg import Pose2D +import rclpy +from rclpy.node import Node +from referee_pb2 import SSL_Referee + +REFEREE_TEXT = { + 0: 'HALT', 1: 'STOP', 3: 'FORCE_START', + 11: 'OUR_KICKOFF_PREPARATION', 12: 'OUR_KICKOFF_START', + 13: 'OUR_PENALTY_PREPARATION', 14: 'OUR_PENALTY_START', + 15: 'OUR_DIRECT_FREE', 16: 'OUR_INDIRECT_FREE', + 17: 'OUR_TIMEOUT', 18: 'OUR_GOAL', 19: 'OUR_BALL_PLACEMENT', + 21: 'THEIR_KICKOFF_PREPARATION', 22: 'THEIR_KICKOFF_START', + 23: 'THEIR_PENALTY_PREPARATION', 24: 'THEIR_PENALTY_START', + 25: 'THEIR_DIRECT_FREE', 26: 'THEIR_INDIRECT_FREE', + 27: 'THEIR_TIMEOUT', 28: 'THEIR_GOAL', 29: 'THEIR_BALL_PLACEMENT', +} +REFEREE_ID = {v: k for k, v in REFEREE_TEXT.items()} + + +class RefereeWrapper(Node): + + def __init__(self): + super().__init__('consai2r2_referee_wrapper') + + common_params = consai2r2_parameters(self) + self._OUR_COLOR = common_params.our_color + self._OUR_SIDE = common_params.our_side + + self._sub_ball_info = self.create_subscription( + BallInfo, 'consai2r2_vision_wrapper/ball_info', self._callback_ball_info, 1) + self._sub_referee = self.create_subscription( + Referee, 'consai2r2_referee_receiver/raw_referee', self._callback_referee, 1) + + self._pub_decoded_referee = self.create_publisher(DecodedReferee, '~/decoded_referee', 1) + + self._DECODE_ID = { + 'OUR': 10, 'THEIR': 20, + 'HALT': 0, 'STOP': 1, 'FORCE_START': 3, # 定数の代わりに定義 + 'KICKOFF_PREPARATION': 1, 'KICKOFF_START': 2, + 'PENALTY_PREPARATION': 3, 'PENALTY_START': 4, + 'DIRECT_FREE': 5, 'INDIRECT_FREE': 6, + 'TIMEOUT': 7, 'GOAL': 8, 'BALL_PLACEMENT': 9, + } + + self._ID_BLUE = self._DECODE_ID['OUR'] + self._ID_YELLOW = self._DECODE_ID['THEIR'] + + if self._OUR_COLOR != 'blue': + self._ID_BLUE = self._DECODE_ID['THEIR'] + self._ID_YELLOW = self._DECODE_ID['OUR'] + + self._INPLAY_DISTANCE = 0.05 # meter + + self._SPEED_LIMIT_OF_ROBOT = 1.5 # meters / sec + self._SPEED_LIMIT_OF_BALL = 6.5 # meters / sec + self._KEEP_OUT_RADIUS_FROM_BALL = 0.5 # meters + self._KEEP_OUT_DISTANCE_FROM_THEIR_DEFENSE_AREA = 0.2 # meters + self._NO_LIMIT = -1.0 + + self._prev_referee = Referee() + self._prev_decoded_msg = DecodedReferee() + + self._ball_pose = Pose2D() + self._stationary_ball_pose = Pose2D() + self._game_is_inplay = False + + def _callback_ball_info(self, msg): + self._ball_pose = msg.pose + + def _callback_referee(self, msg): + # Refereeのデータをチームカラーに合わせて解釈する + + decoded_msg = self._decode_referee(msg) + + self._pub_decoded_referee.publish(decoded_msg) + + # NORMAL_STARTの解釈に前回のコマンドが必要 + self._prev_referee = msg + self._prev_decoded_msg = decoded_msg + + def _decode_referee_id(self, referee_command): + decoded_id = 0 + + # HALT, STOP, FORCE_STARTはチームカラーに依存しない + if referee_command == SSL_Referee.HALT: + decoded_id = SSL_Referee.HALT + elif referee_command == SSL_Referee.STOP: + decoded_id = SSL_Referee.STOP + elif referee_command == SSL_Referee.FORCE_START: + decoded_id = SSL_Referee.FORCE_START + elif referee_command == SSL_Referee.NORMAL_START: + # 複数回IDに加算するのを防ぐため、前回のコマンドと比較する + if self._prev_referee.command != SSL_Referee.NORMAL_START: + # PREPARATIONのIDに1を加えたものがSTARTのIDになる + decoded_id = self._prev_decoded_msg.referee_id + 1 + else: + decoded_id = self._prev_decoded_msg.referee_id + else: + # チームカラーに合わせてコマンドの解釈を変える + if referee_command == SSL_Referee.PREPARE_KICKOFF_YELLOW: + decoded_id = self._ID_YELLOW + self._DECODE_ID['KICKOFF_PREPARATION'] + if referee_command == SSL_Referee.PREPARE_KICKOFF_BLUE: + decoded_id = self._ID_BLUE + self._DECODE_ID['KICKOFF_PREPARATION'] + if referee_command == SSL_Referee.PREPARE_PENALTY_YELLOW: + decoded_id = self._ID_YELLOW + self._DECODE_ID['PENALTY_PREPARATION'] + if referee_command == SSL_Referee.PREPARE_PENALTY_BLUE: + decoded_id = self._ID_BLUE + self._DECODE_ID['PENALTY_PREPARATION'] + if referee_command == SSL_Referee.DIRECT_FREE_YELLOW: + decoded_id = self._ID_YELLOW + self._DECODE_ID['DIRECT_FREE'] + if referee_command == SSL_Referee.DIRECT_FREE_BLUE: + decoded_id = self._ID_BLUE + self._DECODE_ID['DIRECT_FREE'] + if referee_command == SSL_Referee.INDIRECT_FREE_YELLOW: + decoded_id = self._ID_YELLOW + self._DECODE_ID['INDIRECT_FREE'] + if referee_command == SSL_Referee.INDIRECT_FREE_BLUE: + decoded_id = self._ID_BLUE + self._DECODE_ID['INDIRECT_FREE'] + if referee_command == SSL_Referee.TIMEOUT_YELLOW: + decoded_id = self._ID_YELLOW + self._DECODE_ID['TIMEOUT'] + if referee_command == SSL_Referee.TIMEOUT_BLUE: + decoded_id = self._ID_BLUE + self._DECODE_ID['TIMEOUT'] + if referee_command == SSL_Referee.GOAL_YELLOW: + decoded_id = self._ID_YELLOW + self._DECODE_ID['GOAL'] + if referee_command == SSL_Referee.GOAL_BLUE: + decoded_id = self._ID_BLUE + self._DECODE_ID['GOAL'] + if referee_command == SSL_Referee.BALL_PLACEMENT_YELLOW: + decoded_id = self._ID_YELLOW + self._DECODE_ID['BALL_PLACEMENT'] + if referee_command == SSL_Referee.BALL_PLACEMENT_BLUE: + decoded_id = self._ID_BLUE + self._DECODE_ID['BALL_PLACEMENT'] + + return decoded_id + + def _decode_referee(self, msg): + decoded_msg = DecodedReferee() + + decoded_msg.referee_id = self._decode_referee_id(msg.command) + decoded_msg.referee_text = REFEREE_TEXT.get(decoded_msg.referee_id, 'INVALID_COMMAND') + + # サイドチェンジに対応する + decoded_msg.placement_position = msg.designated_position + if self._OUR_SIDE != 'left': + decoded_msg.placement_position.x = -decoded_msg.placement_position.x + decoded_msg.placement_position.y = -decoded_msg.placement_position.y + + # Default Settings + decoded_msg.can_move_robot = False + decoded_msg.speed_limit_of_robot = self._NO_LIMIT + decoded_msg.can_kick_ball = False + decoded_msg.can_enter_their_side = False + decoded_msg.can_enter_center_circle = False + decoded_msg.is_inplay = False + decoded_msg.keep_out_radius_from_ball = self._NO_LIMIT + decoded_msg.keep_out_distance_from_their_defense_area = self._NO_LIMIT + + # Decode restrictions + if decoded_msg.referee_id == self._DECODE_ID['HALT'] \ + or decoded_msg.referee_id == self._DECODE_ID['OUR'] + self._DECODE_ID['GOAL'] \ + or decoded_msg.referee_id == self._DECODE_ID['THEIR'] + self._DECODE_ID['GOAL']: + # Reference : Rule 2019, 5.1.2 Halt + pass # Keep default settings + + elif decoded_msg.referee_id == self._DECODE_ID['STOP']: + # Reference : Rule 2019, 5.1.1 Stop + decoded_msg.can_move_robot = True + decoded_msg.speed_limit_of_robot = self._SPEED_LIMIT_OF_ROBOT + decoded_msg.can_enter_their_side = True + decoded_msg.can_enter_center_circle = True + decoded_msg.keep_out_radius_from_ball = self._KEEP_OUT_RADIUS_FROM_BALL + decoded_msg.keep_out_distance_from_their_defense_area = \ + self._KEEP_OUT_DISTANCE_FROM_THEIR_DEFENSE_AREA + + elif decoded_msg.referee_id == self._DECODE_ID['FORCE_START']: + # Reference : Rule 2019, 5.3.5 Force Start + # Reference : Rule 2019, 8.1.6 Ball Speed + decoded_msg.can_move_robot = True + decoded_msg.can_kick_ball = True + decoded_msg.can_enter_their_side = True + decoded_msg.can_enter_center_circle = True + decoded_msg.is_inplay = True + + elif decoded_msg.referee_id == self._DECODE_ID['OUR'] \ + + self._DECODE_ID['KICKOFF_PREPARATION']: + # Reference : Rule 2019, 5.3.2 Kick-Off + decoded_msg.can_move_robot = True + decoded_msg.can_enter_center_circle = True + decoded_msg.keep_out_radius_from_ball = 0.0 # No limit but robot do not touch the ball + decoded_msg.keep_out_distance_from_their_defense_area = \ + self._KEEP_OUT_DISTANCE_FROM_THEIR_DEFENSE_AREA + + elif decoded_msg.referee_id == self._DECODE_ID['OUR'] + self._DECODE_ID['KICKOFF_START']: + # Reference : Rule 2019, 5.3.1 Normal Start + # Reference : Rule 2019, 5.3.2 Kick-Off + decoded_msg.can_move_robot = True + decoded_msg.can_kick_ball = True + decoded_msg.can_enter_center_circle = True + decoded_msg.keep_out_distance_from_their_defense_area = \ + self._KEEP_OUT_DISTANCE_FROM_THEIR_DEFENSE_AREA + + elif decoded_msg.referee_id == self._DECODE_ID['OUR'] \ + + self._DECODE_ID['PENALTY_PREPARATION']: + # Reference : Rule 2019, 5.3.6 Penalty Kick + decoded_msg.can_move_robot = True + decoded_msg.can_kick_ball = False + decoded_msg.can_enter_their_side = True + decoded_msg.can_enter_center_circle = True + decoded_msg.keep_out_radius_from_ball = 0.0 # No limit but robot do not touch the ball + + elif decoded_msg.referee_id == self._DECODE_ID['OUR'] + self._DECODE_ID['PENALTY_START']: + # Reference : Rule 2019, 5.3.1 Normal Start + # Reference : Rule 2019, 5.3.6 Penalty Kick + decoded_msg.can_move_robot = True + decoded_msg.can_kick_ball = True + decoded_msg.can_enter_their_side = True + decoded_msg.can_enter_center_circle = True + + elif decoded_msg.referee_id == self._DECODE_ID['OUR'] + self._DECODE_ID['DIRECT_FREE'] \ + or decoded_msg.referee_id == self._DECODE_ID['OUR'] \ + + self._DECODE_ID['INDIRECT_FREE']: + # Reference : Rule 2019, 5.3.3 Direct Free Kick + # Reference : Rule 2019, 5.3.6 Indirect Free Kick + decoded_msg.can_move_robot = True + decoded_msg.can_kick_ball = True + decoded_msg.can_enter_their_side = True + decoded_msg.can_enter_center_circle = True + decoded_msg.keep_out_distance_from_their_defense_area = \ + self._KEEP_OUT_DISTANCE_FROM_THEIR_DEFENSE_AREA + + elif decoded_msg.referee_id == self._DECODE_ID['OUR'] + self._DECODE_ID['BALL_PLACEMENT']: + # Reference : Rule 2019, 5.2 Ball Placement + # Reference : Rule 2019, 8.2.8 Robot Stop Speed + decoded_msg.can_move_robot = True + decoded_msg.can_kick_ball = True + decoded_msg.can_enter_their_side = True + decoded_msg.can_enter_center_circle = True + decoded_msg.keep_out_distance_from_their_defense_area = \ + self._KEEP_OUT_DISTANCE_FROM_THEIR_DEFENSE_AREA + + elif decoded_msg.referee_id == self._DECODE_ID['THEIR'] \ + + self._DECODE_ID['KICKOFF_PREPARATION'] \ + or decoded_msg.referee_id == self._DECODE_ID['THEIR'] \ + + self._DECODE_ID['KICKOFF_START']: + # Reference : Rule 2019, 5.3.2 Kick-Off + decoded_msg.can_move_robot = True + decoded_msg.keep_out_radius_from_ball = self._KEEP_OUT_RADIUS_FROM_BALL + decoded_msg.keep_out_distance_from_their_defense_area = \ + self._KEEP_OUT_DISTANCE_FROM_THEIR_DEFENSE_AREA + + elif decoded_msg.referee_id == self._DECODE_ID['THEIR'] \ + + self._DECODE_ID['PENALTY_PREPARATION'] \ + or decoded_msg.referee_id == self._DECODE_ID['THEIR'] \ + + self._DECODE_ID['PENALTY_START']: + # Reference : Rule 2019, 5.3.6 Penalty Kick + decoded_msg.can_move_robot = True + decoded_msg.can_enter_their_side = True + decoded_msg.can_enter_center_circle = True + decoded_msg.keep_out_radius_from_ball = self._KEEP_OUT_RADIUS_FROM_BALL + decoded_msg.keep_out_distance_from_their_defense_area = \ + self._KEEP_OUT_DISTANCE_FROM_THEIR_DEFENSE_AREA + + elif decoded_msg.referee_id == self._DECODE_ID['THEIR'] + self._DECODE_ID['DIRECT_FREE'] \ + or decoded_msg.referee_id == self._DECODE_ID['THEIR'] \ + + self._DECODE_ID['INDIRECT_FREE'] \ + or decoded_msg.referee_id == self._DECODE_ID['THEIR'] \ + + self._DECODE_ID['BALL_PLACEMENT']: + # Reference : Rule 2019, 5.3.3 Direct Free Kick + # Reference : Rule 2019, 5.3.6 Indirect Free Kick + # Reference : Rule 2019, 8.2.3 Ball Placement Interference + decoded_msg.can_move_robot = True + decoded_msg.can_enter_their_side = True + decoded_msg.can_enter_center_circle = True + decoded_msg.keep_out_radius_from_ball = self._KEEP_OUT_RADIUS_FROM_BALL + decoded_msg.keep_out_distance_from_their_defense_area = \ + self._KEEP_OUT_DISTANCE_FROM_THEIR_DEFENSE_AREA + + elif decoded_msg.referee_id == self._DECODE_ID['OUR'] + self._DECODE_ID['TIMEOUT'] \ + or decoded_msg.referee_id == self._DECODE_ID['THEIR'] + self._DECODE_ID['TIMEOUT']: + # Reference : Rule 2019, 4.4.2 Timeouts + # No limitations + decoded_msg.can_move_robot = True + decoded_msg.speed_limit_of_robot = self._NO_LIMIT + decoded_msg.can_kick_ball = True + decoded_msg.can_enter_their_side = True + decoded_msg.can_enter_center_circle = True + decoded_msg.keep_out_radius_from_ball = self._NO_LIMIT + decoded_msg.keep_out_distance_from_their_defense_area = self._NO_LIMIT + + else: + decoded_msg.can_move_robot = False + decoded_msg.speed_limit_of_robot = self._NO_LIMIT + decoded_msg.can_kick_ball = False + decoded_msg.can_enter_their_side = False + decoded_msg.can_enter_center_circle = False + decoded_msg.keep_out_radius_from_ball = self._NO_LIMIT + decoded_msg.keep_out_distance_from_their_defense_area = self._NO_LIMIT + + # Consider inplay + # Reference : Rule 2019, 8.1.3 Double Touch + # Reference : Rule 2019, A.1 Ball In And Out Of Play + if decoded_msg.referee_id == self._DECODE_ID['HALT'] \ + or decoded_msg.referee_id == self._DECODE_ID['STOP'] \ + or decoded_msg.referee_id == self._DECODE_ID['OUR'] \ + + self._DECODE_ID['KICKOFF_PREPARATION'] \ + or decoded_msg.referee_id == self._DECODE_ID['THEIR'] \ + + self._DECODE_ID['KICKOFF_PREPARATION'] \ + or decoded_msg.referee_id == self._DECODE_ID['OUR'] \ + + self._DECODE_ID['PENALTY_PREPARATION'] \ + or decoded_msg.referee_id == self._DECODE_ID['THEIR'] \ + + self._DECODE_ID['PENALTY_PREPARATION'] \ + or decoded_msg.referee_id == self._DECODE_ID['OUR'] \ + + self._DECODE_ID['BALL_PLACEMENT'] \ + or decoded_msg.referee_id == self._DECODE_ID['THEIR'] \ + + self._DECODE_ID['BALL_PLACEMENT']: + self._stationary_ball_pose = self._ball_pose + self._game_is_inplay = False + + elif decoded_msg.referee_id == self._DECODE_ID['OUR'] \ + + self._DECODE_ID['KICKOFF_START'] \ + or decoded_msg.referee_id == self._DECODE_ID['OUR'] \ + + self._DECODE_ID['PENALTY_START'] \ + or decoded_msg.referee_id == self._DECODE_ID['OUR'] \ + + self._DECODE_ID['DIRECT_FREE'] \ + or decoded_msg.referee_id == self._DECODE_ID['OUR'] \ + + self._DECODE_ID['INDIRECT_FREE'] \ + or decoded_msg.referee_id == self._DECODE_ID['THEIR'] \ + + self._DECODE_ID['KICKOFF_START'] \ + or decoded_msg.referee_id == self._DECODE_ID['THEIR'] \ + + self._DECODE_ID['PENALTY_START'] \ + or decoded_msg.referee_id == self._DECODE_ID['THEIR'] \ + + self._DECODE_ID['DIRECT_FREE'] \ + or decoded_msg.referee_id == self._DECODE_ID['THEIR'] \ + + self._DECODE_ID['INDIRECT_FREE']: + + # ボールが静止位置から動いたかを判断する + if self._game_is_inplay is False: + diff_pose = Pose2D() + diff_pose.x = self._ball_pose.x - self._stationary_ball_pose.x + diff_pose.y = self._ball_pose.y - self._stationary_ball_pose.y + move_distance = math.hypot(diff_pose.x, diff_pose.y) + + if move_distance > self._INPLAY_DISTANCE: + self._game_is_inplay = True + + # インプレイのときは行動制限を解除する + if self._game_is_inplay is True: + decoded_msg.can_move_robot = True + decoded_msg.speed_limit_of_robot = self._NO_LIMIT + decoded_msg.can_kick_ball = True + decoded_msg.can_enter_their_side = True + decoded_msg.can_enter_center_circle = True + decoded_msg.keep_out_radius_from_ball = self._NO_LIMIT + decoded_msg.keep_out_distance_from_their_defense_area = self._NO_LIMIT + decoded_msg.is_inplay = True + + decoded_msg.referee_text += '(INPLAY)' + + return decoded_msg + + +def main(args=None): + rclpy.init(args=args) + wrapper = RefereeWrapper() + + rclpy.spin(wrapper) + + wrapper.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/consai2r2_referee_wrapper/package.xml b/consai2r2_referee_wrapper/package.xml new file mode 100755 index 0000000..ea62cfe --- /dev/null +++ b/consai2r2_referee_wrapper/package.xml @@ -0,0 +1,25 @@ + + + + consai2r2_referee_wrapper + 0.0.0 + Referee Wrapper + akshota + MIT + + rclpy + consai2r2_msgs + consai2r2_receiver + consai2r2_description + + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/consai2r2_referee_wrapper/resource/consai2r2_referee_wrapper b/consai2r2_referee_wrapper/resource/consai2r2_referee_wrapper new file mode 100755 index 0000000..e69de29 diff --git a/consai2r2_referee_wrapper/setup.cfg b/consai2r2_referee_wrapper/setup.cfg new file mode 100755 index 0000000..4824896 --- /dev/null +++ b/consai2r2_referee_wrapper/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/consai2r2_referee_wrapper +[install] +install-scripts=$base/lib/consai2r2_referee_wrapper diff --git a/consai2r2_referee_wrapper/setup.py b/consai2r2_referee_wrapper/setup.py new file mode 100755 index 0000000..df8d9dd --- /dev/null +++ b/consai2r2_referee_wrapper/setup.py @@ -0,0 +1,35 @@ +from setuptools import setup + +package_name = 'consai2r2_referee_wrapper' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + author='akshota', + author_email='macakasit@gmail.com', + maintainer='akshota', + maintainer_email='macakasit@gmail.com', + keywords=['ROS'], + classifiers=[ + 'Intended Audience :: Developers', + 'License :: OSI Approved :: MIT License', + 'Programming Language :: Python', + 'Topic :: Software Development', + ], + description='Translate raw referee message to AI-friendly message', + license='MIT License', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'referee_wrapper = consai2r2_referee_wrapper.referee_wrapper:main', + ], + }, +) diff --git a/consai2r2_referee_wrapper/test/test_copyright.py b/consai2r2_referee_wrapper/test/test_copyright.py new file mode 100755 index 0000000..aa74f3e --- /dev/null +++ b/consai2r2_referee_wrapper/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.']) + assert rc == 0, 'Found errors' diff --git a/consai2r2_referee_wrapper/test/test_flake8.py b/consai2r2_referee_wrapper/test/test_flake8.py new file mode 100755 index 0000000..eff8299 --- /dev/null +++ b/consai2r2_referee_wrapper/test/test_flake8.py @@ -0,0 +1,23 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc = main(argv=[]) + assert rc == 0, 'Found errors' diff --git a/consai2r2_referee_wrapper/test/test_pep257.py b/consai2r2_referee_wrapper/test/test_pep257.py new file mode 100755 index 0000000..399afc9 --- /dev/null +++ b/consai2r2_referee_wrapper/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/consai2r2_vision_wrapper/consai2r2_vision_wrapper/__init__.py b/consai2r2_vision_wrapper/consai2r2_vision_wrapper/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/consai2r2_vision_wrapper/consai2r2_vision_wrapper/vision_wrapper.py b/consai2r2_vision_wrapper/consai2r2_vision_wrapper/vision_wrapper.py new file mode 100755 index 0000000..9afc1d4 --- /dev/null +++ b/consai2r2_vision_wrapper/consai2r2_vision_wrapper/vision_wrapper.py @@ -0,0 +1,268 @@ +# Copyright (c) 2019 SSL-Roots +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import math + +from consai2r2_description import consai2r2_parameters + +from consai2r2_msgs.msg import BallInfo, RobotInfo, VisionDetections +from geometry_msgs.msg import Pose2D + +import rclpy +from rclpy.clock import Clock, ClockType +from rclpy.node import Node +from rclpy.time import Time + + +class VisionWrapper(Node): + + def __init__(self): + super().__init__('consai2r2_vision_wrapper') + + QUEUE_SIZE = 10 + self._DISAPPERED_TIME_THRESH = 3.0 + self._PUBLISH_ROBOT = {'blue': False, 'yellow': False} + + self.declare_parameter('publish_ball', True) + self.declare_parameter('publish_blue', True) + self.declare_parameter('publish_yellow', True) + + common_params = consai2r2_parameters(self) + self._OUR_COLOR = common_params.our_color + self._MAX_ID = common_params.max_id + + self._PUBLISH_BALL = self.get_parameter('publish_ball').value + self._PUBLISH_ROBOT['blue'] = self.get_parameter('publish_blue').value + self._PUBLISH_ROBOT['yellow'] = self.get_parameter( + 'publish_yellow').value + + self._pub_ball_info = None + self._ball_info = BallInfo() + + if self._PUBLISH_BALL: + self._pub_ball_info = self.create_publisher( + BallInfo, '~/ball_info', QUEUE_SIZE) + + self._robot_info = {'blue': [], 'yellow': []} + self._pubs_robot_info = {'blue': [], 'yellow': []} + + for robot_id in range(self._MAX_ID + 1): + # 末尾に16進数の文字列をつける + topic_id = hex(robot_id)[2:] + + self._robot_info['blue'].append(RobotInfo()) + self._robot_info['yellow'].append(RobotInfo()) + + if self._PUBLISH_ROBOT['blue']: + topic_name = '~/robot_info_blue_' + topic_id + pub_robot_info = self.create_publisher( + RobotInfo, topic_name, QUEUE_SIZE) + self._pubs_robot_info['blue'].append(pub_robot_info) + + if self._PUBLISH_ROBOT['yellow']: + topic_name = '~/robot_info_yellow_' + topic_id + pub_robot_info = self.create_publisher( + RobotInfo, topic_name, QUEUE_SIZE) + self._pubs_robot_info['yellow'].append(pub_robot_info) + + self.create_subscription( + VisionDetections, 'consai2r2_vision_receiver/raw_vision_detections', + self._callback_detections, 1) + + def _callback_detections(self, msg): + # Visionのデータからロボットとボールの位置を抽出する + + time_stamp = msg.header.stamp + + detection_balls = [] + detection_blues = [] + detection_yellows = [] + for frame in msg.frames: + for ball in frame.balls: + detection_balls.append(ball) + + for blue in frame.robots_blue: + detection_blues.append(blue) + + for yellow in frame.robots_yellow: + detection_yellows.append(yellow) + + self._extract_ball_pose(detection_balls, time_stamp) + self._extract_robot_pose('blue', detection_blues, time_stamp) + self._extract_robot_pose('yellow', detection_yellows, time_stamp) + + def _extract_ball_pose(self, detection_balls, time_stamp): + # ボール座標を抽出する + # ボール座標が複数ある場合は、平均値を座標とする + if detection_balls: + average_pose = self._average_pose(detection_balls) + velocity = self._velocity( + self._ball_info.pose, average_pose, self._ball_info.detection_stamp, time_stamp) + + self._ball_info.pose = average_pose + self._ball_info.last_detection_pose = average_pose + self._ball_info.velocity = velocity + self._ball_info.detected = True + self._ball_info.detection_stamp = time_stamp + self._ball_info.disappeared = False + else: + self._ball_info.detected = False + + if self._ball_info.disappeared is False: + # 座標を受け取らなかった場合は、速度を用いて線形予測する + + diff_time_stamp = Clock(clock_type=ClockType.ROS_TIME).now( + ) - Time.from_msg(self._ball_info.detection_stamp) + diff_time_secs = diff_time_stamp.nanoseconds / 1000000000 + + self._ball_info.pose = self._estimate( + self._ball_info.last_detection_pose, + self._ball_info.velocity, diff_time_secs) + + # 一定時間、座標を受け取らなかったら消滅判定にする + if diff_time_secs > self._DISAPPERED_TIME_THRESH: + self._ball_info.disappeared = True + + if self._PUBLISH_BALL: + self._pub_ball_info.publish(self._ball_info) + + def _extract_robot_pose(self, color, detection_robots, time_stamp): + # ロボット姿勢を抽出する + # ロボット姿勢が複数ある場合は、平均値を姿勢とする + detections = [[] for i in range(len(self._robot_info[color]))] + + # ID毎のリストに分ける + for robot in detection_robots: + robot_id = robot.robot_id + detections[robot_id].append(robot) + + for robot_id, robots in enumerate(detections): + if robots: + average_pose = self._average_pose(robots) + velocity = self._velocity( + self._robot_info[color][robot_id].pose, average_pose, + self._robot_info[color][robot_id].detection_stamp, time_stamp) + + self._robot_info[color][robot_id].robot_id = robot_id + self._robot_info[color][robot_id].pose = average_pose + self._robot_info[color][robot_id].last_detection_pose = average_pose + self._robot_info[color][robot_id].velocity = velocity + self._robot_info[color][robot_id].detected = True + self._robot_info[color][robot_id].detection_stamp = time_stamp + self._robot_info[color][robot_id].disappeared = False + else: + self._robot_info[color][robot_id].detected = False + self._robot_info[color][robot_id].robot_id = robot_id + + # 座標を受け取らなかった場合は、速度を用いて線形予測する + if self._robot_info[color][robot_id].disappeared is False: + duration = Clock(clock_type=ClockType.ROS_TIME).now() - \ + Time.from_msg(self._robot_info[color][robot_id].detection_stamp) + diff_time_secs = duration.nanoseconds / 1000000000 + + self._robot_info[color][robot_id].pose = self._estimate( + self._robot_info[color][robot_id].last_detection_pose, + self._robot_info[color][robot_id].velocity, diff_time_secs) + + # 一定時間、座標を受け取らなかったら消滅判定にする + if diff_time_secs > self._DISAPPERED_TIME_THRESH: + self._robot_info[color][robot_id].disappeared = True + + if self._PUBLISH_ROBOT[color]: + for robot_id in range(len(self._pubs_robot_info[color])): + self._pubs_robot_info[color][robot_id].publish( + self._robot_info[color][robot_id]) + + def _average_pose(self, detections): + # 姿勢の平均値を求める + # 角度をpi ~ -piに収めるため、極座標系に角度を変換している + + sum_pose = Pose2D() + detection_num = float(len(detections)) + + # 角度計算用 + sum_x = 0.0 + sum_y = 0.0 + for detection in detections: + sum_pose.x += detection.pose.x + sum_pose.y += detection.pose.y + sum_x += math.cos(detection.pose.theta) + sum_y += math.sin(detection.pose.theta) + sum_pose.x = sum_pose.x / detection_num + sum_pose.y = sum_pose.y / detection_num + sum_pose.theta = math.fmod(math.atan2(sum_y, sum_x), math.pi) + + return sum_pose + + def _velocity(self, prev_pose, current_pose, prev_stamp, current_stamp): + # 姿勢の差分から速度を求める + + velocity = Pose2D() + diff_time_stamp = Time.from_msg( + current_stamp) - Time.from_msg(prev_stamp) + diff_time_secs = diff_time_stamp.nanoseconds / 1000000000 + + if diff_time_secs > 0: + diff_pose = Pose2D() + diff_pose.x = current_pose.x - prev_pose.x + diff_pose.y = current_pose.y - prev_pose.y + diff_pose.theta = self._angle_normalize( + current_pose.theta - prev_pose.theta) + + velocity.x = diff_pose.x / diff_time_secs + velocity.y = diff_pose.y / diff_time_secs + velocity.theta = diff_pose.theta / diff_time_secs + + return velocity + + def _estimate(self, pose, velocity, diff_time_secs): + # 速度と時間をもとに、姿勢を線形予測する + + estimated_pose = Pose2D() + + estimated_pose.x = pose.x + velocity.x * diff_time_secs + estimated_pose.y = pose.y + velocity.y * diff_time_secs + estimated_pose.theta = pose.theta + velocity.theta * diff_time_secs + + return estimated_pose + + def _angle_normalize(self, angle): + # 角度をpi ~ -piの範囲に変換する + while angle > math.pi: + angle -= 2*math.pi + + while angle < -math.pi: + angle += 2*math.pi + + return angle + + +def main(args=None): + rclpy.init(args=args) + wrapper = VisionWrapper() + + rclpy.spin(wrapper) + + wrapper.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/consai2r2_vision_wrapper/package.xml b/consai2r2_vision_wrapper/package.xml new file mode 100755 index 0000000..76fd5e3 --- /dev/null +++ b/consai2r2_vision_wrapper/package.xml @@ -0,0 +1,25 @@ + + + + consai2r2_vision_wrapper + 0.0.0 + Vision Wrapper + akshota + MIT + + rclpy + consai2r2_msgs + consai2r2_receiver + consai2r2_description + + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/consai2r2_vision_wrapper/resource/consai2r2_vision_wrapper b/consai2r2_vision_wrapper/resource/consai2r2_vision_wrapper new file mode 100755 index 0000000..e69de29 diff --git a/consai2r2_vision_wrapper/setup.cfg b/consai2r2_vision_wrapper/setup.cfg new file mode 100755 index 0000000..53d8da0 --- /dev/null +++ b/consai2r2_vision_wrapper/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/consai2r2_vision_wrapper +[install] +install-scripts=$base/lib/consai2r2_vision_wrapper diff --git a/consai2r2_vision_wrapper/setup.py b/consai2r2_vision_wrapper/setup.py new file mode 100755 index 0000000..446ec7d --- /dev/null +++ b/consai2r2_vision_wrapper/setup.py @@ -0,0 +1,35 @@ +from setuptools import setup + +package_name = 'consai2r2_vision_wrapper' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + author='akshota', + author_email='macakasit@gmail.com', + maintainer='akshota', + maintainer_email='macakasit@gmail.com', + keywords=['ROS'], + classifiers=[ + 'Intended Audience :: Developers', + 'License :: OSI Approved :: MIT License', + 'Programming Language :: Python', + 'Topic :: Software Development', + ], + description='Convert raw vision message to AI-friendly message', + license='MIT License', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'vision_wrapper = consai2r2_vision_wrapper.vision_wrapper:main', + ], + }, +) diff --git a/consai2r2_vision_wrapper/test/test_copyright.py b/consai2r2_vision_wrapper/test/test_copyright.py new file mode 100755 index 0000000..aa74f3e --- /dev/null +++ b/consai2r2_vision_wrapper/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.']) + assert rc == 0, 'Found errors' diff --git a/consai2r2_vision_wrapper/test/test_flake8.py b/consai2r2_vision_wrapper/test/test_flake8.py new file mode 100755 index 0000000..eff8299 --- /dev/null +++ b/consai2r2_vision_wrapper/test/test_flake8.py @@ -0,0 +1,23 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc = main(argv=[]) + assert rc == 0, 'Found errors' diff --git a/consai2r2_vision_wrapper/test/test_pep257.py b/consai2r2_vision_wrapper/test/test_pep257.py new file mode 100755 index 0000000..399afc9 --- /dev/null +++ b/consai2r2_vision_wrapper/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.']) + assert rc == 0, 'Found code style errors / warnings'