def _update_interpose(self):
base_pose = WorldModel.get_pose(self._base)
target_pose = WorldModel.get_pose(self._target)
if base_pose is None or target_pose is None:
return False
angle_to_target = tool.getAngle(base_pose, target_pose)
interposed_pose = Pose(0, 0, 0)
if not self._to_dist is None:
trans = tool.Trans(base_pose, angle_to_target)
tr_interposed_pose = Pose(self._to_dist, 0.0, 0)
interposed_pose = trans.invertedTransform(tr_interposed_pose)
elif not self._from_dist is None:
angle_to_base = tool.getAngle(target_pose, base_pose)
trans = tool.Trans(target_pose, angle_to_base)
tr_interposed_pose = Pose(self._from_dist, 0.0, 0)
interposed_pose = trans.invertedTransform(tr_interposed_pose)
interposed_pose.theta = angle_to_target
self.pose = interposed_pose
return True
def getAngle(fromPose, toPose):
diffPose = Pose()
diffPose.x = toPose.x - fromPose.x
diffPose.y = toPose.y - fromPose.y
return math.atan2(diffPose.y, diffPose.x)
def getSize(fromPose, toPose):
diffPose = Pose()
diffPose.x = toPose.x - fromPose.x
diffPose.y = toPose.y - fromPose.y
return math.hypot(diffPose.x, diffPose.y)
def __init__(self):
super(PlayTheirPenaltyStart, self).__init__('PlayTheirPenaltyStart')
self.applicable = "THEIR_PENALTY_START"
self.done_aborted = "THEIR_PENALTY_START"
pose_x = -constants.FieldHalfX + 0.05
pose_y = constants.GoalHalfSize - constants.RobotRadius
pose1 = Pose(pose_x, pose_y, 0)
pose2 = Pose(pose_x, -pose_y, 0)
self.roles[0].loop_enable = True
self.roles[0].behavior.add_child(
TacticLookIntersection('TacticLookIntersection',
self.roles[0].my_role, target='Threat_0',
pose1 = pose1, pose2= pose2)
)
for i in range(1,6):
x = -3.0 + 0.3*i
y = 2.0
theta = -math.pi * 0.5
self.roles[i].loop_enable = True
self.roles[i].behavior.add_child(
TacticPosition('TacticPosition', self.roles[i].my_role,
x, y ,theta))
def invertedTransform(self, pose):
c_point = pose.x + pose.y * 1.0j
c_output = c_point * self._c_rotate + self._c_center
output = Pose()
output.x = c_output.real
output.y = c_output.imag
return output
def transform(self, pose):
c_point = pose.x + pose.y * 1.0j
c_output = (c_point - self._c_center) * numpy.conj(self._c_rotate)
output = Pose()
output.x = c_output.real
output.y = c_output.imag
return output
def __init__(self, name, my_role, base='CONST_OUR_GOAL', target='Ball',
pose1=Pose(-2,3,0), pose2=Pose(-2,-3,0)):
super(TacticIntersection, self).__init__(name)
self._coordinate = Coordinate()
self._coordinate.set_intersection(base=base, target=target,
pose1=pose1, pose2=pose2)
self.add_child(DynamicDrive('DynamicDrive', my_role, self._coordinate,
always_running = True))
def __init__(self, name='PlayStop'):
super(PlayStop, self).__init__(name)
self.applicable = "STOP"
self.done_aborted = "STOP"
keep_x = -constants.FieldHalfX + constants.RobotRadius * 2.0
self.roles[0].loop_enable = True
self.roles[0].behavior.add_child(
TacticGoalie('TacticGoalie',
self.roles[0].my_role,
keep_x=keep_x,
range_high=constants.GoalHalfSize,
range_low=-constants.GoalHalfSize))
self.roles[1].loop_enable = True
self.roles[1].behavior.add_child(
TacticInterpose('TacticInterpose',
self.roles[1].my_role,
from_dist=0.5))
self.roles[2].loop_enable = True
self.roles[2].behavior.add_child(
TacticInterpose('TacticInterpose',
self.roles[2].my_role,
to_dist=1.5))
range_y = constants.FieldHalfY - 0.7
self.roles[3].loop_enable = True
self.roles[3].behavior.add_child(
TacticKeep('TacticKeep',
self.roles[3].my_role,
keep_x=-2.0,
range_high=range_y,
range_low=0.5))
self.roles[4].loop_enable = True
self.roles[4].behavior.add_child(
TacticKeep('TacticKeep',
self.roles[4].my_role,
keep_x=-2.0,
range_high=-0.5,
range_low=-range_y))
pose1 = Pose(-2.5, range_y, 0)
pose2 = Pose(-2.5, -range_y, 0)
self.roles[5].loop_enable = True
self.roles[5].behavior.add_child(
TacticIntersection('TacticIntersection',
self.roles[5].my_role,
pose1=pose1,
pose2=pose2))
def _update_keep_y(self):
target_pose = WorldModel.get_pose(self._target)
if target_pose is None:
return False
keep_pose = Pose(target_pose.x, self._keep_y, 0.0)
keep_pose.x = tool.limit(keep_pose.x, self._range_x[0],
self._range_x[1])
angle = tool.getAngle(keep_pose, target_pose)
self.pose = Pose(keep_pose.x, keep_pose.y, angle)
return True
def get_intersection(pose1, pose2, pose3, pose4):
# get intersection of line1(pose1, pose2) and line2(pose3, pose4)
# reference:http://imagingsolution.blog107.fc2.com/blog-entry-137.html
s1 = ((pose4.x - pose3.x) * (pose1.y - pose3.y) \
- (pose4.y - pose3.y) * (pose1.x - pose3.x)) / 2.0
s2 = ((pose4.x - pose3.x) * (pose3.y - pose2.y) \
- (pose4.y - pose3.y) * (pose3.x - pose2.x)) / 2.0
coefficient = s1 / (s1 + s2)
output = Pose(0, 0, 0)
output.x = pose1.x + (pose2.x - pose1.x) * coefficient
output.y = pose1.y + (pose2.y - pose1.y) * coefficient
return output
def _update_receive_ball(self):
ball_pose = WorldModel.get_pose('Ball')
ball_vel = WorldModel.get_velocity('Ball')
result = False
if WorldModel.ball_is_moving():
angle_velocity = tool.getAngleFromCenter(ball_vel)
trans = tool.Trans(ball_pose, angle_velocity)
role_pose = WorldModel.get_pose(self._my_role)
if role_pose is None:
return False
tr_pose = trans.transform(role_pose)
fabs_y = math.fabs(tr_pose.y)
if self._receiving == False and \
fabs_y < self._can_receive_dist - self._can_receive_hysteresis:
self._receiving = True
elif self._receiving == True and \
fabs_y > self._can_receive_dist + self._can_receive_hysteresis:
self._receiving = False
if self._receiving and tr_pose.x > 0.0:
tr_pose.y = 0.0
inv_pose = trans.invertedTransform(tr_pose)
angle_to_ball = tool.getAngle(inv_pose, ball_pose)
self.pose = Pose(inv_pose.x, inv_pose.y, angle_to_ball)
result = True
return result
def __init__(self,
name,
my_role,
target='Enemy_1',
pose1=Pose(-2, 3, 0),
pose2=Pose(-2, -3, 0)):
super(TacticLookIntersection, self).__init__(name)
self._coordinate = Coordinate()
self._coordinate.set_look_intersection(target=target,
pose1=pose1,
pose2=pose2)
self.add_child(
DynamicDrive('DynamicDrive',
my_role,
self._coordinate,
always_running=True))
def __init__(self):
super(PlayInPlayOurDefence, self).__init__('PlayInPlayOurDefence')
self.applicable = "BALL_IN_OUR_DEFENCE"
self.done_aborted = "BALL_IN_OUR_DEFENCE"
self.roles[0].loop_enable = True
self.roles[0].behavior.add_child(
TacticClear('TacticClear', self.roles[0].my_role))
self.roles[1].loop_enable = True
self.roles[1].behavior.add_child(
TacticPosition('TacticPosition', self.roles[1].my_role, 0, 0, 0))
self.roles[2].loop_enable = True
self.roles[2].behavior.add_child(
TacticInterpose('TacticInterpose',
self.roles[2].my_role,
to_dist=1.5))
range_y = constants.FieldHalfY - 0.7
self.roles[3].loop_enable = True
self.roles[3].behavior.add_child(
TacticKeep('TacticKeep',
self.roles[3].my_role,
keep_x=-2.0,
range_high=range_y,
range_low=0.5))
self.roles[4].loop_enable = True
self.roles[4].behavior.add_child(
TacticKeep('TacticKeep',
self.roles[4].my_role,
keep_x=-2.0,
range_high=-0.5,
range_low=-range_y))
pose1 = Pose(-2.5, range_y, 0)
pose2 = Pose(-2.5, -range_y, 0)
self.roles[5].loop_enable = True
self.roles[5].behavior.add_child(
TacticIntersection('TacticIntersection',
self.roles[5].my_role,
pose1=pose1,
pose2=pose2))
def get_enemy_pose(cls, robot_id):
if robot_id is None:
return None
position = WorldModel._enemy_odoms[robot_id].pose.pose.position
orientation = WorldModel._enemy_odoms[robot_id].pose.pose.orientation
yaw = tool.yawFromQuaternion(orientation)
return Pose(position.x, position.y, yaw)
def __init__(self):
self.pose = Pose() # pos_x, pos_y, thta
self._base = None # string data
self._target = None # string data
self._update_func = None
self._range_x = [constants.FieldHalfX, -constants.FieldHalfX]
self._range_y = [constants.FieldHalfY, -constants.FieldHalfY]
# arrival parameters
self._arrived_position_tolerance = 0.1 # unit:meter
self._arrived_angle_tolerance = 3.0 * math.pi / 180.0
# interpose
self._to_dist = None
self._from_dist = None
# approach to shoot
self._pose_max = Pose()
self._my_role = None
self._role_is_lower_side = False
self._role_pose_hystersis = 0.1
self._tuning_param_x = 0.3
self._tuning_param_y = 0.3
self._tuning_param_pivot_y = 0.1
self._tuning_angle = 30.0 * math.pi / 180.0 # 0 ~ 90 degree, do not edit 'math.pi / 180.0'
# keep x, y
self._keep_x = 0.0
self._keep_y = 0.0
self._range_x = [constants.FieldHalfX, -constants.FieldHalfX]
self._range_y = [constants.FieldHalfY, -constants.FieldHalfY]
# intersection
self._pose1 = Pose(0, constants.FieldHalfY, 0)
self._pose2 = Pose(0, -constants.FieldHalfY, 0)
# receive_ball
self._can_receive_dist = 1.0 # unit:meter
self._can_receive_hysteresis = 0.3
self._receiving = False
def __init__(self):
self._hysteresis = 0.05 # unit:meter
self._moved_threshold = 0.03 # unit:meter
self._ball_initial_pose = Pose()
self._moving_speed_threshold = 1.0
self._moving_speed_hysteresis = 0.3
self._ball_is_in_field = False
self._ball_is_moved = False
self._ball_is_in_our_defence = False
self._ball_is_in_their_defence = False
self._ball_is_moving = False
def _update_look_intersection(self):
target_pose = WorldModel.get_pose(self._target)
if target_pose is None:
return False
target_theta = target_pose.theta
dist = 300 # フィールドサイズが300 mを超えたら書き直す必要あり
look_pose = Pose(dist * math.cos(target_theta),
dist * math.sin(target_theta), 0)
intersection = tool.get_intersection(look_pose, target_pose,
self._pose1, self._pose2)
angle = tool.getAngle(intersection, target_pose)
intersection.x = tool.limit(intersection.x, self._range_x[0],
self._range_x[1])
intersection.y = tool.limit(intersection.y, self._range_y[0],
self._range_y[1])
self.pose = Pose(intersection.x, intersection.y, angle)
return True
def __init__(self):
self.target_pose = PoseStamped()
self.target_velocity = Twist()
self.aim = Pose()
self.kick_power = 0.0
self.dribble_power = 0.0
self.velocity_control_enable = False
self.chip_enable = False
self.navigation_enable = True
self.avoid_ball = True
self.avoid_defence_area = True
self._MAX_KICK_POWER = 8.0
self._MAX_DRIBBLE_POWER = 8.0
self.set_target_velocity(0, 0, 0)
def _update_intersection(self):
target_pose = WorldModel.get_pose(self._target)
base_pose = WorldModel.get_pose(self._base)
if target_pose is None or base_pose is None:
return False
intersection = tool.get_intersection(base_pose, target_pose,
self._pose1, self._pose2)
angle = tool.getAngle(intersection, target_pose)
intersection.x = tool.limit(intersection.x, self._range_x[0],
self._range_x[1])
intersection.y = tool.limit(intersection.y, self._range_y[0],
self._range_y[1])
self.pose = Pose(intersection.x, intersection.y, angle)
return True
def get_pose(cls, name):
pose = None
if name == 'Ball':
ball_pose = WorldModel._ball_odom.pose.pose.position
pose = Pose(ball_pose.x, ball_pose.y, 0)
elif name[:4] == 'Role':
robot_id = WorldModel.assignments[name]
pose = WorldModel.get_friend_pose(robot_id)
elif name[:5] == 'Enemy':
robot_id = WorldModel.enemy_assignments[name]
pose = WorldModel.get_enemy_pose(robot_id)
elif name[:6] == 'Threat':
robot_id = WorldModel._threat_assignments[name]
pose = WorldModel.get_enemy_pose(robot_id)
elif name[:5] == 'CONST':
pose = constants.poses[name]
return pose
def set_pose(self, x=0.0, y=0.0, theta=0.0):
# 任意の位置に移動する
self.pose = Pose(x, y, theta)
self._update_func = self._update_pose
def _update_approach_to_shoot(self):
# Reference to this idea
# http://wiki.robocup.org/images/f/f9/Small_Size_League_-_RoboCup_2014_-_ETDP_RoboDragons.pdf
ball_pose = WorldModel.get_pose('Ball')
target_pose = WorldModel.get_pose(self._target)
role_pose = WorldModel.get_pose(self._my_role)
if target_pose is None or role_pose is None:
return False
# ボールからターゲットを見た座標系で計算する
angle_ball_to_target = tool.getAngle(ball_pose, target_pose)
trans = tool.Trans(ball_pose, angle_ball_to_target)
tr_role_pose = trans.transform(role_pose)
# tr_role_poseのloser_side判定にヒステリシスをもたせる
if self._role_is_lower_side == True and \
tr_role_pose.y > self._role_pose_hystersis:
self._role_is_lower_side = False
elif self._role_is_lower_side == False and \
tr_role_pose.y < - self._role_pose_hystersis:
self._role_is_lower_side = True
if self._role_is_lower_side:
tr_role_pose.y *= -1.0
tr_approach_pose = Pose(0, 0, 0)
if tr_role_pose.x > 0:
# 1.ボールの斜め後ろへ近づく
# copysign(x,y)でyの符号に合わせたxを取得できる
tr_approach_pose = Pose(
-self._pose_max.x,
math.copysign(self._pose_max.y, tr_role_pose.y), 0)
else:
# ボール裏へ回るためのピボットを生成
pivot_pose = Pose(0, self._tuning_param_pivot_y, 0)
angle_pivot_to_role = tool.getAngle(pivot_pose, tr_role_pose)
limit_angle = self._tuning_angle + math.pi * 0.5
if tr_role_pose.y > self._tuning_param_pivot_y and \
angle_pivot_to_role < limit_angle:
# 2.ボール後ろへ回りこむ
diff_angle = tool.normalize(limit_angle - angle_pivot_to_role)
decrease_coef = diff_angle / self._tuning_angle
tr_approach_pose = Pose(-self._pose_max.x,
self._pose_max.y * decrease_coef, 0)
else:
# 3.ボールに向かう
diff_angle = tool.normalize(angle_pivot_to_role - limit_angle)
approach_coef = diff_angle / (math.pi * 0.5 -
self._tuning_angle)
if approach_coef > 1.0:
approach_coef = 1.0
pos_x = approach_coef * (
2.0 * constants.BallRadius -
self._tuning_param_x) + self._tuning_param_x
tr_approach_pose = Pose(-pos_x, 0, 0)
# 上下反転していたapproach_poseを元に戻す
if self._role_is_lower_side:
tr_approach_pose.y *= -1.0
self.pose = trans.invertedTransform(tr_approach_pose)
self.pose.theta = angle_ball_to_target
return True
def getConjugate(pose):
output = Pose()
output.x = pose.x
output.y = -pose.y
return output