def _path_tracking(self, color, robot_id, path):
ARRIVED_THRESH = 0.5 # meters
arrived = False
command = RobotCommand()
command.robot_id = robot_id
path_index = self._path_index[color][robot_id]
robot_pose = self._robot_info[color][robot_id].pose
if len(path) == 0:
# パスがセットされてない場合
# 何もせず初期値のcommandを返す
pass
elif path_index == len(path):
# ゴールまで到達した場合
# 最後のposeに移動し続ける
pose = path[-1]
command = self._move_to_pose(color, robot_id, pose)
# ゴールに到着したか判定
if distance_2_poses(pose, robot_pose) < self._ARRIVED_THRESH:
arrived = True
else:
# 経路追従
pose = path[path_index]
command = self._move_to_pose(color, robot_id, pose)
# poseに近づいたか判定する
if distance_2_poses(pose, robot_pose) < self._APPROACH_THRESH:
# 近づいたら次の経由位置へ向かう
self._path_index[color][robot_id] += 1
return command, arrived
def _control_update(self, color, robot_id):
# ロボットの走行制御を更新する
# ControlTargetを受け取ってない場合は停止司令を生成する
command = RobotCommand()
command.robot_id = robot_id
control_target = self._control_target[color][robot_id]
# 制御目標が有効であれば
if control_target.control_enable:
# 経路がセットされていれば
if len(control_target.path) != 0:
command, arrived = self._path_tracking(color, robot_id,
control_target.path)
command.kick_power = control_target.kick_power
command.chip_enable = control_target.chip_enable
command.dribble_power = control_target.dribble_power
if arrived:
# 到達したことをpublish
self._pubs_is_arrived[color][robot_id].publish(True)
else:
# 到達してないことをpublish
self._pubs_is_arrived[color][robot_id].publish(False)
else:
# 保存していた制御速度をリセットする
self._reset_control_velocity(color, robot_id)
return command
def _make_command_velocity(self, color, robot_id, goal_velocity):
command = RobotCommand()
command.robot_id = robot_id
robot_info = self._robot_info[color][robot_id]
current_control_velocity = self._control_velocity[color][robot_id]
# 制御速度の生成
robot_velocity = robot_info.velocity
new_control_velocity = self._velocity_control(
goal_velocity, current_control_velocity)
# 速度方向をロボット座標系に変換
theta = robot_info.pose.theta
command.vel_surge = math.cos(
theta) * new_control_velocity.x + math.sin(
theta) * new_control_velocity.y
command.vel_sway = -math.sin(theta) * new_control_velocity.x + math.cos(
theta) * new_control_velocity.y
command.vel_angular = new_control_velocity.theta
# 制御速度の保存
self._control_velocity[color][robot_id] = new_control_velocity
return command
def _make_command(self, color, robot_id):
# ロボットの動作司令を生成する
# ControlTargetを受け取ってない場合は停止司令を生成する
command = RobotCommand()
command.robot_id = robot_id
control_target = self._control_target[color][robot_id]
# 経路がセットされていれば
if len(control_target.path) != 0:
command, arrived = self._path_tracking(color, robot_id,
control_target.path)
if arrived:
# 到達したことをpublish
self._pubs_is_arrived[color][robot_id].publish(True)
else:
# 到達してないことをpublish
self._pubs_is_arrived[color][robot_id].publish(False)
else:
rospy.logdebug("Velocity Control")
command = self._make_command_velocity(color, robot_id,
control_target.goal_velocity)
# キック・ドリブルパラメータのセット
command.kick_power = control_target.kick_power
command.chip_enable = control_target.chip_enable
command.dribble_power = control_target.dribble_power
return command
def _move_to_pose(self, color, robot_id, goal_pose):
command = RobotCommand()
command.robot_id = robot_id
robot_info = self._robot_info[color][robot_id]
current_control_velocity = self._control_velocity[color][robot_id]
# 制御速度を計算
# self._control_velocityにも同値を保存する
control_velocity = self._pid_pose_control(color, robot_id,
robot_info.pose, goal_pose,
current_control_velocity)
# 速度方向をロボット座標系に変換
theta = robot_info.pose.theta
command.vel_surge = math.cos(theta) * control_velocity.x + math.sin(
theta) * control_velocity.y
command.vel_sway = -math.sin(theta) * control_velocity.x + math.cos(
theta) * control_velocity.y
command.vel_angular = control_velocity.theta
return command
def _direct_control_update(self):
# 直接ロボットを操縦する
robot_commands = RobotCommands()
robot_commands.header.stamp = rospy.Time.now()
command = RobotCommand()
# メッセージを取得してない場合は抜ける
if self._joy_msg is None:
return
# シャットダウン
if self._joy_msg.buttons[self._BUTTON_SHUTDOWN_1] and\
self._joy_msg.buttons[self._BUTTON_SHUTDOWN_2]:
rospy.signal_shutdown('finish')
return
# チームカラーの変更
if self._joy_msg.buttons[self._BUTTON_COLOR_ENABLE]:
if math.fabs(self._joy_msg.axes[self._AXIS_COLOR_CHANGE]) > 0:
self._is_yellow = not self._is_yellow
print 'is_yellow: ' + str(self._is_yellow)
# キーが離れるまでループ
while self._joy_msg.axes[self._AXIS_COLOR_CHANGE] != 0:
pass
# IDの変更
if self._joy_msg.buttons[self._BUTTON_ID_ENABLE]:
if math.fabs(self._joy_msg.axes[self._AXIS_ID_CHANGE]) > 0:
# 十字キーの入力に合わせて、IDを増減させる
self._robot_id += int(self._joy_msg.axes[self._AXIS_ID_CHANGE])
if self._robot_id > self._MAX_ID:
self._robot_id = self._MAX_ID
if self._robot_id < 0:
self._robot_id = 0
print 'robot_id:' + str(self._robot_id)
# キーが離れるまでループ
while self._joy_msg.axes[self._AXIS_ID_CHANGE] != 0:
pass
# 全ID操作の変更
if self._joy_msg.buttons[self._BUTTON_ALL_ID_1] and \
self._joy_msg.buttons[self._BUTTON_ALL_ID_2] and \
self._joy_msg.buttons[self._BUTTON_ALL_ID_3] and \
self._joy_msg.buttons[self._BUTTON_ALL_ID_4]:
self._all_member = not self._all_member
print 'all_member: ' + str(self._all_member)
# キーが離れるまでループ
while self._joy_msg.buttons[self._BUTTON_ALL_ID_1] != 0 or \
self._joy_msg.buttons[self._BUTTON_ALL_ID_2] != 0 or\
self._joy_msg.buttons[self._BUTTON_ALL_ID_3] != 0 or\
self._joy_msg.buttons[self._BUTTON_ALL_ID_4] != 0:
pass
# 走行
if self._joy_msg.buttons[self._BUTTON_MOVE_ENABLE]:
command.vel_surge = self._joy_msg.axes[
self._AXIS_VEL_SURGE] * self._MAX_VEL_SURGE
command.vel_sway = self._joy_msg.axes[
self._AXIS_VEL_SWAY] * self._MAX_VEL_SWAY
command.vel_angular = self._joy_msg.axes[
self._AXIS_VEL_ANGULAR] * self._MAX_VEL_ANGULAR
# キック
if self._joy_msg.buttons[self._BUTTON_KICK_ENABLE]:
if self._joy_msg.buttons[self._BUTTON_KICK_STRAIGHT]:
command.kick_power = self._kick_power
elif self._joy_msg.buttons[self._BUTTON_KICK_CHIP]:
command.kick_power = self._kick_power
command.chip_enable = True
# パワーの変更
axis_value = self._joy_msg.axes[self._AXIS_KICK_POWER]
if math.fabs(axis_value) > 0:
self._kick_power += math.copysign(self._KICK_POWER_CONTROL,
axis_value)
if self._kick_power > self._MAX_KICK_POWER:
self._kick_power = self._MAX_KICK_POWER
if self._kick_power < 0.001:
self._kick_power = 0.0
print 'kick_power :' + str(self._kick_power)
# キーが離れるまでループ
while self._joy_msg.axes[self._AXIS_KICK_POWER] != 0:
pass
# ドリブラー
if self._joy_msg.buttons[self._BUTTON_DRIBBLE_ENABLE]:
command.dribble_power = self._dribble_power
# パワーの変更
axis_value = self._joy_msg.axes[self._AXIS_DRIBBLE_POWER]
if math.fabs(axis_value) > 0:
self._dribble_power += math.copysign(
self._DRIBBLE_POWER_CONTROL, axis_value)
if self._dribble_power > self._MAX_DRIBBLE_POWER:
self._dribble_power = self._MAX_DRIBBLE_POWER
if self._dribble_power < 0.001:
self._dribble_power = 0.0
print 'dribble_power:' + str(self._dribble_power)
# キーが離れるまでループ
while self._joy_msg.axes[self._AXIS_DRIBBLE_POWER] != 0:
pass
# チームカラーをセット
robot_commands.is_yellow = self._is_yellow
# IDとコマンドをセット
if self._all_member:
# 全IDのロボットを動かす
for robot_id in range(self._MAX_ID + 1):
command.robot_id = robot_id
robot_commands.commands.append(copy.deepcopy(command))
else:
command.robot_id = self._robot_id
robot_commands.commands.append(command)
self._pub_commands.publish(robot_commands)