# WWW-POSIM route starter · IOES-Lab · MIT
# 課題: underwater_return · BlueROV2
#
# 目標: 水中障害物経路を進んで戻り、最後に浮上して保持します。
#
# 制御: 小型船体向け迂回、速度誤差の積分、6 推進器への配分を使います。
#
# 実行: インストール版 → 練習アリーナ → この課題・条件 → ROS 2 直接制御 → ワールド開始 → 採点開始。準備済みの Python ターミナルで python main.py
# を実行します。
#
# ウェブにはこのファイルを main.py としてアップロードします。サーバーが Python と SDK を準備します。SDK: www-posim-ros
# 0.4.2。認証情報をコードに含めません。
#
# 単位: map/world x,y,z はメートルで、+z は上、水中目標は負です。クォータニオン由来の yaw/roll はラジアン、mission の
# current_angle/current_swing は度です。命令は前進 m/s、yaw_rate rad/s、基本ジョイント順の推進力 N です。
#
# 時間: ROS 時刻・保持タイマー・dt はシミュレーション秒、max_age・duration は実時間秒です。duration=0.25 の間、SDK は 10 Hz
# で命令を更新し、戻る前に命令リースを解放します。
#
# 測定結果: 共通条件 5 回で 9.3347/100。WWW-POSIM 採点規則バージョン 5。
#
# 学習用制御器です。5 回の記録を確認し、一度に 1 変数を調整します。ゲイン・ゲートの仮定・船体余裕は付属モデル用です。実行ロジックを変えず、Python AST の一致を検査します。
#
# 採点原本:
# https://www-posim.vercel.app/course/examples/maritime-class-5/measured/underwater_return_controller.py
# コード解説: https://www-posim.vercel.app/course/examples.ja.html
# Ctrl+C または入力停止で終了します。Session の後処理で制御権を解放します。予期しない SDK エラーは表示し、このファイルではサーバー採点規則を設定しません。

"""A deliberately simple waypoint follower for surface and underwater challenges.

Choose a challenge in ROS 2 direct-control mode, start its evaluation, then run
this file. Positions, velocities and course targets come from the normal SDK.
The native evaluator owns the score and ends control when the run finishes.
"""
import math
import heapq
from www_posim_ros import Session


# 出力を正負対称に制限します。物理単位での上限は呼び出し側が指定します。
def clamp(value, limit):
    return max(-limit, min(limit, value))


# 方位誤差を [-pi, pi) ラジアンに折り返し、短い方向に旋回します。
def angle(value):
    return (value + math.pi) % (2 * math.pi) - math.pi


# 船体の余裕分だけブイの円を拡大し、小さな可視性グラフで Dijkstra 探索を行います。
def slalom_detours(home, targets, obstacles, hull_radius=3.5, obstacle_margin=1.3):
    """Walk around the published buoy circles with a small visibility graph."""
    # ゲートの柱には通路の余裕を保ち、その他のブイには選択した障害物余裕を適用します。
    def margin(obstacle):
        gate_post=any(t.get('gate') and abs(t['x']-obstacle['x'])<1e-6 for t in targets)
        return 1.3 if gate_post else obstacle_margin
    circles=[(o['x'],o['y'],hull_radius+o['radius_m']+margin(o)) for o in obstacles]
    result=[];previous=home
    # 拡大した障害物の円と交差する直線のグラフ辺を除外します。
    def clear(a,b):
        dx,dy=b['x']-a['x'],b['y']-a['y'];length=dx*dx+dy*dy
        for x,y,r in circles:
            t=max(0,min(1,((x-a['x'])*dx+(y-a['y'])*dy)/length)) if length else 0
            if math.hypot(a['x']+t*dx-x,a['y']+t*dy-y)<r:return False
        return True
    for goal in targets:
        nodes=[previous,goal]
        for x,y,r in circles:
            for i in range(8):
                theta=i*math.pi/4
                nodes.append(dict(x=x+(r+1.2)*math.cos(theta),y=y+(r+1.2)*math.sin(theta),z=goal['z']))
        distance={0:0};parent={};queue=[(0,0)]
        while queue:
            cost,i=heapq.heappop(queue)
            if cost!=distance[i]:continue
            if i==1:break
            for j,node in enumerate(nodes):
                if i==j or not clear(nodes[i],node):continue
                candidate=cost+math.hypot(node['x']-nodes[i]['x'],node['y']-nodes[i]['y'])
                if candidate<distance.get(j,float('inf')):
                    distance[j]=candidate;parent[j]=i;heapq.heappush(queue,(candidate,j))
        if 1 not in distance:raise ValueError('No clear route through published buoy geometry')
        route=[];i=1
        while i:route.append(nodes[i]);i=parent[i]
        result.extend(reversed(route));previous=goal
    return result


# 連続した計測姿勢を X 方向のゲート平面で補間し、幅と深度を確認します。
def gate_crossed(previous_pose, position, goal, previous_target, depth_tolerance):
    """Confirm passage through the published gate plane using observed poses."""
    if previous_pose is None:return False
    direction=1 if goal['x']>=previous_target['x'] else -1
    before=(previous_pose['x']-goal['x'])*direction
    after=(position['x']-goal['x'])*direction
    if not before<0<=after:return False
    fraction=(goal['x']-previous_pose['x'])/(position['x']-previous_pose['x'])
    y=previous_pose['y']+fraction*(position['y']-previous_pose['y'])
    z=previous_pose['z']+fraction*(position['z']-previous_pose['z'])
    return abs(y-goal['y'])<goal['gate']['usable_half_width_m'] and abs(z-goal['z'])<depth_tolerance


# 通過に失敗したら入口側へ戻り、横ずれと速度を整えて再進入します。
def gate_aim(position, goal, previous_target, recovering, passed, lateral_speed):
    """Stay on the entry side while cross-current alignment is unsettled."""
    direction=1 if goal['x']>=previous_target['x'] else -1
    side=(position['x']-goal['x'])*direction
    lateral=goal['y']-position['y']
    width=goal['gate']['usable_half_width_m']
    safe_lateral=width-min(.75,width/2)
    if passed:recovering=False
    elif side>=0 or abs(lateral)>safe_lateral:recovering=True
    if recovering and side<=-2 and abs(lateral)<min(.5,safe_lateral/2) and abs(lateral_speed)<.15:
        recovering=False
    aim_x=goal['x']+direction*(-2.5 if recovering else 1.5)
    return aim_x-position['x'],lateral,recovering


# 制限した機体速度誤差をシミュレーション dt でワールド軸に積分し、力の推定値を制限します。
def velocity_integral(ix, iy, vx, vy, linear, yaw, dt, gain=1.):
    """Estimate the needed world-frame force without winding up in transit."""
    ex,ey=vx-linear['x'],vy-linear['y']
    world_x=ex*math.cos(yaw)-ey*math.sin(yaw)
    world_y=ex*math.sin(yaw)+ey*math.cos(yaw)
    return (clamp(ix+gain*clamp(world_x,.6)*dt,15),
            clamp(iy+gain*clamp(world_y,.6)*dt,15))


def run():
    # インストール版またはサンドボックスのセッションを検出し、制御権を取得します。コンテキスト終了時に SDK セッションを閉じます。
    with Session.local(control=True) as sim:
        # セッションから今回の目標と条件を読みます。コース座標をコードに固定しません。
        mission = sim.profile.get('mission')
        if not mission or mission['task'] == 'surface_station':
            raise ValueError('Choose a route challenge; station keeping has its own starter.')
        underwater = mission['robot'] == 'bluerov2'
        # ROS メッセージ型でオドメトリを選び、車両固有のトピック名への依存を避けます。
        topic = next(name for name, kind in sim.topics.items()
                     if kind == 'nav_msgs/msg/Odometry')
        # 制御器の進行状態を、サーバーの独立した採点状態と分けて管理します。
        targets = mission['targets']
        # 水上での復路は障害物を横切らず、中間目標を逆順にたどります。
        if mission['task'] in ('surface_gates','surface_slalom'):
            # 通った通路を戻ります。出発点への直線はゲートの柱や障害物ブイを横切る場合があります。
            retrace = [{key:point[key] for key in ('x','y','z')}
                       for point in reversed(targets[:-1])]
            targets = targets[:-1]+retrace+targets[-1:]
        if mission['task']=='surface_slalom':
            # この課題で公開されている障害物に合わせ、余裕を持った経由点で経路を置き換えます。
            targets=slalom_detours(mission['home'],targets,mission['obstacles'])
        if mission['task']=='underwater_return':
            # この課題で公開されている障害物に合わせ、余裕を持った経由点で経路を置き換えます。
            targets=slalom_detours(mission['home'],targets,mission['obstacles'],
                                  hull_radius=.5,obstacle_margin=3.5)
        # 更新の間も目標番号、連続保持タイマー、ゲート再進入状態を保持します。
        index, holding_since = 0, None
        integral_x=integral_y=0.;previous_time=None;previous_pose=None;recovering_gate=False
        while True:
            # 実時間 0.5 秒以内に受信したサンプルを要求します。古い計測値は SDK エラーになります。
            message = sim.latest(topic, max_age=0.5)
            pose, twist = message['pose']['pose'], message['twist']['twist']
            p, q = pose['position'], pose['orientation']
            # クォータニオンを yaw（rad）へ変換します。経路制御では直立姿勢の安定化に roll も求めます。
            yaw = math.atan2(2*(q['w']*q['z']+q['x']*q['y']),
                             1-2*(q['y']**2+q['z']**2))
            roll = math.atan2(2*(q['w']*q['x']+q['y']*q['z']),
                              1-2*(q['x']**2+q['y']**2))
            # 積分と保持時間には ROS のシミュレーション時刻を使い、命令の実時間長と区別します。
            stamp = message['header']['stamp']
            now = stamp['sec']+stamp['nanosec']*1e-9
            dt=0 if previous_time is None else max(0,min(0.5,now-previous_time))
            previous_time=now
            # 現在の目標を選びます。位置と目標座標は同じ map/world 座標系です。
            goal = targets[min(index, len(targets)-1)]
            previous = mission['home'] if index == 0 else targets[index-1]
            dx, dy = goal['x']-p['x'], goal['y']-p['y']
            distance = math.hypot(dx, dy)
            passage = False
            if underwater and goal.get('gate'):
                passage=gate_crossed(previous_pose,p,goal,previous,mission['depth_tolerance_m'])
                velocity=twist['linear']
                lateral_speed=velocity['x']*math.sin(yaw)+velocity['y']*math.cos(yaw)
                dx,dy,recovering_gate=gate_aim(p,goal,previous,recovering_gate,passage,lateral_speed)
            segment = math.atan2(goal['y']-previous['y'], goal['x']-previous['x'])
            passed = (p['x']-goal['x'])*math.cos(segment)+(p['y']-goal['y'])*math.sin(segment)
            near = distance < (0.5 if underwater else 0.8)
            depth_ok = not underwater or abs(p['z']-goal['z']) < 0.2
            desired = goal.get('yaw', segment) if near and goal.get('hold') else math.atan2(dy, dx)
            if goal.get('gate') and not underwater:
                # ゲート平面の先を狙います。手前で曲がると通過を逃す場合があります。
                lead=max(0.3,min(2,distance*0.1))
                desired = math.atan2(dy+lead*math.sin(segment), dx+lead*math.cos(segment))
            # 公開された変動海流の位相に応じて保持方位を調整します。
            if goal.get('yaw_current_follow') and near:
                elapsed = now-mission['current_epoch_sim_time']
                desired += math.radians(mission['current_swing'])*math.sin(
                    2*math.pi*elapsed/mission['current_period']+mission['current_phase'])
            yaw_error = angle(desired-yaw)
            # このローカル到着判定は制御器の目標を進めるだけで、実際の点数はサーバーが判定します。
            reached = (passage if underwater and goal.get('gate') else
                       (passed > 0 and distance < 2 if goal.get('gate') else near) and depth_ok)
            # 位置・深度・方位の条件を外れたら保持タイマーをリセットします。保持は連続して必要です。
            if goal.get('hold'):
                reached = reached and abs(yaw_error) < mission['heading_tolerance_rad']
                holding_since = (now if holding_since is None else holding_since) if reached else None
                reached = holding_since is not None and now-holding_since >= goal['hold']
            if reached and index < len(targets)-1:
                index += 1
                holding_since = None
                recovering_gate = False
            previous_pose = dict(p)
            try:
                # WAM-V は機体の前進速度と yaw 角速度の上位命令を使います。
                if not underwater:
                    if near and goal.get('hold'):
                        forward=clamp(0.3*(dx*math.cos(yaw)+dy*math.sin(yaw)),0.25)
                    else:
                        # 公開された評価条件から定常海流を取得します。
                        # 狭いゲートの前で上流を狙うように差し引きます。この例は突風や波の動きを推定しません。
                        speed=min(0.6,0.3*distance)
                        flow=math.radians(mission['current_angle'])
                        gx,gy=speed*math.cos(desired),speed*math.sin(desired)
                        desired=math.atan2(gy-mission['current_speed']*math.sin(flow),
                                           gx-mission['current_speed']*math.cos(flow))
                        yaw_error=angle(desired-yaw)
                        turn_limit=0.35 if mission['task']=='surface_slalom' else 0.7
                        forward=max(0,gx*math.cos(yaw)+gy*math.sin(yaw)) if abs(yaw_error)<turn_limit else 0.
                    sim.set_velocity(forward=forward, yaw_rate=clamp(0.6*yaw_error, 0.5), duration=0.25)
                else:
                    # 付属の 6 推進器モデルのジョイント順で配分します。
                    # 漂流補償の推定値と 6 推進器それぞれの命令を制限します。
                    # 検査コースには長い移動区間が 3 つあります。
                    # 移動を少し速め、復路の時間を残します。
                    # 共通の水中制御ループが課題別の移動ゲインと速度制限を選びます。
                    inspection = mission['task']=='underwater_path'
                    returning = mission['task']=='underwater_return'
                    gain = 0.24 if inspection else 0.17
                    vx = clamp(gain*(dx*math.cos(yaw)+dy*math.sin(yaw)), 0.3 if returning else 0.6 if inspection else 0.45)
                    vy = clamp(gain*(-dx*math.sin(yaw)+dy*math.cos(yaw)), 0.25 if returning else 0.5 if inspection else 0.35)
                    linear, angular = twist['linear'], twist['angular']
                    if mission['task']=='underwater_return':
                        # 長い旋回中に位置だけを積分すると、標識を越えて近くの柱へ加速し続けることがあります。
                        # 定常海流には緩やかに適応し、変動海流には変化する力へ速く応答します。
                        integral_x,integral_y=velocity_integral(
                            integral_x,integral_y,vx,vy,linear,yaw,dt,
                            gain=4. if mission.get('current_gust',0)>0 else 1.)
                    else:
                        integral_x=clamp(integral_x+0.25*clamp(dx,1)*dt,15)
                        integral_y=clamp(integral_y+0.25*clamp(dy,1)*dt,15)
                    # 機体速度のフィードバックに、ワールド軸の漂流推定を機体軸へ戻した値を加えます。
                    fx = 10*(vx-linear['x'])+integral_x*math.cos(yaw)+integral_y*math.sin(yaw)
                    fy = 10*(vy-linear['y'])-integral_x*math.sin(yaw)+integral_y*math.cos(yaw)
                    # yaw/roll のフィードバックで姿勢を安定化し、垂直 PD と固定トリムで負の z の深度目標を制御します。
                    mz = 0.7*yaw_error-0.4*angular['z']
                    fz = 12*(goal['z']-p['z'])-8*linear['z']-2
                    mx = -2*roll-0.4*angular['x']
                    # 付属モデルの形状に従って力とトルクを配分します。水平推進器は 45 度、レバーアームはメートル単位です。
                    a, b, c = fx/(4*math.sqrt(0.5)), fy/(4*math.sqrt(0.5)), mz/(4*0.167)
                    forces = [-a-b-c, -a+b+c, a-b+c, a+b-c,
                              -fz/2+mx/(2*0.109), -fz/2-mx/(2*0.109)]
                    # 基本ジョイント順で 6 個の力を送り、この例では各推進器を 35 N に制限します。
                    sim.set_thrusters([clamp(force, 35) for force in forces], duration=0.25)
            # 正常な制御権の解放のみでループを終了し、予期しないエラーは診断できるよう表示します。
            except RuntimeError as error:
                if str(error) != 'control_lease_ended':
                    raise
                break


if __name__ == '__main__':
    run()
