def __init__(self):
self._OUR_COLOR = rospy.get_param('consai2_description/our_color', 'blue')
self._OUR_SIDE = rospy.get_param('consai2_description/our_side', 'left')
self._sub_ball_info = rospy.Subscriber(
'vision_wrapper/ball_info',
BallInfo,
self._callback_ball_info)
self._sub_referee = rospy.Subscriber(
'referee_receiver/raw_referee',
Referee,
self._callback_referee)
self._pub_decoded_referee = rospy.Publisher(
'~decoded_referee', DecodedReferee, queue_size=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
self._prev_referee = Referee()
self._prev_decoded_msg = DecodedReferee()
self._ball_pose = Pose2D()
self._stationary_ball_pose = Pose2D()
self._game_is_inplay = False
#!/usr/bin/env python2
# coding: UTF-8
# 複数台のロボットを同時に動かすサンプルプログラム
import rospy
from consai2_msgs.msg import ControlTarget, DecodedReferee
from std_msgs.msg import Bool
from geometry_msgs.msg import Pose2D
import math
decoded_referee = DecodedReferee()
def callback_decodedref(msg):
global decoded_referee
decoded_referee = msg
class CollectiveController(object):
def __init__(self):
self._ID_NUM = 16
self._is_busy = False
self._move_forward = True
self._is_arrived = []
self._control_target = []
self._pubs_control_target = []
def __init__(self):
QUEUE_SIZE = 1
self._FAR_DISTANCE = 1e+10
self._OUR_COLOR = rospy.get_param('consai2_description/our_color', 'blue')
self._MAX_ID = rospy.get_param('consai2_description/max_id', 15)
self._GOALIE_ID = rospy.get_param('consai2_description/goalie_id', 0)
self._THEIR_COLOR = 'yellow'
if self._OUR_COLOR == 'yellow':
self._THEIR_COLOR = 'blue'
self._robot_node = []
for robot_id in range(self._MAX_ID + 1):
self._robot_node.append(RobotNode(robot_id))
# ゴーリーを割り当てる
# ゴーリーはconsai2起動時のみにしか変更しない
if robot_id == self._GOALIE_ID:
self._robot_node[robot_id].set_goalie()
self._roledecision = role.RoleDecision(self._MAX_ID, self._GOALIE_ID)
self._sub_geometry = rospy.Subscriber(
'vision_receiver/raw_vision_geometry', VisionGeometry,
self._callback_geometry, queue_size=1)
self._decoded_referee = DecodedReferee()
self._sub_decoded_referee = rospy.Subscriber(
'referee_wrapper/decoded_referee', DecodedReferee,
self._callback_referee, queue_size=1)
self._ball_info = BallInfo()
self._sub_ball_info = rospy.Subscriber(
'vision_wrapper/ball_info', BallInfo,
self._callback_ball_info, queue_size=1)
self._robot_info = {'our':[],'their':[]}
self._subs_robot_info = {'our':[],'their':[]}
self._pubs_control_target = []
for robot_id in range(self._MAX_ID + 1):
# 末尾に16進数の文字列をつける
topic_id = hex(robot_id)[2:]
self._robot_info['our'].append(RobotInfo())
self._robot_info['their'].append(RobotInfo())
topic_name = 'vision_wrapper/robot_info_' + self._OUR_COLOR + '_' + topic_id
sub_robot_info = rospy.Subscriber(topic_name, RobotInfo,
self._callback_our_info, queue_size=QUEUE_SIZE,
callback_args=robot_id)
self._subs_robot_info['our'].append(sub_robot_info)
topic_name = 'vision_wrapper/robot_info_' + self._THEIR_COLOR + '_' + topic_id
sub_robot_info = rospy.Subscriber(topic_name, RobotInfo,
self._callback_their_info, queue_size=QUEUE_SIZE,
callback_args=robot_id)
self._subs_robot_info['their'].append(sub_robot_info)
topic_name = 'consai2_game/control_target_' + self._OUR_COLOR+'_' + topic_id
pub_control_target = rospy.Publisher(topic_name, ControlTarget,
queue_size=1)
self._pubs_control_target.append(pub_control_target)
# 障害物回避のためのクラス
self._obstacle_avoidance = avoidance.ObstacleAvoidance()
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
# 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
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
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_kick_ball = False
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.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
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.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 = True
decoded_msg.keep_out_radius_from_ball = 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.speed_limit_of_robot = self._NO_LIMIT
decoded_msg.can_kick_ball = True
decoded_msg.can_enter_their_side = False
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._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.speed_limit_of_robot = self._NO_LIMIT
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 # No limit but robot do not touch the ball
decoded_msg.keep_out_distance_from_their_defense_area = self._NO_LIMIT
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.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
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.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._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.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._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.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._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.speed_limit_of_robot = self._NO_LIMIT
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 = 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.speed_limit_of_robot = self._NO_LIMIT
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 = 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["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.referee_text += "(INPLAY)"
return decoded_msg