def __init__(self): rospy.init_node("Calculation_node") self.robot_color = str(rospy.get_param("friend_color")) self.robot_side = str(rospy.get_param("team_side")) # Composition self.objects = Objects(self.robot_color, self.robot_side, config.NUM_FRIEND_ROBOT, config.NUM_ENEMY_ROBOT, node="calculation") self.robot_friend = self.objects.robot self.robot_enemy = self.objects.enemy self.ball_params = self.objects.ball self.ball_sub_params = Ball_sub_params() self.def_pos = Def_pos() self.ball_frame = 10 # ボールの軌道直線フィッティングと速度の計算フレーム数 self.ball_move_threshold = 0.01 # ボールが移動したと判定する閾値[m] self.same_pos_count = 0 # 停止判定用カウント self.ball_pos_count = 0 # 計算用カウント、フレーム単位でカウント self.calc_flag = False # 計算フラグ、停止判定時は計算しない self.ball_pos_x_array = np.array([0.0] * self.ball_frame) # ボールのx座標保存用配列 self.ball_pos_y_array = np.array([0.0] * self.ball_frame) # ボールのy座標保存用配列 self.ball_vel_array = np.array([0.0] * self.ball_frame) # ボールの速度保存用配列 self.ball_vel_x_array = np.array([0.0] * self.ball_frame) # ボールのx方向の速度保存用配列 self.ball_vel_y_array = np.array([0.0] * self.ball_frame) # ボールのy方向の速度保存用配列 self.ball_vel_time_array = np.array([0.0] * self.ball_frame) # 加速度計算用、時間配列 self.ball_vel = 0. # ボール速度 self.ball_vel_a = 0. # ボール速度の傾き self.ball_vel_b = 0. # ボール速度の切片 self.ball_vel_x_a = 0. # x方向の速度の傾き self.ball_vel_x_b = 0. # x方向の速度の切片 self.ball_vel_y_a = 0. # y方向の速度の傾き self.ball_vel_y_b = 0. # y方向の速度の切片 self.ball_stop_time_x = 0. # x方向の停止までの時間 self.ball_stop_time_y = 0. # y方向の停止までの時間 # 守備の時のロボットのポジション座標計算用変数 # 現状、青チームのみ対応 self.g_up_x = -6.0 # ゴールポストの上側のx座標:y_GL self.g_up_y = 0.6 # ゴールポストの上側のy座標:x_GL self.g_down_x = -6.0 # ゴールポストの下側のx座標:y_GR self.g_down_y = -0.6 # ゴールポストの下側のy座標:x_GR self.g_center_x = -6.0 # ゴールの中央のx座標:y_GC self.g_center_y = 0.0 # ゴールの中央のy座標:x_GC self.p_area_up_x = -4.8 # ペナルティエリアの上側の角のx座標:y_PL self.p_area_up_y = 1.2 # ペナルティエリアの上側の角のy座標:x_PL self.p_area_down_x = -4.8 # ペナルティエリアの下側の角のx座標:y_PR self.p_area_down_y = -1.2 # ペナルティエリアの下側の角のy座標:x_PR self.line_up_x = 0.0 # ボールとゴールポストを結んだ線と防御ラインとの交点の上側のx座標:y_LL self.line_up_y = 0.0 # ボールとゴールポストを結んだ線と防御ラインとの交点の上側のy座標:x_LL self.line_down_x = 0.0 # ボールとゴールポストを結んだ線と防御ラインとの交点の下側のx座標:y_LR self.line_down_y = 0.0 # ボールとゴールポストを結んだ線と防御ラインとの交点の下側のy座標:x_LR self.line_up_r_x = 0.0 # ロボットの半径を考慮した補正後の座標:y_LL' self.line_up_r_y = 0.0 # ロボットの半径を考慮した補正後の座標:x_LL' self.line_down_r_x = 0.0 # ロボットの半径を考慮した補正後の座標:y_LR' self.line_down_r_y = 0.0 # ロボットの半径を考慮した補正後の座標:x_LR' self.offset_r = 0. # オフセット値 self.robot_r = 90.0 / 1000.0 # ロボット半径 self.robot_a = 1.0 # ロボットの加速度 self.ball_MAX_SPEED = 6.5 # ボールの最大速度 self.delay_time_ms = 100.0 # 遅延時間[ms] self.L_a = 0.0 # 壁のラインとボールまでの距離 self.L_G = 0.0 # ボール到達までに移動可能な距離
def __init__(self, robot_id=None): self.robot_color = str(rospy.get_param("friend_color")) self.robot_side = str(rospy.get_param("team_side")) if robot_id is None: self.robot_id = str(rospy.get_param("~robot_num")) else: self.robot_id = str(robot_id) self.robot_total = config.NUM_FRIEND_ROBOT self.enemy_total = config.NUM_ENEMY_ROBOT self.cmd = robot_commands() # type: robot_commands self.cmd.kick_speed_x = 0 self.cmd.kick_speed_z = 0 self.cmd.dribble_power = 0 self._command_pub = RobotCommandPublisherWrapper( self.robot_color, self.robot_id) # Composition self.objects = Objects(self.robot_color, self.robot_side, self.robot_total, self.enemy_total, node="robot" + str(self.robot_id)) self.ctrld_robot = self.objects.robot[int( self.robot_id)] # type: entity.Robot self.robot_friend = self.objects.robot self.robot_enemy = self.objects.enemy self.ball_params = self.objects.ball self.pid = RobotPid(self.robot_id, self.objects, self.cmd) self.status = RobotStatus(self.pid) self.kick = RobotKick(self.pid, self.cmd, self.status) self.defence = RobotDefence(self.status, self.kick) self.keeper = RobotKeeper(self.kick) # listner 起動 self.status_listener() self.def_pos_listener() self.set_pid_server() rospy.Timer(rospy.Duration(1.0 / 30.0), self.pid.replan_timer_callback) self._last_pub_time = rospy.Time.now() self._last_vel_surge_sway_vec = [0.0, 0.0] self._last_omega = 0.0 self._fps = [] self._current_loop_time = 0 self._last_loop_time = 0 rospy.Timer(rospy.Duration(1.0), self.print_fps_callback) self.robot_fps_publisher = rospy.Publisher("fps/robot_" + str(robot_id), Float32, queue_size=1) self.loop_rate = rospy.Rate(ROBOT_LOOP_RATE) rospy.loginfo("start robot node: " + self.robot_id)
def __init__(self): rospy.init_node("world_model") self._team_color = str(rospy.get_param('friend_color')) self._team_side = str(rospy.get_param('team_side')) """----上の5つの変数、インスタンスをまとめたもの、callbackをもつ---""" self._objects = Objects(self._team_color, self._team_side, config.NUM_FRIEND_ROBOT, config.NUM_ENEMY_ROBOT, node="world_model") self.robot_pys = [] self.robot_py_ths = [] self.robot_ids = self._objects.get_robot_ids() # スレッドを使わないときはここをコメントアウト --ここから-- for robot_id in self.robot_ids: robot_py = robot.Robot(robot_id) self.robot_pys.append(robot_py) th = threading.Thread(target=robot_py.run) th.setDaemon(True) th.start() self.robot_py_ths.append(th) # スレッドを使わないときはここをコメントアウト --ここまで-- """---Referee---""" self._referee = Referee(self._objects) self._stcalcurator = { 'normal_start_normal': NormalStartStrategyCalcurator(self._objects), 'normal_start_kickoff': NormalStartKickOffStrategyCalcurator(self._objects), 'normal_start_kickoff_defence': NormalStartKickOffDefenceStrategyCalcurator(self._objects), 'stop': StopStrategyCalculator(self._objects), 'penalty_attack': PenaltyAttack(self._objects), 'penalty_defence': NormalStartPenaltyDefenceStrategyCalcurator(self._objects), 'direct_free_attack': DirectFreeAttack(self._objects), 'direct_free_defence': DirectFreeDefence(self._objects), 'indirect_free_attack': IndirectFreeAttack(self._objects), 'indirect_free_defence': IndirectFreeDefence(self._objects), 'friend_ball_placement': FriendBallPlacement(self._objects), 'enemy_ball_placement': EnemyBallPlacement(self._objects) } self._status_publisher = WorldModelStatusPublisher( self._team_color, robot_ids=self._objects.get_robot_ids()) # 積分などに必要な情報を保存するオブジェクト self._strategy_context = StrategyContext() # とりあえず例として"last_number"という名前で10フレーム分のコンテキストを作成。 # 初期値は全て0を指定。 self._strategy_context.register_new_context("last_number", 10, 0) self._strategy_context.register_new_context("normal_strat_state", 2, 0, namespace="normal_strat") self._strategy_context.register_new_context("kickoff_complete", 1, False, namespace="world_model") self._strategy_context.register_new_context("enemy_kick", 1, False, namespace="world_model") self._strategy_context.register_new_context("referee_branch", 1, "NONE", namespace="world_model") self._strategy_context.register_new_context("defence_or_attack", 1, False, namespace="world_model") self._strategy_context.register_new_context("placed_ball_position", 1, [0, 0], namespace="world_model") self._strategy_context.register_new_context("indirect_finish", 1, False, namespace="world_model") self._strategy_context.register_new_context("direct_finish", 1, False, namespace="world_model") self._strategy_context.register_new_context("penalty_finish", 1, False, namespace="world_model") self._loop_events = [] custom_referee_branch_subs = rospy.Subscriber( "/force_referee_branch", String, self.force_referee_branch) self.custom_referee_branch = "" self.world_model_fps_publisher = rospy.Publisher("fps/world_model", Float32, queue_size=1)
class Robot(object): def __init__(self, robot_id=None): self.robot_color = str(rospy.get_param("friend_color")) self.robot_side = str(rospy.get_param("team_side")) if robot_id is None: self.robot_id = str(rospy.get_param("~robot_num")) else: self.robot_id = str(robot_id) self.robot_total = config.NUM_FRIEND_ROBOT self.enemy_total = config.NUM_ENEMY_ROBOT self.cmd = robot_commands() # type: robot_commands self.cmd.kick_speed_x = 0 self.cmd.kick_speed_z = 0 self.cmd.dribble_power = 0 self._command_pub = RobotCommandPublisherWrapper( self.robot_color, self.robot_id) # Composition self.objects = Objects(self.robot_color, self.robot_side, self.robot_total, self.enemy_total, node="robot" + str(self.robot_id)) self.ctrld_robot = self.objects.robot[int( self.robot_id)] # type: entity.Robot self.robot_friend = self.objects.robot self.robot_enemy = self.objects.enemy self.ball_params = self.objects.ball self.pid = RobotPid(self.robot_id, self.objects, self.cmd) self.status = RobotStatus(self.pid) self.kick = RobotKick(self.pid, self.cmd, self.status) self.defence = RobotDefence(self.status, self.kick) self.keeper = RobotKeeper(self.kick) # listner 起動 self.status_listener() self.def_pos_listener() self.set_pid_server() rospy.Timer(rospy.Duration(1.0 / 30.0), self.pid.replan_timer_callback) self._last_pub_time = rospy.Time.now() self._last_vel_surge_sway_vec = [0.0, 0.0] self._last_omega = 0.0 self._fps = [] self._current_loop_time = 0 self._last_loop_time = 0 rospy.Timer(rospy.Duration(1.0), self.print_fps_callback) self.robot_fps_publisher = rospy.Publisher("fps/robot_" + str(robot_id), Float32, queue_size=1) self.loop_rate = rospy.Rate(ROBOT_LOOP_RATE) rospy.loginfo("start robot node: " + self.robot_id) def store_and_publish_commands(self): acc_clip_threshold_max = 0.075 acc_clip_threshold_min = 0.01 current_pub_time = rospy.Time.now() dt = (current_pub_time - self._last_pub_time).to_sec() self.ctrld_robot.update_expected_velocity_context( self.cmd.vel_x, self.cmd.vel_y, self.cmd.omega) current_acc = np.array([ self.cmd.vel_surge - self._last_vel_surge_sway_vec[0], self.cmd.vel_sway - self._last_vel_surge_sway_vec[1] ]) / dt current_omega_acc = (self.cmd.omega - self._last_omega) / dt clipped_acc \ = functions.clip_vector2(current_acc, acc_clip_threshold_max) clipped_omega_acc, _ \ = functions.clip_vector2((current_omega_acc, 0.0), acc_clip_threshold_max / self.objects.robot[0].size_r) vel_acc_clip_threshold_min = acc_clip_threshold_min omega_acc_clip_threshold_min = acc_clip_threshold_min / self.objects.robot[ 0].size_r if -vel_acc_clip_threshold_min < clipped_acc[ 0] < vel_acc_clip_threshold_min: self.cmd.vel_surge = self._last_vel_surge_sway_vec[ 0] + clipped_acc[0] if -vel_acc_clip_threshold_min < clipped_acc[ 1] < vel_acc_clip_threshold_min: self.cmd.vel_sway = self._last_vel_surge_sway_vec[1] + clipped_acc[ 1] if -omega_acc_clip_threshold_min < clipped_omega_acc < omega_acc_clip_threshold_min: self.cmd.omega = self._last_omega + clipped_omega_acc self.ctrld_robot.handle_loop_callback() self._command_pub.publish(self.cmd) self._last_pub_time = rospy.Time.now() self._last_vel_surge_sway_vec = (self.cmd.vel_surge, self.cmd.vel_sway) self._last_omega = self.cmd.omega # self.reset_cmd() def reset_cmd(self): default_cmd = robot_commands() default_cmd.kick_speed_x = 0 default_cmd.kick_speed_z = 0 default_cmd.dribble_power = 0 self.cmd.vel_x = default_cmd.vel_x self.cmd.vel_y = default_cmd.vel_y self.cmd.vel_surge = default_cmd.vel_surge self.cmd.vel_sway = default_cmd.vel_sway self.cmd.omega = default_cmd.omega self.cmd.theta = default_cmd.theta self.cmd.kick_speed_x = default_cmd.kick_speed_x self.cmd.kick_speed_z = default_cmd.kick_speed_z self.cmd.dribble_power = default_cmd.dribble_power def shutdown_cmd(self): self.reset_cmd() self.cmd.shutdown_flag = True self.store_and_publish_commands() def print_fps_callback(self, event): if len(self._fps) > 0: msg = Float32() msg.data = sum(self._fps) / len(self._fps) self.robot_fps_publisher.publish(msg) self._fps = [] def run(self): # Loop 処理 while not rospy.is_shutdown(): self.run_once() def run_once(self): if True: # インデント変えたくなかっただけのif True self._current_loop_time = time.time() elapsed_time = self._current_loop_time - self._last_loop_time self._fps.append(1. / elapsed_time) # 場外にいる場合、カルマンフィルタの信念(分散)を初期化する if self.ctrld_robot.get_id( ) not in self.objects.get_active_robot_ids(): self.ctrld_robot.reset_own_belief() # カルマンフィルタ,恒等関数フィルタの適用 # vision_positionからcurrent_positionを決定してつめる for robot in self.robot_friend: if robot.get_id() == self.ctrld_robot.get_id(): # kalman_filter(self.ctrld_robot) identity_filter(self.ctrld_robot) else: identity_filter(robot) for enemy in self.robot_enemy: identity_filter(enemy) self.ctrld_robot.set_max_velocity( rospy.get_param( "/robot_max_velocity", default=config.ROBOT_MAX_VELOCITY)) # m/s 機体の最高速度 if self.status.robot_status == "move_linear": self.pid.pid_linear(self.ctrld_robot.get_future_position()[0], self.ctrld_robot.get_future_position()[1], self.ctrld_robot.get_future_orientation()) elif self.status.robot_status == "move_circle": self.pid.pid_circle(self.pid.pid_circle_center_x, self.pid.pid_circle_center_y, self.ctrld_robot.get_future_position()[0], self.ctrld_robot.get_future_position()[1], self.ctrld_robot.get_future_orientation()) if self.status.robot_status == "move_linear_straight": self.pid.pid_straight( self.ctrld_robot.get_future_position()[0], self.ctrld_robot.get_future_position()[1], self.ctrld_robot.get_future_orientation()) elif self.status.robot_status == "pass": self.kick.pass_ball( self.ctrld_robot.get_pass_target_position()[0], self.ctrld_robot.get_pass_target_position()[1]) elif self.status.robot_status == "pass_chip": self.kick.pass_ball( self.ctrld_robot.get_pass_target_position()[0], self.ctrld_robot.get_pass_target_position()[1], is_tip_kick=True) elif self.status.robot_status == "prepare_pass": self.kick.pass_ball( self.ctrld_robot.get_pass_target_position()[0], self.ctrld_robot.get_pass_target_position()[1], should_wait=True) elif self.status.robot_status == "ball_place_kick": self.kick.pass_ball( self.ctrld_robot.get_pass_target_position()[0], self.ctrld_robot.get_pass_target_position()[1], place=True) elif self.status.robot_status == "ball_place_dribble": self.kick.dribble_ball( self.ctrld_robot.get_pass_target_position()[0], self.ctrld_robot.get_pass_target_position()[1], ignore_penalty_area=True) elif self.status.robot_status == "shoot": self.kick.shoot_ball() elif self.status.robot_status == "shoot_right": self.kick.shoot_ball(target="right") elif self.status.robot_status == "shoot_left": self.kick.shoot_ball(target="left") elif self.status.robot_status == "shoot_center": self.kick.shoot_ball(target="center") elif self.status.robot_status == "prepare_shoot": self.kick.shoot_ball(should_wait=True) elif self.status.robot_status == "penalty_shoot": self.kick.shoot_ball(ignore_penalty_area=True) elif self.status.robot_status == "penalty_shoot_right": self.kick.shoot_ball(target="right", ignore_penalty_area=True) elif self.status.robot_status == "penalty_shoot_left": self.kick.shoot_ball(target="left", ignore_penalty_area=True) elif self.status.robot_status == "penalty_shoot_center": self.kick.shoot_ball(target="center", ignore_penalty_area=True) elif self.status.robot_status == "receive": self.kick.receive_ball( self.ctrld_robot.get_future_position()[0], self.ctrld_robot.get_future_position()[1]) elif self.status.robot_status == "receive_direct_pass": self.kick.receive_and_direct_pass( self.ctrld_robot.get_future_position(), self.ctrld_robot.get_pass_target_position()) elif self.status.robot_status == "receive_direct_shoot": self.kick.receive_and_direct_shoot( self.ctrld_robot.get_future_position()) elif self.status.robot_status == "receive_direct_shoot_left": self.kick.receive_and_direct_shoot( self.ctrld_robot.get_future_position(), target="left") elif self.status.robot_status == "receive_direct_shoot_right": self.kick.receive_and_direct_shoot( self.ctrld_robot.get_future_position(), target="right") elif self.status.robot_status == "receive_direct_shoot_center": self.kick.receive_and_direct_shoot( self.ctrld_robot.get_future_position(), target="center") elif self.status.robot_status == "defence1": self.defence.move_defence(self.defence.def1_pos_x, self.defence.def1_pos_y) elif self.status.robot_status == "defence2": self.defence.move_defence(self.defence.def2_pos_x, self.defence.def2_pos_y) elif self.status.robot_status == "defence3": self.kick.receive_ball(self.defence.def1_pos_x, self.defence.def1_pos_y) elif self.status.robot_status == "defence4": self.kick.receive_ball(self.defence.def2_pos_x, self.defence.def2_pos_y) elif self.status.robot_status == "keeper": self.keeper.keeper() elif self.status.robot_status == "keeper_pass_chip": self.kick.pass_ball( self.ctrld_robot.get_pass_target_position()[0], self.ctrld_robot.get_pass_target_position()[1], is_tip_kick=True, ignore_penalty_area=True) elif self.status.robot_status == "stop": self.reset_cmd() self.status.robot_status = "none" elif self.status.robot_status == "halt": self.reset_cmd() elif self.status.robot_status == "shutdown": self.shutdown_cmd() self.store_and_publish_commands() self._last_loop_time = self._current_loop_time self.loop_rate.sleep() """ def goal_pose_listener(self): rospy.Subscriber("/robot_" + self.robot_id + "/goal_pose", Pose, self.ctrld_robot.goal_pose_callback) def target_pose_listener(self): rospy.Subscriber("/robot_" + self.robot_id + "/target_pose", Pose, self.ctrld_robot.target_pose_callback) """ def status_listener(self): rospy.Subscriber( "/" + self.robot_color + "/robot_" + self.robot_id + "/status", Status, self.status.status_callback) def set_pid_server(self): service_name = "/" + self.robot_color + "/robot_" + self.robot_id + "/set_pid" if service_name not in rosservice.get_service_list(): rospy.Service(service_name, pid, self.pid.set_pid_callback) """ def kick(self, req): self.cmd.kick_speed_x = 3 return True def kick_end(self, req): self.cmd.kick_speed_x = 0 return True def kick_server(self): rospy.Service("/robot_0/kick", Kick, self.kick) rospy.Service("/robot_0/kick_end", Kick, self.kick_end) """ def def_pos_listener(self): rospy.Subscriber("/" + self.robot_color + "/def_pos", Def_pos, self.defence.def_pos_callback)