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 main(self): while self.count == 0: MASUO_SSL = 1 ID_MOUNT = 8 message = Message() sender = SEND() senderw = SEND() sendero = SEND() message.send = [] TH_goal_info = Pose2D(6, 0, 0) OUR_goal_info = Pose2D(-6, 0, 0) devide = Role.role_decision(self.robot_blue, self.ball_info, TH_goal_info, OUR_goal_info, self.count) ref_command = referee_translate.main(self, self.referee_info) ref_command = "STOP" if ref_command == "HALT": send = attacker.halt(send) elif ref_command == "STOP": #message.send.append(goalie.main(OUR_goal_info,self.robot_blue[devide.goalie],self.ball_info,self.geometry_msg,senderw)) #message.send.append(attacker.main(TH_goal_info,self.robot_blue[devide.attacker],self.ball_info,sender)) #for defense_id in devide.defense: sendero = SEND() #message.send.append(defense.main(OUR_goal_info,self.robot_blue[defense_id],self.ball_info,self.geometry_msg,sendero)) elif ref_command == "NORMAL_START": send = attacker.halt(send) elif ref_command == "FORCE_START": send = attacker.halt(send) elif ref_command == "OUR_KICKOFF_START": send = attacker.main(TH_goal_info, self.robot_blue[devide.attacker], self.ball_info, send) elif referee_command == "THEIR_KICKOFF_START": send = attacker.halt(send) elif ref_command == "OUR_PANALTY_PRE": send = attacker.halt(send) elif ref_command == "THEIR_PENALTY_PRE": send = attacker.halt(send) elif ref_command == "OUR_DIRECT": send = attacker.halt(send) elif ref_command == "THEIR_DIRECT": send = attacker.halt(send) elif ref_command == "OUR_INDIRECT": send = attacker.halt(send) elif ref_command == "THEIR_INDIRECT": send = attacker.halt(send) self.pub_send_all(message)
def halt(): send_val = SEND() send_val.stop = True send_val.direction = 0 send_val.move_degree = -1 send_val.spinner = False send_val.kickspeedx = 0 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 __init__(self): self.host = '127.0.0.1' self.port = 20011 self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self.sub_robot_info = rospy.Subscriber("vision_our_send", RobotInfo, self.recieve_callback, callback_args=0, queue_size=1) self.sub_ball_info = rospy.Subscriber("ball_info", BALL_INFO, self.ball_callback, queue_size=1) self.sub_recieve_info = rospy.Subscriber("send_value", Message, self.send_value, queue_size=1) self.our_robot_info = {"our": [], "their": []} self.our_robot_info['our'].append(RobotInfo()) self.ball_info = [] self.ball_info.append(BALL_INFO()) self.recieve_info = [] self.recieve_info.append(SEND()) self.move_degree = 0
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