def __init__(self):
rospy.init_node("robot")
self.robot_color = str(rospy.get_param("~robot_color"))
self.robot_num = str(rospy.get_param("~robot_num"))
self.friend_num = 8
self.enemy_num = 8
self.cmd = robot_commands()
self.cmd.kick_speed_x = 0
self.cmd.kick_speed_z = 0
self.cmd.dribble_power = 0
self.command_pub = rospy.Publisher("/" + self.robot_color + "/robot_" +
self.robot_num + "/robot_commands",
robot_commands,
queue_size=10)
# Composition
self.robot_params = RobotParameters(self.robot_num, self.friend_num,
self.enemy_num)
self.ball_params = Ball()
self.pid = RobotPid(self.robot_params, self.ball_params, self.cmd,
self.command_pub)
self.status = RobotStatus(self.pid, self.robot_params)
self.kick = RobotKick(self.ball_params, self.robot_params, self.pid,
self.cmd, self.status, self.command_pub)
# listner 起動
self.odom_listener()
#self.goal_pose_listener()
#self.target_pose_listener()
self.status_listener()
rospy.Timer(rospy.Duration(0.1), self.pid.replan_timerCallback)
#Loop 処理
self.loop_rate = rospy.Rate(ROBOT_LOOP_RATE)
while not rospy.is_shutdown():
if self.status.robot_status == "move_linear":
self.pid.pid_linear(self.robot_params.pid_goal_pos_x,
self.robot_params.pid_goal_pos_y,
self.robot_params.pid_goal_theta)
if self.status.robot_status == "move_circle":
self.pid.pid_circle(self.pid.pid_circle_center_x,
self.pid.pid_circle_center_y,
self.robot_params.pid_goal_pos_x,
self.robot_params.pid_goal_pos_y,
self.robot_params.pid_goal_theta)
if self.status.robot_status == "kick":
self.kick.kick_x()
if self.status.robot_status == "pass":
self.kick.pass_ball(self.robot_params.pass_target_pos_x,
self.robot_params.pass_target_pos_y)
if self.status.robot_status == "receive":
self.kick.recieve_ball(self.robot_params.pass_target_pos_x,
self.robot_params.pass_target_pos_y)
self.loop_rate.sleep()
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 publish(self, joy):
commands = robot_commands()
if buttons.isEdgeOn(self.select_button) == True:
self.holonomic = not self.holonomic
if self.holonomic == True:
rospy.loginfo('Change controller mode : holonomic')
else:
rospy.loginfo('Change controller mode : NON-holonomic')
if joy.buttons[self.turbo_button] == 0:
vel_scale = 1
else:
vel_scale = 3
if self.holonomic == True:
commands.vel_surge = joy.axes[self.surge_axis] * vel_scale
commands.vel_sway = 0.0
commands.omega = joy.axes[self.sway_axis] * 1.5 * vel_scale
else:
commands.vel_surge = joy.axes[self.surge_axis] * vel_scale
commands.vel_sway = joy.axes[self.sway_axis] * vel_scale
omega = -joy.axes[self.turn_l_axis] + joy.axes[self.turn_r_axis]
commands.omega = omega * 1.5 * vel_scale
if joy.buttons[self.kick_x_button] == 1:
commands.kick_speed_x = 3
commands.kick_speed_z = 0
elif joy.buttons[self.kick_z_button] == 1:
commands.kick_speed_x = 3
commands.kick_speed_z = 3
else:
commands.kick_speed_x = 0
commands.kick_speed_z = 0
if joy.buttons[self.dribble_button] == 1:
commands.dribble_power = 1
else:
commands.dribble_power = 0
self.pub.publish(commands)
def __init__(self):
rospy.init_node("world_model")
self.team_color = str(rospy.get_param("team_color"))
"""---devisionの選択---"""
self.devision = "A"
self.devision = "B"
"""---味方ロボット台数と敵ロボット台数---"""
self.robot_num = 4
self.enemy_num = 8
"""---フィールドとロボットのパラメータ---"""
self.robot_r = 0.09
if self.devision == "A":
self.field_x_size = 12.
self.field_y_size = 9.
elif self.devision == "B":
self.field_x_size = 9.
self.field_y_size = 6.
self.field_x_min = - self.field_x_size / 2.
self.field_x_max = self.field_x_size / 2.
self.field_y_min = - self.field_y_size / 2.
self.field_y_max = self.field_y_size / 2.
self.goal_y_size = 1.2
self.goal_y_min = -self.goal_y_size / 2.
self.goal_y_max = self.goal_y_size / 2.
"""---team cloorと攻撃方向---"""
#self.color = "Yellow"
self.color = "Blue"
self.goal_direction = "Right"
#self.goal_direction = "Left"
"""---PID制御用パラメータ---"""
self.Kpv = 0.3
self.Kpr = 0.1
"""---Potential法用パラメータ---"""
self.Kpv_2 = 0.5
self.Kpr_2 = 0.1
"""---refereeからの指示をもとにどのループを回すかの指標---"""
self.referee_branch = None
"""---ボール軌道の考慮時間幅(linear Regressionで軌道予測するため)---"""
self.ball_dynamics_window = 5
self.ball_dynamics_x = [0. for i in range(self.ball_dynamics_window)]
self.ball_dynamics_y = [0. for i in range(self.ball_dynamics_window)]
self.goal_keeper_x = self.field_x_min + 0.05
"""---robot、ballオブジェクトのインスタンス---"""
self.robot = [entity.Robot() for i in range(self.robot_num)]
self.enemy = [entity.Robot() for i in range(self.enemy_num)]
self.ball = entity.Ball()
"""---Publisherオブジェクトのインスタンス---"""
self.robot_0_pub = rospy.Publisher("/" + self.team_color + "/robot_0/robot_commands", robot_commands, queue_size=10)
self.robot_1_pub = rospy.Publisher("/" + self.team_color + "/robot_1/robot_commands", robot_commands, queue_size=10)
self.robot_2_pub = rospy.Publisher("/" + self.team_color + "/robot_2/robot_commands", robot_commands, queue_size=10)
self.robot_3_pub = rospy.Publisher("/" + self.team_color + "/robot_3/robot_commands", robot_commands, queue_size=10)
"""
self.robot_4_pub = rospy.Publisher("/robot_4/robot_commands", robot_commands, queue_size=10)
self.robot_5_pub = rospy.Publisher("/robot_5/robot_commands", robot_commands, queue_size=10)
self.robot_6_pub = rospy.Publisher("/robot_6/robot_commands", robot_commands, queue_size=10)
self.robot_7_pub = rospy.Publisher("/robot_7/robot_commands", robot_commands, queue_size=10)
"""
"""---status Publisherオブジェクトのインスタンス---"""
self.robot_0_status_pub = rospy.Publisher("/" + self.team_color + "/robot_0/status", Status, queue_size=10)
self.robot_1_status_pub = rospy.Publisher("/" + self.team_color + "/robot_1/status", Status, queue_size=10)
self.robot_2_status_pub = rospy.Publisher("/" + self.team_color + "/robot_2/status", Status, queue_size=10)
self.robot_3_status_pub = rospy.Publisher("/" + self.team_color + "/robot_3/status", Status, queue_size=10)
self.robot_4_status_pub = rospy.Publisher("/" + self.team_color + "/robot_4/status", Status, queue_size=10)
self.robot_5_status_pub = rospy.Publisher("/" + self.team_color + "/robot_5/status", Status, queue_size=10)
self.robot_6_status_pub = rospy.Publisher("/" + self.team_color + "/robot_6/status", Status, queue_size=10)
self.robot_7_status_pub = rospy.Publisher("/" + self.team_color + "/robot_7/status", Status, queue_size=10)
"""pid用のflag初期化"""
for id in range(self.robot_num):
self.robot[id].pid_init_flag = True
"""cmdの立ち上げ"""
self.cmd = [robot_commands() for i in range(self.robot_num)]
"""statusの立ち上げ"""
self.status = [Status() for i in range(self.robot_num)]
"""cmdのパラメータ初期化"""
for id in range(self.robot_num):
self.cmd[id].vel_surge = 0. #vel_x
self.cmd[id].vel_sway = 0. #vel_y
self.cmd[id].omega = 0. #vel_theta
self.cmd[id].kick_speed_x = 0
self.cmd[id].kick_speed_z = 0
self.cmd[id].dribble_power = 0
"""statusのパラメータ初期化"""
for id in range(self.robot_num):
self.status[id].status = "None"
"""positionの割り振り(動的にPositionが切り替わるのは多分大事、現状使っていない)"""
"""
if self.robot_num == 8:
self.robot[0].position = "GK"
self.robot[1].position = "LCB"
self.robot[2].position = "RCB"
self.robot[3].position = "LSB"
self.robot[4].position = "RSB"
self.robot[5].position = "LMF"
self.robot[6].position = "RMF"
elif self.robot_num == 4:
self.robot[0].position = "GK"
self.robot[1].position = "LCB"
self.robot[2].position = "CCB"
self.robot[3].position = "RCB"
"""
"""publishのrate(多分rospyのを使うからいらない)"""
self.publish_rate = 0.05 #s #
"""strategyの選択(いまはつかっていない)"""
self.strategy = None
"""refereeから受信する情報(現状cmdしか使っていない)"""
self.referee = None
"""---敵と味方どっちがボールをもっているか(現状使ってない)---"""
self.which_has_a_ball = None
"""---攻撃かdefenceか(これをうまく使っていきたい)---"""
self.attack_or_defence = None
"""---ポテンシャル法用のパラメータ（1/x）---"""
self.weight_enemy = 1.0
self.weight_goal = 100.0
self.delta = 0.0000001
"""---ポテンシャル法用のパラメータ（ガウス関数)---"""
self.weight_enemy_2 = 7.0
self.weight_goal_2 = 20000000.0
self.width_enemy = 0.1
self.width_goal = 0.1
self.delta_2 = 0.00001
# get parameters
friend_color = rospy.get_param('/friend_color')
# Define Subscriber
rospy.Subscriber("cmd_vel", geometry_msgs.msg.Twist, recvCmdvel)
rospy.Subscriber("kick_velocity", std_msgs.msg.Float32, recvKickVel)
rospy.Subscriber("ai_status", AIStatus, recvAIStatus)
r = rospy.Rate(60)
while not rospy.is_shutdown():
if G_is_cmdvel_received \
and G_is_kickvel_received \
and G_is_AIStatus_received:
commands = robot_commands()
commands.vel_surge = G_cmd_vel.linear.x
commands.vel_sway = G_cmd_vel.linear.y
commands.omega = G_cmd_vel.angular.z
commands.kick_speed_x = G_kick_vel.data
if G_AIStatus.do_chip:
commands.kick_speed_z = 1.0
commands.dribble_power = G_AIStatus.dribble_power
pub.publish(commands)
G_is_cmdvel_received = False
def __init__(self):
rospy.init_node("robot")
self.robot_color = str(rospy.get_param("~robot_color"))
self.robot_id = str(rospy.get_param("~robot_num"))
self.robot_total = 8
self.enemy_total = 8
self.cmd = robot_commands()
self.cmd.kick_speed_x = 0
self.cmd.kick_speed_z = 0
self.cmd.dribble_power = 0
self.command_pub = rospy.Publisher("/" + self.robot_color + "/robot_" + self.robot_id + "/robot_commands", robot_commands, queue_size=10)
# Composition
_objects = Objects(self.robot_color, self.robot_total, self.enemy_total)
self.robot_params = _objects.robot[int(self.robot_id)]
self.robot_friend = _objects.robot
self.robot_enemy = _objects.enemy
self.ball_params = _objects.ball
# self.pid = RobotPid(self.robot_params, self.ball_params, self.cmd, self.command_pub)
self.pid = RobotPid(self.robot_id, _objects, self.cmd, self.command_pub)
self.status = RobotStatus(self.pid, self.robot_params)
self.kick = RobotKick(self.ball_params, self.robot_params, self.pid, self.cmd, self.status, self.command_pub)
# listner 起動
# self.odom_listener()
#self.goal_pose_listener()
#self.target_pose_listener()
self.status_listener()
rospy.Timer(rospy.Duration(0.1), self.pid.replan_timerCallback)
#Loop 処理
self.loop_rate = rospy.Rate(ROBOT_LOOP_RATE)
print("Robot start: "+self.robot_id)
while not rospy.is_shutdown():
if self.status.robot_status == "move_linear":
self.pid.pid_linear(self.robot_params.get_future_position()[0], self.robot_params.get_future_position()[1],
self.robot_params.get_future_orientation())
if self.status.robot_status == "move_circle":
self.pid.pid_circle(self.pid.pid_circle_center_x, self.pid.pid_circle_center_y,
self.robot_params.get_future_position()[0],
self.robot_params.get_future_position()[1],
self.robot_params.get_future_orientation())
if self.status.robot_status == "kick":
self.kick.kick_x()
if self.status.robot_status == "pass":
self.kick.pass_ball(self.robot_params.get_pass_target_position()[0], self.robot_params.get_pass_target_position()[1])
if self.status.robot_status == "receive":
self.kick.recieve_ball(self.robot_params.get_pass_target_position()[0],self.robot_params.get_pass_target_position()[1])
self.loop_rate.sleep()
# def odom_listener(self):
# for i in range(self.robot_total):
# rospy.Subscriber("/" + self.robot_color + "/robot_" + str(i) + "/odom", Odometry, self.robot_params.friend_odom_callback, callback_args=i)
# for j in range(self.enemy_total):
# rospy.Subscriber("/" + self.robot_color + "/enemy_" + str(j) + "/odom", Odometry, self.robot_params.enemy_odom_callback, callback_args=j)
# rospy.Subscriber("/" + self.robot_color + "/ball_observer/estimation", Odometry, self.ball_params.odom_callback)
"""
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)