def __init__(self):
# ボール情報の初期化
self._ball_info = BallInfo()
# ロボット情報の初期化
self._robot_info = RobotInfo()
# 障害物検出の幅[m]
self._range_tr_y = 0.5
self._range_tr_x = 0.2
# 経路生成時の相対的に移動する位置(ロボットから見た座標系)
self._tr_move_x = 0.2
self._tr_move_y = 0.5
def __init__(self):
QUEUE_SIZE = 10
self._DISAPPERED_TIME_THRESH = 3.0
self._PUBLISH_ROBOT = {'blue': False, 'yellow': False}
self._MAX_ID = rospy.get_param('consai2_description/max_id', 15)
self._PUBLISH_BALL = rospy.get_param('~publish_ball', True)
self._PUBLISH_ROBOT['blue'] = rospy.get_param('~publish_blue', True)
self._PUBLISH_ROBOT['yellow'] = rospy.get_param(
'~publish_yellow', False)
self._pub_ball_info = None
self._ball_info = BallInfo()
if self._PUBLISH_BALL:
self._pub_ball_info = rospy.Publisher('~ball_info',
BallInfo,
queue_size=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 = rospy.Publisher(topic_name,
RobotInfo,
queue_size=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 = rospy.Publisher(topic_name,
RobotInfo,
queue_size=QUEUE_SIZE)
self._pubs_robot_info['yellow'].append(pub_robot_info)
self._sub_detections = rospy.Subscriber(
'vision_receiver/raw_vision_detections', VisionDetections,
self._callback_detections)
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 __init__(self, parent=None):
super(PaintWidget, self).__init__(parent)
self._WHITE_LINE_THICKNESS = 2 # 白線の太さ
self._ZOOM_RATE = 0.1 # 拡大縮小率
self._SCALE_LIMIT = 0.2 # 縮小率の限界値
self._ALPHA_DETECTED = 255 # 検出できたロボット・ボールの透明度
self._ALPHA_NOT_DETECTED = 127 # 検出できなかったロボット・ボールの透明度
self._COLOR_BALL = QColor(Qt.red)
self._COLOR_ROBOT = {
'blue': QColor(Qt.cyan),
'yellow': QColor(Qt.yellow)
}
self._ID_POS = (0.15, 0.15) # IDを描画するロボット中心からの位置
self._FLAG_POS = (0.15, 0) # control_targetのフラグを描画するロボット中心からの位置
# Replace
self._REPLACE_CLICK_POS_THRESHOLD = 0.1
self._REPLACE_CLICK_VEL_ANGLE_THRESHOLD = self._REPLACE_CLICK_POS_THRESHOLD + 0.1
self._REPLACE_BALL_VELOCITY_GAIN = 3.0
self._REPLACE_MAX_BALL_VELOCITY = 8.0
self._BALL_RADIUS = rospy.get_param('consai2_description/ball_radius',
0.0215)
self._ROBOT_RADIUS = rospy.get_param(
'consai2_description/robot_radius', 0.09)
self._MAX_ID = rospy.get_param('consai2_description/max_id', 15)
self._SIDE = rospy.get_param('consai2_description/our_side', 'left')
# チームサイドの反転
self._invert_side = False
if self._SIDE != 'left':
self._invert_side = True
# GUIパラメータ
self._trans = QPointF(0.0, 0.0) # x, y方向の移動
self._mouse_trans = QPointF(0.0, 0.0) # マウス操作による移動
self._scale = QPointF(1.0, 1.0) # 拡大, 縮小
self._do_rotate_view = False # 90度回転判定
self._view_height = self.height() # 描画サイズ（縦）
self._view_width = self.width() # 描画サイズ（横）
self._scale_field_to_view = 1.0 # フィールドから描画領域に縮小するスケール
self._click_point = QPointF(0.0, 0.0) # マウスでクリックした描画座標
self._current_mouse_pos = QPointF(0.0, 0.0) # マウスカーソル位置
self._replace_func = None
self._replace_id = 0
self._replace_is_yellow = False
self._do_replacement = False
self._replacement_target = {
'ball_pos': False,
'ball_vel': False,
'robot_pos': False,
'robot_angle': False
}
# フィールド形状
# raw_vision_geometryの値で更新される
self._field_length = 9.0
self._field_width = 6.0
self._field_goal_width = 1.0
self._field_goal_depth = 0.2
self._field_boundary_width = 0.3
self._field_lines = []
self._field_arcs = []
# ジョイスティック情報
self._joy_target = ControlTarget()
# ロボット・ボール情報
self._ball_info = BallInfo()
self._robot_info = {'blue': [], 'yellow': []}
# レフェリー情報
self._decoded_referee = None
# Publisher
self._pub_replace = rospy.Publisher('sim_sender/replacements',
Replacements,
queue_size=1)
# Subscribers
self._sub_decoded_referee = rospy.Subscriber(
'referee_wrapper/decoded_referee',
DecodedReferee,
self._callback_referee,
queue_size=1)
self._sub_geometry = rospy.Subscriber(
'vision_receiver/raw_vision_geometry',
VisionGeometry,
self._callback_geometry,
queue_size=1)
self._sub_ball_info = rospy.Subscriber('vision_wrapper/ball_info',
BallInfo,
self._callback_ball_info,
queue_size=1)
self._sub_joy_target = rospy.Subscriber('consai2_examples/joy_target',
ControlTarget,
self._callback_joy_target,
queue_size=1)
self._subs_robot_info = {'blue': [], 'yellow': []}
self._control_targets = {'blue': [], 'yellow': []}
self._subs_control_target = {'blue': [], 'yellow': []}
for robot_id in range(self._MAX_ID + 1):
self._robot_info['blue'].append(RobotInfo())
self._robot_info['yellow'].append(RobotInfo())
self._control_targets['blue'].append(ControlTarget())
self._control_targets['yellow'].append(ControlTarget())
# 末尾に16進数の文字列をつける
topic_id = hex(robot_id)[2:]
topic_name = 'vision_wrapper/robot_info_blue_' + topic_id
self._subs_robot_info['blue'].append(
rospy.Subscriber(topic_name,
RobotInfo,
self._callback_blue_info,
callback_args=robot_id))
topic_name = 'vision_wrapper/robot_info_yellow_' + topic_id
self._subs_robot_info['yellow'].append(
rospy.Subscriber(topic_name,
RobotInfo,
self._callback_yellow_info,
callback_args=robot_id))
topic_name = 'consai2_game/control_target_blue_' + topic_id
self._subs_control_target['blue'].append(
rospy.Subscriber(topic_name,
ControlTarget,
self._callback_blue_target,
callback_args=robot_id))
topic_name = 'consai2_game/control_target_blue_' + topic_id
self._subs_control_target['blue'].append(
rospy.Subscriber(topic_name,
ControlTarget,
self._callback_blue_target,
callback_args=robot_id))
topic_name = 'consai2_game/control_target_yellow_' + topic_id
self._subs_control_target['yellow'].append(
rospy.Subscriber(topic_name,
ControlTarget,
self._callback_yellow_target,
callback_args=robot_id))
# Configs
# This function enables mouse tracking without pressing mouse button
self.setMouseTracking(True)