def stop_move(TH_goal_info,OUR_goal_info,robot_info,devide,ball_info,geometry,message): message.send.append(attacker.stop_posi(TH_goal_info,robot_info[devide.attacker],ball_info,0.5)) target_position=Pose2D(-geometry.field_length/2,0,0) goalie_target_info=calculate.Calc(robot_info[devide.goalie].pose,target_position) goalie_target_degree=goalie_target_info.degree_calc() - math.degrees(robot_info[devide.goalie].pose.theta) +180 goalie_target_distance=goalie_target_info.distance_calc() goalie_target_degree=calculate.degree_improve(goalie_target_degree) if goalie_target_distance>=1: goalie_target_distance=1 send_val=SEND() send_val.robot_id=robot_info[devide.goalie].robot_id send_val.distance=goalie_target_distance send_val.move_degree=math.radians(goalie_target_degree) send_val.direction=-robot_info[devide.goalie].pose.theta send_val.stop=False message.send.append(send_val) for offense_id in devide.offense: offense_ball_info=calculate.Calc(robot_info[offense_id].pose,ball_info.pose) offense_ball_degree=offense_ball_info.degree_calc() - math.degrees(robot_info[offense_id].pose.theta) +180 offense_ball_degree=calculate.degree_improve(offense_ball_degree) send_val=SEND() send_val.robot_id=robot_info[offense_id].robot_id send_val.distance=0.0 send_val.move_degree=0.0 send_val.direction=-robot_info[offense_id].pose.theta send_val.stop=False send_val.spinner=False send_val.kickspeedx=0.0 message.send.append(send_val) for defense_id in devide.defense: defense_ball_info=calculate.Calc(robot_info[defense_id].pose,ball_info.pose) defense_ball_degree=defense_ball_info.degree_calc() - math.degrees(robot_info[defense_id].pose.theta) +180 defense_ball_degree=calculate.degree_improve(defense_ball_degree) send_val=SEND() send_val.robot_id=robot_info[defense_id].robot_id send_val.distance=0.0 send_val.move_degree=0.0 send_val.direction=math.radians(defense_ball_degree) send_val.stop=False send_val.spinner=False send_val.kickspeedx=0.0 message.send.append(send_val)
def main(TH_goal_info, attacker, offense, ball_info, geometry):
send_val = SEND()
AT_TH_info = calculate.Calc(attacker.pose, TH_goal_info)
AT_TH_distance = AT_TH_info.distance_calc()
AT_TH_degree = AT_TH_info.degree_calc()
AT_TH_degree = calculate.degree_improve(AT_TH_degree)
AT_OFF_info = calculate.Calc(attacker.pose, offense.pose)
AT_OFF_degree = AT_OFF_info.degree_calc() - math.degrees(
offense.pose.theta)
AT_OFF_distance = AT_OFF_info.distance_calc()
AT_OFF_degree = calculate.degree_improve(AT_OFF_degree)
target_position = Pose2D(
attacker.pose.x + math.cos(attacker.pose.theta) * AT_TH_distance / 2,
attacker.pose.y + math.sin(attacker.pose.theta) * AT_TH_distance / 2,
0)
target_robot_info = calculate.Calc(target_position, offense.pose)
target_robot_distance = target_robot_info.distance_calc()
target_robot_degree = target_robot_info.degree_calc() - math.degrees(
offense.pose.theta)
target_robot_degree = calculate.degree_improve(target_robot_degree)
send_val.stop = False
send_val.direction = math.radians(AT_OFF_degree)
send_val.move_degree = math.radians(target_robot_degree)
send_val.spinner = False
send_val.kickspeedx = 0
send_val.distance = target_robot_distance
if send_val.distance >= 1:
send_val.distance = 1
send_val.robot_id = offense.robot_id
return send_val
def main(our_goal_info, defense, ball_info, geometry):
send_val = SEND()
penalty_length = 0
target_position = Pose2D()
for line_field in geometry.line_field:
if line_field.name == "LeftPenaltyStretch":
penalty_length = line_field.p2_x
target_position = Pose2D(penalty_length, ball_info.pose.y, 0)
if defense.pose.y >= math.fabs(line_field.p1_y):
target_position.y = math.fabs(line_field.p1_y)
elif defense.pose.y <= -math.fabs(line_field.p1_y):
target_position.y = -math.fabs(line_field.p1_y)
robot_target_info = calculate.Calc(target_position, defense.pose)
robot_target_degree = robot_target_info.degree_calc() - math.degrees(
defense.pose.theta)
robot_ball_info = calculate.Calc(defense.pose, ball_info.pose)
robot_ball_degree = robot_ball_info.degree_calc() - math.degrees(
defense.pose.theta) + 180
robot_target_degree = calculate.degree_improve(robot_target_degree)
robot_ball_degree = calculate.degree_improve(robot_ball_degree)
#print(penalty_length)
send_val.stop = False
send_val.direction = -defense.pose.theta
send_val.move_degree = math.radians(robot_target_degree)
send_val.spinner = True
send_val.kickspeedx = 0.0
send_val.distance = math.fabs(math.radians(robot_ball_degree))
send_val.robot_id = defense.robot_id
return send_val
def stop(self):
message = Message()
message.send = []
#message.send.append(attacker.main(Pose2D(0,0,0),self.robot_blue[0],self.ball_info))
for i in range(8):
send_val = SEND()
send_val.robot_id = i
send_val.stop = True
send_val.move_degree = -1
message.send.append(send_val)
#print(message)
self.pub_send_all(message)
def normal_position(attacker, offense, devide, ball_info, geometry, count):
target_position = Pose2D()
if len(devide.offense) == 4:
if count == 1:
target_position = Pose2D(0, 0, 0)
elif count == 2:
target_position = Pose2D(4.5, 3.5, 0)
elif count == 3:
target_position = Pose2D(4.5, -3.5, 0)
elif count == 4:
target_position = Pose2D(3.5, 0, 0)
elif len(devide.offense) == 3:
if count == 1:
target_position = Pose2D(0, 0, 0)
elif count == 2:
target_position = Pose2D(3.5, 3.5, 0)
elif count == 3:
target_position = Pose2D(3.5, -3.5, 0)
elif len(devide.offense) == 2:
if count == 1:
target_position = Pose2D(0, 0, 0)
elif count == 2:
target_position = Pose2D(3.5, 3.5, 0)
elif len(devide.offense) == 1:
if count == 1:
target_position = Pose2D(2.5, 3, 0)
target_robot_info = calculate.Calc(target_position, offense.pose)
target_robot_degree = target_robot_info.degree_calc() - math.degrees(
offense.pose.theta) + 180
target_robot_distance = target_robot_info.distance_calc()
target_robot_degree = calculate.degree_improve(target_robot_degree)
robot_ball_info = calculate.Calc(offense.pose, ball_info.pose)
robot_ball_degree = robot_ball_info.degree_calc() - math.degrees(
offense.pose.theta) + 180
robot_ball_distance = robot_ball_info.distance_calc()
robot_ball_degree = calculate.degree_improve(robot_ball_degree)
send_val = SEND()
send_val.stop = False
send_val.direction = math.radians(robot_ball_degree)
send_val.move_degree = math.radians(target_robot_degree)
send_val.spinner = False
send_val.kickspeedx = 0.0
send_val.distance = target_robot_distance
if send_val.distance >= 1:
send_val.distance = 1
send_val.robot_id = offense.robot_id
return send_val
def stop_posi(TH_goal_info, attacker, ball_info, DISTANCE):
send_val = SEND()
DISTANCE_SPACE = Pose2D(
math.cos(attacker.pose.theta) * DISTANCE,
math.sin(attacker.pose.theta) * DISTANCE, 0)
target_posi = Pose2D(ball_info.pose.x - DISTANCE_SPACE.x,
ball_info.pose.y - DISTANCE_SPACE.y, 0)
TH_goal_robot = calculate.Calc(TH_goal_info, attacker.pose)
TH_goal_robot_degree = TH_goal_robot.degree_calc(
) - attacker.pose.theta * 180 / 3.14
target_robot = calculate.Calc(target_posi, attacker.pose)
target_robot_distance = target_robot.distance_calc()
robot_ball_info = calculate.Calc(attacker.pose, target_posi)
robot_ball_degree = robot_ball_info.degree_calc(
) - attacker.pose.theta * 180 / 3.14
wrap_degree = robot_ball_info.wrap_calc(math.degrees(attacker.pose.theta))
robot_ball_distance = robot_ball_info.distance_calc()
if target_robot_distance >= 1:
target_robot_distance = 1
wrap_degree = calculate.degree_improve(wrap_degree)
robot_ball_degree = calculate.degree_improve(robot_ball_degree)
ball_detect = False
if robot_ball_degree <= 10 and robot_ball_degree >= -10:
ball_detect = True
spinner = False
if robot_ball_distance < 0.4 and ball_detect == True:
spinner = True
kick_pow_x = 0
if TH_goal_robot_degree >= -10 and TH_goal_robot_degree <= 10:
kick_pow_x = 0.0
send_val.stop = False
send_val.direction = math.radians(TH_goal_robot_degree)
send_val.move_degree = math.radians(wrap_degree)
send_val.spinner = True
send_val.kickspeedx = kick_pow_x
send_val.distance = target_robot_distance
send_val.robot_id = attacker.robot_id
return send_val
def main(our_goal_info,goalie,ball_info,geometry): send_val=SEND() OUR_goal_position_middle = Pose2D(-geometry.field_length/2,0,0) target_position=Pose2D(OUR_goal_position_middle.x+0.2,ball_info.pose.y,0) flag=False if goalie.pose.y>=geometry.goal_width/2: target_position.y=geometry.goal_width/2 flag=True if goalie.pose.y<=-geometry.goal_width/2: target_position.y=-geometry.goal_width/2 flag=True OUR_goal_robot=calculate.Calc(our_goal_info,goalie.pose) OUR_goal_robot_degree=OUR_goal_robot.degree_calc() - math.degrees(goalie.pose.theta) +180 OUR_goal_robot_distance=OUR_goal_robot.distance_calc() robot_ball_info=calculate.Calc(goalie.pose,ball_info.pose) robot_ball_degree=robot_ball_info.degree_calc() - math.degrees(goalie.pose.theta) +180 robot_ball_distance=robot_ball_info.distance_calc() robot_ball_degree = calculate.degree_improve(robot_ball_degree) OUR_goal_robot_degree = calculate.degree_improve(OUR_goal_robot_degree) target_info=calculate.Calc(target_position,goalie.pose) target_degree=target_info.degree_calc() - goalie.pose.theta*180/3.14 target_degree=calculate.degree_improve(target_degree) kick_pow_x=0.0 #target_degree= send_val.stop=False send_val.direction=math.degrees(goalie.pose.theta) send_val.direction=calculate.degree_improve(send_val.direction) send_val.direction=math.radians(-send_val.direction) send_val.move_degree=math.radians(target_degree) send_val.spinner=True send_val.kickspeedx=OUR_goal_robot_distance send_val.distance=math.fabs(math.radians(robot_ball_degree)) if flag==True: send_val.distance=0.1 send_val.robot_id=goalie.robot_id return send_val
def move_straight(TH_goal_info, offense, ball_info):
send_val = SEND()
TH_robot_info = calculate.Calc(TH_goal_info, offense.pose)
TH_robot_degree = TH_robot_info.degree_calc() - math.degrees(
offense.pose.theta)
TH_robot_distance = TH_robot_info.distance_calc()
TH_robot_degree = calculate.degree_improve(TH_robot_degree)
robot_ball_info = calculate.Calc(offense.pose, ball_info.pose)
robot_ball_degree = robot_ball_info.degree_calc() - math.degrees(
offense.pose.theta) + 180
robot_ball_distance = robot_ball_info.distance_calc()
robot_ball_degree = calculate.degree_improve(robot_ball_degree)
ball_detect = False
if robot_ball_degree <= 3 and robot_ball_degree >= -3:
ball_detect = True
spinner = False
if robot_ball_distance < 0.12 and ball_detect == True:
spinner = True
if spinner == True and ball_detect == True:
direction = TH_robot_degree
else:
direction = robot_ball_degree
kick_pow_x = 0
if TH_robot_degree >= -5 and TH_robot_degree <= 5:
kick_pow_x = 10.0
send_val.stop = False
send_val.direction = math.radians(direction)
send_val.move_degree = math.radians(robot_ball_degree)
send_val.spinner = True
send_val.kickspeedx = kick_pow_x
send_val.distance = robot_ball_distance
if send_val.distance >= 1:
send_val.distance = 1
send_val.robot_id = attacker.robot_id
return send_val
def main(TH_goal_info, attacker, ball_info):
send_val = SEND()
TH_goal_robot = calculate.Calc(TH_goal_info, attacker.pose)
TH_goal_robot_degree = TH_goal_robot.degree_calc(
) - attacker.pose.theta * 180 / 3.14
robot_ball_info = calculate.Calc(attacker.pose, ball_info.pose)
robot_ball_degree = robot_ball_info.degree_calc(
) - attacker.pose.theta * 180 / 3.14
wrap_degree = robot_ball_info.wrap_calc(math.degrees(attacker.pose.theta))
robot_ball_distance = robot_ball_info.distance_calc()
calculate.degree_improve(wrap_degree)
calculate.degree_improve(robot_ball_degree)
ball_detect = False
if robot_ball_degree <= 10 and robot_ball_degree >= -10:
ball_detect = True
spinner = False
if robot_ball_distance < 0.4 and ball_detect == True:
spinner = True
kick_pow_x = 0
if TH_goal_robot_degree >= -10 and TH_goal_robot_degree <= 10:
kick_pow_x = 6.0
send_val.direction = math.radians(TH_goal_robot_degree)
send_val.move_degree = math.radians(wrap_degree)
send_val.spinner = spinner
send_val.distance = 0.85
send_val.kickspeedx = kick_pow_x
send_val.robot_id = attacker.robot_id
send_val.stop = True
return send_val
def pass_move(TH_goal_info, attacker, offense, ball_info, geometry, target,
flag):
send_val = SEND()
TARGET_POSITION_X = 3.5
TARGET_POSITION_Y = -3.5
AT_TH_info = calculate.Calc(attacker.pose, TH_goal_info)
AT_TH_distance = AT_TH_info.distance_calc()
AT_TH_degree = AT_TH_info.degree_calc()
AT_OFF_info = calculate.Calc(attacker.pose, offense.pose)
AT_OFF_distance = AT_OFF_info.distance_calc()
AT_OFF_degree = AT_OFF_info.degree_calc() - math.degrees(
offense.pose.theta)
robot_ball_info = calculate.Calc(offense.pose, ball_info.pose)
robot_ball_degree = robot_ball_info.degree_calc() - math.degrees(
offense.pose.theta) + 180
robot_ball_degree = calculate.degree_improve(robot_ball_degree)
robot_ball_distance = robot_ball_info.distance_calc()
y = flag.slope * target.x + flag.intercept
if flag.kick_flag == True:
target_position = Pose2D(target.x, y, 0)
else:
target_position = Pose2D(target.x, target.y, 0)
if flag.different_place == True:
target_position = Pose2D(target.x, target.y, 0)
target_position = position.line_limit(target_position, geometry)
flag.arrive[flag.count_offense] = False
if (target.x - 0.1) <= offense.pose.x and (target.x +
0.1) >= offense.pose.x:
if (target.y - 0.1) <= offense.pose.y and (target.y +
0.1) >= offense.pose.y:
flag.arrive[flag.count_offense] = True
target_robot_info = calculate.Calc(target_position, offense.pose)
target_robot_distance = target_robot_info.distance_calc()
target_robot_degree = target_robot_info.degree_calc() - math.degrees(
offense.pose.theta)
target_robot_degree = calculate.degree_improve(target_robot_degree)
TH_robot_info = calculate.Calc(TH_goal_info, offense.pose)
TH_robot_degree = TH_robot_info.degree_calc() - math.degrees(
offense.pose.theta)
TH_robot_distance = TH_robot_info.distance_calc()
ball_detect = False
if robot_ball_degree <= 10 and robot_ball_degree >= -10:
ball_detect = True
spinner = False
if robot_ball_distance < 0.15 and ball_detect == True:
spinner = True
if spinner == True and ball_detect == True:
direction = TH_robot_degree
else:
direction = robot_ball_degree
kick_pow_x = 0
if TH_robot_degree >= -5 and TH_robot_degree <= 5:
kick_pow_x = 5.0
if robot_ball_distance < 0.15:
flag.kick_flag = False
send_val.stop = False
send_val.direction = math.radians(direction)
send_val.move_degree = math.radians(target_robot_degree)
send_val.spinner = spinner
send_val.kickspeedx = kick_pow_x
send_val.distance = target_robot_distance * 6
if send_val.distance >= 1:
send_val.distance = 1
send_val.robot_id = offense.robot_id
return send_val
def main(TH_goal_info,OUR_goal_info,robot_info,devide,ball_info,geometry,message): message.send.append(attacker.stop_posi(TH_goal_info,robot_info[devide.attacker],ball_info,0.12)) target_position=Pose2D(-geometry.field_length/2,0,0) goalie_target_info=calculate.Calc(robot_info[devide.goalie].pose,target_position) goalie_target_degree=goalie_target_info.degree_calc() - math.degrees(robot_info[devide.goalie].pose.theta) +180 goalie_target_distance=goalie_target_info.distance_calc() goalie_target_degree=calculate.degree_improve(goalie_target_degree) if goalie_target_distance>=1: goalie_target_distance=1 send_val=SEND() send_val.robot_id=robot_info[devide.goalie].robot_id send_val.distance=goalie_target_distance send_val.move_degree=math.radians(goalie_target_degree) send_val.direction=-robot_info[devide.goalie].pose.theta send_val.stop=False message.send.append(send_val) count=-1 for offense_id in devide.offense: count=count+1 target_position=calculate.kick_offense_posiiton_calc(devide.offense,geometry.field_width) offense_target_info=calculate.Calc(robot_info[offense_id].pose,target_position[count]) offense_target_distance=offense_target_info.distance_calc() offense_target_degree=offense_target_info.degree_calc() - math.degrees(robot_info[offense_id].pose.theta) +180 offense_target_degree=calculate.degree_improve(offense_target_degree) if offense_target_distance>=1: offense_target_distance=1 send_val=SEND() send_val.robot_id=robot_info[offense_id].robot_id send_val.distance=offense_target_distance send_val.move_degree=math.radians(offense_target_degree) send_val.stop=False send_val.direction=-robot_info[offense_id].pose.theta send_val.spinner=True send_val.kickspeedx=0.0 message.send.append(send_val) count=-1 for defense_id in devide.defense: count=count+1 num=calculate.defense_position_calc(devide.defense,geometry.field_width) if len(num)>count: target_position=Pose2D(-4.5,num[count],0) defense_target_info=calculate.Calc(robot_info[defense_id].pose,target_position) defense_target_distance=defense_target_info.distance_calc() defense_target_degree=defense_target_info.degree_calc() - math.degrees(robot_info[defense_id].pose.theta) +180 defense_target_degree=calculate.degree_improve(defense_target_degree) if defense_target_distance>=1: defense_target_distance=1 send_val=SEND() send_val.robot_id=robot_info[defense_id].robot_id send_val.distance=defense_target_distance send_val.move_degree=math.radians(defense_target_degree) send_val.direction=-robot_info[defense_id].pose.theta send_val.stop=False send_val.spinner=True send_val.kickspeedx=0.0 message.send.append(send_val)
def pass_move(TH_goal_info, attacker, ball_info, flag, target_position):
send_val = SEND()
TARGET_POSITION_X = 3.5
TARGET_POSITION_Y = -3.5
TH_robot_info = calculate.Calc(TH_goal_info, attacker.pose)
TH_robot_degree = TH_robot_info.degree_calc() - math.degrees(
attacker.pose.theta)
TH_robot_distance = TH_robot_info.distance_calc()
TH_robot_degree = calculate.degree_improve(TH_robot_degree)
robot_ball_info = calculate.Calc(attacker.pose, ball_info.pose)
robot_ball_degree = robot_ball_info.degree_calc() - math.degrees(
attacker.pose.theta) + 180
robot_ball_degree = calculate.degree_improve(robot_ball_degree)
robot_ball_distance = robot_ball_info.distance_calc()
#target_position=Pose2D(TARGET_POSITION_X, TARGET_POSITION_Y,0)
target_robot_info = calculate.Calc(target_position, attacker.pose)
target_robot_degree = target_robot_info.degree_calc() - math.degrees(
attacker.pose.theta)
target_robot_degree = calculate.degree_improve(target_robot_degree)
target_robot_distance = target_robot_info.distance_calc()
#print(target_robot_degree)
ball_detect = False
if robot_ball_degree <= 5 and robot_ball_degree >= -5:
ball_detect = True
spinner = False
if ball_detect == True and robot_ball_distance < 0.12:
spinner = True
kick_pow_x = 0
direction = robot_ball_degree
if spinner == True and ball_detect == True:
direction = target_robot_degree
if target_robot_degree >= -5 and target_robot_degree <= 5 and not target_position.x == 100:
kick_pow_x = 5
flag.flag_temp = True
if flag.flag_temp == True:
if robot_ball_distance > 0.2:
flag.flag_temp = False
flag.kick_flag = True
flag.robot_id_rem = attacker.robot_id
if not attacker.robot_id == flag.robot_id_rem and robot_ball_distance < 0.3:
flag.kick_flag = False
send_val.stop = False
send_val.direction = math.radians(direction)
send_val.move_degree = math.radians(robot_ball_degree)
send_val.spinner = spinner
send_val.kickspeedx = kick_pow_x
send_val.distance = robot_ball_distance
send_val.robot_id = attacker.robot_id
if send_val.distance >= 1:
send_val.distance = 1
return send_val