BTR2022の解説（game2022.py）

まずは，わかりやすい方から．
ゲームを実行するときに，game2022.py を使います．

以下，頭から見た構成です．

#!/usr/bin/python
TEAMNAME = "BabyTigers"

FIELDMINX = -5
FIELDMAXX = -1
FIELDMINY =  1
FIELDMAXY =  5
FIELDSIZEX = (FIELDMAXX - FIELDMINX) + 1
FIELDSIZEY = (FIELDMAXY - FIELDMINY) + 1
FIELDSIZE = FIELDSIZEX * FIELDSIZEY
MAXSTEP = 999

最初に定数を定義しています．
チーム名と経路探索用の情報です．
今は関係しませんので，読み飛ばしましょう．
その次にimport 関係が並んでいます．
（本当は，定数よりも先に書くべきなのかも…．あとで順番を入れ替えます）
ここも，今は読む必要はありません．
その後，再度，経路用の定数（配列変数）が並んでいますが，ここも読み飛ばしましょう．

def goToPoint(x, y, theta):
def moveRobotino(x, y, theta):
def goToInputVelt(): # original position is 355 (= 5 + 50*7)
def goToOutputVelt(): # original position is 305 (= 5 + 50*6)
def goToMPSCenter(distance):
def turnClockwise():
def turnCounterClockwise():
def getWork():
def putWork():

Robotino を動かすための昔のコードです．
今は使っていません．
この前まではrobotino2022.pyで，今はbtr2022.pyで扱うようにしています．
のちほど，削除します．

def beaconSignal(data):
def explorationInfo(data):
def gameState(data):
def machineInfo(data):
def machineReportInfo(data):
def orderInfo(data):
def ringInfo(data):
def navigationRoutes(data):
def sendBeacon():
def sendMachineReport(report):
def sendPrepareMachine(data):

RefBox との通信のプログラムです．読み飛ばして下さい．

def robotinoOdometry(data):
def robotinoVelocity(data):

RobViewを使っていた時のソースです．
OdometryはRefBoxにBeaconを送るのに必要なので，あとで修正します．
Velocityは，現Robotino用のROSだと，取ってこれません（必要かなぁ？）

def startGrasping():

Graspingチャレンジのプログラムです．
一部（ワークの把持とMPS周りの回転）のプログラムが本ソースを参照するようになっていますので，あとで修正します．

def initField():
def setField(x, y, number):
def getField(x, y):
def zoneToPose2D(zone):
def setMPStoField():
def getStep(x, y):
def wallCheck(x, y, dx, dy):
def getNextDirection(x, y):
def makeNextPoint(destination):

経路選択用の関数です．

def getNextPoint(pointNumber):
def startNavigation():

Navigationチャレンジのプログラムです．
RefBox から受け取った「未到達のMPSリスト」の先頭のゾーンに対して，経路を構築し，一つずつ進む（縦横のマス目に従って進む）形です．

# main
#
if __name__ == '__main__':

ここからがmain関数です　←　ここを最初に見ろと言う声もアリ．

  args = sys.argv
  if (len(args) >= 2):
    challenge = args[1]
    if (len(args) >= 3):
      robotNum = int(args[2])
    else:
      robotNum = 1

引数として，1つ目がchallenge名，2つ目がロボット番号です（省略したら1）．
RefBox やROS関係の初期設定があり，

  btrRobotino = btr2022.btr2022()

  pose = Pose2D()
  pose.x = -1000 * robotNum - 1500
  pose.y = 500
  pose.theta = 90

btr2022.py をbtrRobotinoとして持ってきています．
初期座標は，ロボット投入口です．
1台目は（-2500,500,90），2台目は（-3500,500,90），3台目は（-4500,500,90）となります．
※チームカラーを見ていない気がする．Magentaの時は反転しないといけませんね．

  if (challenge == "grasping"):
      startX =     [ -500, -4500, -500]
      startY =     [  500,  1500, 4500]
      startTheta = [   90,    90,  180]
      pose.x = startX[robotNum - 1]
      pose.y = startY[robotNum - 1]
      pose.theta = startTheta[robotNum - 1]
  if (challenge == "driving" or challenge == "positioning"):
      pose.x = zoneX["S31"]
      pose.y = zoneY["S31"]
      pose.theta = 90
  print(pose.x, pose.y, pose.theta)
  if (challenge == "reset"):
      # goToPoint(-800, -1500, 90)
      goToPoint(pose.x, pose.y, pose.theta)
      exit()

先ほどの座標はMain Track用ですので，challenge用の座標を個別に設定しています．
（第一引数にresetをいれると，とりあえず初期位置にロボットが直線的に移動します）

  print(challenge)
  challengeFlag = True
  # while True:
  while not rospy.is_shutdown():
    # sendBeacon()
    # print("sendBeacon")

    if (challenge == "test" and challengeFlag):

以降，challenge の値で分岐して，各プログラムが動きます．
プログラムの動作確認をしたいときは，上記のような感じでchallenge名をtest とかにして分岐を書けば，実行できます．
とりあえず，ざくっとgame2022.py の説明は以上です．