AFSim学习-自定义Processor开发4-与python交互通过文件

新建文件

在src/core/wsf/source/processor/下新建文件：
RelativeState.hpp；
WsfSimpleControllerProcessor.hpp;
WsfSimpleControllerProcessor.cpp。

说明

RelativeState.hpp

RelativeState.hpp文件是自己定义的相对态势结构体。

#ifndef RELATIVE_STATE_HPP
#define RELATIVE_STATE_HPP

struct RelativeState
{
   bool valid = false;

   // Distance
   double range = 0.0;
   double horizontalRange = 0.0;

   // Altitude
   double altitudeDifference = 0.0;

   // Speed
   double ownSpeed = 0.0;
   double targetSpeed = 0.0;
   double closingSpeed = 0.0;

   // LOS in WCS/ECEF
   double losX = 0.0;
   double losY = 0.0;
   double losZ = 0.0;

   // Target local ENU position
   double east = 0.0;
   double north = 0.0;
   double up = 0.0;

   // Angular state
   double bearingDeg = 0.0;
   double courseDeg = 0.0;
   double relativeBearingDeg = 0.0;
   double elevationDeg = 0.0;
};

#endif

WsfSimpleControllerProcessor.hpp

WsfSimpleControllerProcessor.hpp文件。
相比自定义Processor开发3，
1、加入：C++ 写给 Python 的状态文件；Python 写入动作的文件；等待 Python 动作的最长时间；RL 交互步数。

   std::string mTargetName; // 目标平台名称
   std::string mDecisionFilename; // 决策输出文件名
   std::ofstream mDecisionFile;// CSV 文件流// 用于写入控制器输出结果

   std::string mObservationFilename;   // C++ 写给 Python 的状态文件
   std::string mActionFilename;     // Python 写入动作的文件
   double mMaxHeadingDeltaDeg;      // 限制最大转向修正角，防止动作太大
   double mActionWaitTimeoutSec;       // 等待 Python 动作的最长时间
   int mRlStepIndex;                   // RL 交互步数

2、把之前WsfStateLoggerProcessor的位置都替换为WsfSimpleControllerProcessor。

WsfSimpleControllerProcessor.cpp

相比自定义Processor开发3，
1、构造函数和拷贝构造函数，加一下：

   , mTargetName("")
   , mDecisionFilename("controller_decision.csv")
   , mObservationFilename("observation.txt")
   , mActionFilename("action.txt")
   , mMaxHeadingDeltaDeg(10.0)
   , mActionWaitTimeoutSec(30.0)
   , mRlStepIndex(0)

   , mTargetName(aSrc.mTargetName)
   , mDecisionFilename(aSrc.mDecisionFilename)
   , mObservationFilename(aSrc.mObservationFilename)
   , mActionFilename(aSrc.mActionFilename)
   , mMaxHeadingDeltaDeg(aSrc.mMaxHeadingDeltaDeg)
   , mActionWaitTimeoutSec(aSrc.mActionWaitTimeoutSec)
   , mRlStepIndex(0)

2、Initialize函数改为打开决策文件

bool WsfSimpleControllerProcessor::Initialize(double aSimTime)
{
    // Initialize 打开决策文件
    bool ok = WsfProcessor::Initialize(aSimTime);
    std::cout<<"=====Initialize====="<<std::endl;

    mDecisionFile.open(mDecisionFilename.c_str(), std::ios::out);// std::ios::out 表示以写入模式打开。
    if (!mDecisionFile.is_open())
    {
        std::cout << "Open " << mDecisionFilename << " FAILED!" << std::endl;
        return false;
    }

    std::cout << "Open " << mDecisionFilename << " SUCCESS!" << std::endl;
    mDecisionFile
      << "time,ownship,target,range_m,relative_bearing_deg,"
      << "elevation_deg,course_deg,heading_delta_deg,desired_heading_deg,"
        << "decision,nx,ny,nz\n";

    return ok;
}

3、ProcessInput函数，加一些指令参数

bool WsfSimpleControllerProcessor::ProcessInput(UtInput& aInput)
{
    std::string command(aInput.GetCommand());
    if (command == "target")
    {
      aInput.ReadValue(mTargetName);
      return true;
    }
    if (command == "decision_file")
    {
       aInput.ReadValue(mDecisionFilename);
       return true;
    }
    if (command == "observation_file")
    {
        aInput.ReadValue(mObservationFilename);
        return true;
    }

    if (command == "action_file")
    {
        aInput.ReadValue(mActionFilename);
        return true;
    }

    if (command == "max_heading_delta_deg")
    {
        aInput.ReadValue(mMaxHeadingDeltaDeg);
        return true;
    }

    if (command == "action_wait_timeout_sec")
    {
        aInput.ReadValue(mActionWaitTimeoutSec);
        return true;
    }
   
   
   return WsfProcessor::ProcessInput(aInput);
    
}

4、Update函数逻辑
先读取所属平台，
再读取目标平台，
再计算相对态势，
接着拿到mover对象，负责平台运动的对象：

// 拿到 mover
    WsfMover* mover = ownship->GetMover();// mover 是真正负责平台运动的对象。
    if (mover == nullptr)
    {
        std::cout << "[Control] No mover on platform: "
              << ownship->GetName() << std::endl;
        return;
    }

    //==========================================================
    // ② 把 mover 转成 WsfWaypointMover
    //==========================================================

    // 由于当前使用的是 WSF_AIR_MOVER，
    // 而 WSF_AIR_MOVER 对应的 WsfAirMover 继承自 WsfWaypointMover，
    // 所以这里可以尝试 dynamic_cast。
    WsfWaypointMover* waypointMover = dynamic_cast<WsfWaypointMover*>(mover);
    // 如果转换失败，说明当前平台用的 mover 不是 WsfWaypointMover 类型，
    // 也就不能调用 TurnToHeading()。
    if (waypointMover == nullptr)
    {
        std::cout << "[Control] Mover is not WsfWaypointMover: "
                << ownship->GetName() << std::endl;
        return;
    }

删除旧 action，准备等待 Python 写新 action：

std::remove(mActionFilename.c_str());

写 observation 给 Python：

// 当前 RL 步号
    int stepId = mRlStepIndex;

    // 为了避免 Python 读到半截文件，先写临时文件，再 rename 成正式文件。
    std::string obsTmpFilename = mObservationFilename + ".tmp";

    std::ofstream obsFile(obsTmpFilename.c_str(), std::ios::out);

    if (obsFile.is_open())
    {
        // observation 格式：
        // step_id time range relative_bearing elevation closing_speed course
        obsFile
            << stepId << " "
            << aSimTime << " "
            << state.range << " "
            << state.relativeBearingDeg << " "
            << state.elevationDeg << " "
            << state.closingSpeed << " "
            << state.courseDeg
            << std::endl;

        obsFile.close();

        // 删除旧 observation 文件
        std::remove(mObservationFilename.c_str());

        // 把 tmp 文件改名为正式 observation 文件
        std::rename(obsTmpFilename.c_str(), mObservationFilename.c_str());
    }
    else
    {
        std::cout << "[RL] Failed to write observation file: "
                << mObservationFilename << std::endl;
    }

从 Python 动作文件读取heading_delta_deg：
用循环读取，每10ms读取一次，如果打开文件了，判断action 文件里的 step_id。

// headingDeltaDeg 是 Python 输出的动作。
    // 单位：度。
    // 含义：
    // 负数 = 向左修正
    // 正数 = 向右修正
    // 0    = 保持当前航向
    double headingDeltaDeg = 0.0;
    bool gotAction = false;

    const int sleepMs = 10;
    int waitedMs = 0;
    int maxWaitMs = static_cast<int>(mActionWaitTimeoutSec * 1000.0);

    while (waitedMs <= maxWaitMs)
    {
        std::ifstream actionFile(mActionFilename.c_str());

        if (actionFile.is_open())
        {
            int actionStepId = -1;
            double actionValue = 0.0;

            actionFile >> actionStepId >> actionValue;
            actionFile.close();

            // 只有 action 文件里的 step_id 和当前 stepId 一致，才接受这个动作。
            if (actionStepId == stepId)
            {
                headingDeltaDeg = actionValue;
                gotAction = true;
                break;
            }
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(sleepMs));
        waitedMs += sleepMs;
    }
    if (!gotAction)
    {
        std::cout << "[RL] Timeout waiting action for step "
                << stepId
                << ", use headingDeltaDeg = 0"
                << std::endl;

        headingDeltaDeg = 0.0;
    }

控制飞机转向：

// TurnToHeading() 需要弧度，所以这里从度转弧度。
    double desiredHeadingRad = desiredHeadingDeg * UtMath::cRAD_PER_DEG;
    // 使用 mover 默认转弯约束。
    double radialAccel = -1.0;
    // 选择最短方向转弯。
    WsfPath::TurnDirection turnDirection = WsfPath::cTURN_DIR_SHORTEST;
    // ==========================================================
    // 真正控制飞机转向
    // ==========================================================
    waypointMover->TurnToHeading(
        aSimTime,
        desiredHeadingRad,
        radialAccel,
        turnDirection
    );

编译要注意

1、注册 processor 类型，
在WsfProcessorTypes.cpp里：

#include "WsfSimpleControllerProcessor.hpp"
AddCoreType("MY_SIMPLE_CONTROLLER_PROCESSOR", ut::make_unique<WsfSimpleControllerProcessor>(aScenario));

2、需要删除重新编译

rm -rf buil
mkdir -p buil
cmake -S src -B buil
cmake --build buil --target wsf_util -j10
cmake --build buil --target wsf -j12
cmake --build buil --target wsf_parser -j12
cmake --build buil --target mission -j12

python交互文件

1、读observation

        with open(OBS_FILE, "r", encoding="utf-8") as f:
            line = f.readline().strip()
        step_id = int(parts[0])
        sim_time = float(parts[1])
        range_m = float(parts[2])
        relative_bearing_deg = float(parts[3])
        elevation_deg = float(parts[4])
        closing_speed = float(parts[5])
        course_deg = float(parts[6])

2、写动作/控制量

    # 写 action 时也用 tmp + rename，防止 C++ 读到半截文件
    tmp_file = ACTION_FILE + ".tmp"

    with open(tmp_file, "w", encoding="utf-8") as f:
        f.write(f"{step_id} {heading_delta_deg}\n")

    os.replace(tmp_file, ACTION_FILE)

总体代码

rl_policy.py

import os
import time

OBS_FILE = "/home/ubuntu/afsim_ws/project1/11-two-platform-logger-2/observation.txt"
ACTION_FILE = "/home/ubuntu/afsim_ws/project1/11-two-platform-logger-2/action.txt"

MAX_HEADING_DELTA_DEG = 10.0

last_step = -1


def clamp(x, low, high):
    return max(low, min(high, x))


print("[Python RL] waiting for observation...")

while True:
    if not os.path.exists(OBS_FILE):
        time.sleep(0.01)
        continue

    try:
        with open(OBS_FILE, "r", encoding="utf-8") as f:
            line = f.readline().strip()
    except Exception:
        time.sleep(0.01)
        continue

    if not line:
        time.sleep(0.01)
        continue

    parts = line.split()

    if len(parts) < 7:
        time.sleep(0.01)
        continue

    try:
        step_id = int(parts[0])
        sim_time = float(parts[1])
        range_m = float(parts[2])
        relative_bearing_deg = float(parts[3])
        elevation_deg = float(parts[4])
        closing_speed = float(parts[5])
        course_deg = float(parts[6])
    except ValueError:
        time.sleep(0.01)
        continue

    # 避免重复处理同一个 observation
    if step_id == last_step:
        time.sleep(0.01)
        continue

    last_step = step_id

    # ======================================================
    # 这里先写一个简单策略，不是真正 RL
    # 目标在右边 relative_bearing > 0，就右转
    # 目标在左边 relative_bearing < 0，就左转
    # ======================================================

    heading_delta_deg = clamp(
        relative_bearing_deg,
        -MAX_HEADING_DELTA_DEG,
        MAX_HEADING_DELTA_DEG
    )

    # 写 action 时也用 tmp + rename，防止 C++ 读到半截文件
    tmp_file = ACTION_FILE + ".tmp"

    with open(tmp_file, "w", encoding="utf-8") as f:
        f.write(f"{step_id} {heading_delta_deg}\n")

    os.replace(tmp_file, ACTION_FILE)

    print(
        f"[Python RL] step={step_id}, "
        f"t={sim_time}, "
        f"range={range_m:.1f}, "
        f"rb={relative_bearing_deg:.3f}, "
        f"action={heading_delta_deg:.3f}"
    )

WsfSimpleControllerProcessor.hpp

// Qiu写的
// WsfSimpleControllerProcessor.hpp
// 作用：声明一个自定义的 AFSIM Processor。
// 这个 Processor 用于：
// 1. 获取当前平台 ownship。
// 2. 根据目标平台 target 计算相对态势。
// 3. 输出控制决策。
// 4. 调用 WSF_AIR_MOVER / WsfWaypointMover 的控制接口，让平台朝目标飞。


// ① 头文件保护（Header Guard）
// 防止同一个头文件被重复包含（#include 多次）。
#ifndef WSFSIMPLECONTROLLERPROCESSOR_HPP
#define WSFSIMPLECONTROLLERPROCESSOR_HPP

// ============================================================
// ② C++ 标准库头文件
// ============================================================
// sqrt()计算速度大小
#include <cmath>
// 输出到终端
#include <iostream>
#include <fstream>// std::ofstream 用于向 CSV 文件写入控制器输出结果。
#include <string>// std::string 用于保存目标平台名称、输出文件名等字符串。

// ============================================================
// ③ AFSIM 导出宏
// ------------------------------------------------------------
// WSF_EXPORT 是 AFSIM 的导出宏。
// 在 Windows 下，它可能展开为 __declspec(dllexport) 或 dllimport。
// 在 Linux 下通常为空。
// 写这个宏是为了让类可以被 AFSIM 插件系统正确识别和链接。
// ============================================================
#include "wsf_export.h"


// ============================================================
// ④ 前向声明 Forward Declaration
// ------------------------------------------------------------
// 这里只告诉编译器：有这些类名存在。
// 因为这里只用到了指针或引用，不需要知道类的完整结构。
// 真正调用这些类的成员函数时，在 .cpp 里 include 对应头文件。
// ============================================================
class UtInput;
class WsfPlatform;
class WsfScenario;


// ============================================================
// ⑤ AFSIM 父类头文件
// ------------------------------------------------------------
// 因为本类要继承 WsfProcessor，所以这里必须 include 完整定义，
// 不能只写 class WsfProcessor;。
// ============================================================
#include "WsfProcessor.hpp"

// ============================================================
// ⑥ 自己定义的相对态势结构体
// ------------------------------------------------------------
// RelativeState 里面保存相对距离、相对方位角、视线向量、
// 目标速度、接近速度等信息。
// 本类的 CalculateRelativeState() 返回 RelativeState，
// 所以这里需要 include。
// ============================================================
#include "RelativeState.hpp"





// ============================================================
// ⑦ 类定义
// ------------------------------------------------------------
// WsfSimpleControllerProcessor 继承自 WsfProcessor。
// 它本质上是一个挂在平台上的“处理器”，
// AFSIM 会按照 update_interval 周期调用它的 Update()。
// ============================================================
class WSF_EXPORT WsfSimpleControllerProcessor : public WsfProcessor
{
public:
   // ---------------------------------------------------------
   // 构造函数
   // ---------------------------------------------------------
   // 创建 Processor 对象时调用。
   // aScenario 表示当前仿真场景。
   // AFSIM 注册 Processor 类型时，会用这个构造函数创建模板对象。
   // ---------------------------------------------------------
   WsfSimpleControllerProcessor(WsfScenario& aScenario);

   // ---------------------------------------------------------
   // 禁止赋值
   // ---------------------------------------------------------
   // 例如下面这种写法会被禁止：
   //
   // WsfSimpleControllerProcessor a(...);
   // WsfSimpleControllerProcessor b(...);
   // a = b;  // 编译错误
   //
   // 因为 Processor 内部可能包含文件流、平台指针等资源，
   // 直接赋值容易造成资源混乱。
   // ---------------------------------------------------------
   WsfSimpleControllerProcessor& operator=(const WsfSimpleControllerProcessor&) = delete;

   // ---------------------------------------------------------
   // 析构函数
   // ---------------------------------------------------------
   // 对象销毁时调用。
   // 如果 .cpp 中打开了文件，可以在析构函数中关闭文件。
   // override 表示重写父类 WsfProcessor 的虚析构函数。
   // ---------------------------------------------------------
   ~WsfSimpleControllerProcessor() override;

   // ---------------------------------------------------------
   // Clone()
   // ---------------------------------------------------------
   // AFSIM 中 platform_type 里的 Processor 通常先作为模板存在。
   // 当具体平台 blue_1、red_1 被创建时，
   // AFSIM 会调用 Clone() 复制出每个平台自己的 Processor 实例。
   //
   // 简单理解：
   // 模板 Processor -> Clone() -> blue_1 上真正运行的 Processor
   // ---------------------------------------------------------
   WsfProcessor* Clone() const override;

   // ---------------------------------------------------------
   // Initialize()
   // ---------------------------------------------------------
   // 仿真初始化阶段调用。
   // 一般用于：
   // 1. 打开 CSV 文件。
   // 2. 写入表头。
   // 3. 检查参数是否有效。
   // ---------------------------------------------------------
   bool Initialize(double aSimTime) override;

   // ---------------------------------------------------------
   // ProcessInput()
   // ---------------------------------------------------------
   // 解析 mission script 中 processor 块里的命令。
   //
   // 例如场景文件中：
   //
   // processor controller MY_SIMPLE_CONTROLLER_PROCESSOR
   //    update_interval 10 sec
   //    target red_1
   //    decision_file controller_decision.csv
   // end_processor
   //
   // 其中 target 和 decision_file 就是在这里解析。
   // update_interval 通常交给父类 WsfProcessor 解析。
   // ---------------------------------------------------------
   bool ProcessInput(UtInput& aInput) override;

   // ---------------------------------------------------------
   // Update()
   // ---------------------------------------------------------
   // Processor 最核心的函数。
   // AFSIM 仿真推进时，会按照 update_interval 周期调用它。
   //
   // 当前控制器的主要逻辑：
   // 1. 获取当前平台 ownship。
   // 2. 根据 mTargetName 找到目标平台 target。
   // 3. 计算相对态势 RelativeState。
   // 4. 生成简单决策 GUIDE_TO_TARGET。
   // 5. 调用 mover 的 TurnToHeading()，让 ownship 朝 target 飞。
   // 6. 写入 controller_decision.csv。
   // ---------------------------------------------------------
   void Update(double aSimTime) override;

protected:
   //! Copy constructor for Clone()
//    拷贝构造函数，这就是刚才 Clone() 用到的，*this就是调用WsfSimpleControllerProcessor(const ...)
   WsfSimpleControllerProcessor(const WsfSimpleControllerProcessor& aSrc);
private:
   std::string mTargetName; // 目标平台名称
   std::string mDecisionFilename; // 决策输出文件名
   std::ofstream mDecisionFile;// CSV 文件流// 用于写入控制器输出结果

   std::string mObservationFilename;   // C++ 写给 Python 的状态文件
   std::string mActionFilename;     // Python 写入动作的文件
   double mMaxHeadingDeltaDeg;      // 限制最大转向修正角，防止动作太大
   double mActionWaitTimeoutSec;       // 等待 Python 动作的最长时间
   int mRlStepIndex;                   // RL 交互步数

   //  计算函数
    RelativeState CalculateRelativeState(
      double aSimTime,// aSimTime  当前仿真时间
      WsfPlatform* aOwnship,// aOwnship  当前平台，例如 blue_1
      WsfPlatform* aTarget);// aTarget   目标平台，例如 red_1
};

#endif

WsfSimpleControllerProcessor.cpp

#include "WsfSimpleControllerProcessor.hpp"
// AFSIM输入解析类（ProcessInput会用到）
#include "UtInput.hpp"// ProcessInput(UtInput& aInput) 里面要用它读取 mission script 中的参数。
// AFSIM平台类，可以获取飞机、舰艇、导弹等平台的信息
#include "WsfPlatform.hpp"// 这里用于获取当前平台 ownship 和目标平台 target 的位置、速度、名称等信息。
// AFSIM 仿真对象类。
#include "WsfSimulation.hpp"// 这里主要用于 GetSimulation()->GetPlatformByName(mTargetName)

// AFSIM mover 基类。
#include "WsfMover.hpp"// ownship->GetMover() 返回的就是 WsfMover*。
// 航路点 mover 类。
#include "WsfWaypointMover.hpp"// 然后调用 TurnToHeading() 控制飞机转向。
#include "WsfPath.hpp"// WsfPath 里面定义了转弯方向枚举。
#include "UtMath.hpp"// AFSIM 数学工具类

#include <thread>
#include <chrono>
#include <cstdio>


//==============================================================
// 构造函数
//==============================================================

// 创建Processor时调用。
// aScenario表示当前所属的仿真场景。
WsfSimpleControllerProcessor::WsfSimpleControllerProcessor(WsfScenario& aScenario)
// 调用父类WsfProcessor的构造函数
   : WsfProcessor(aScenario)
   , mTargetName("")
   , mDecisionFilename("controller_decision.csv")
   , mObservationFilename("observation.txt")
   , mActionFilename("action.txt")
   , mMaxHeadingDeltaDeg(10.0)
   , mActionWaitTimeoutSec(30.0)
   , mRlStepIndex(0)
{
}

WsfSimpleControllerProcessor::~WsfSimpleControllerProcessor()
{
    if (mDecisionFile.is_open())
    {
        mDecisionFile.close();
    }
}

//==============================================================
// 拷贝构造函数
//==============================================================

// Clone()会调用这里。
// AFSIM内部复制Processor时使用。
WsfSimpleControllerProcessor::WsfSimpleControllerProcessor(const WsfSimpleControllerProcessor& aSrc)
// 先复制父类的数据
   : WsfProcessor(aSrc)
   , mTargetName(aSrc.mTargetName)
   , mDecisionFilename(aSrc.mDecisionFilename)
   , mObservationFilename(aSrc.mObservationFilename)
   , mActionFilename(aSrc.mActionFilename)
   , mMaxHeadingDeltaDeg(aSrc.mMaxHeadingDeltaDeg)
   , mActionWaitTimeoutSec(aSrc.mActionWaitTimeoutSec)
   , mRlStepIndex(0)
{
}

//==============================================================
// Clone()
//==============================================================

// AFSIM要求所有Processor都必须能够Clone。
// 系统在复制平台时，就是调用这个函数。
WsfProcessor* WsfSimpleControllerProcessor::Clone() const
{
    // 调用拷贝构造函数创建一个新的对象
   return new WsfSimpleControllerProcessor(*this);
}

bool WsfSimpleControllerProcessor::Initialize(double aSimTime)
{
    // Initialize 打开决策文件
    bool ok = WsfProcessor::Initialize(aSimTime);
    std::cout<<"=====Initialize====="<<std::endl;

    mDecisionFile.open(mDecisionFilename.c_str(), std::ios::out);// std::ios::out 表示以写入模式打开。
    if (!mDecisionFile.is_open())
    {
        std::cout << "Open " << mDecisionFilename << " FAILED!" << std::endl;
        return false;
    }

    std::cout << "Open " << mDecisionFilename << " SUCCESS!" << std::endl;
    mDecisionFile
      << "time,ownship,target,range_m,relative_bearing_deg,"
      << "elevation_deg,course_deg,heading_delta_deg,desired_heading_deg,"
        << "decision,nx,ny,nz\n";

    return ok;
}

//==============================================================
// ProcessInput()
//==============================================================
// 读取脚本里的参数。
bool WsfSimpleControllerProcessor::ProcessInput(UtInput& aInput)
{
    std::string command(aInput.GetCommand());
    if (command == "target")
    {
      aInput.ReadValue(mTargetName);
      return true;
    }
    if (command == "decision_file")
    {
       aInput.ReadValue(mDecisionFilename);
       return true;
    }
    if (command == "observation_file")
    {
        aInput.ReadValue(mObservationFilename);
        return true;
    }

    if (command == "action_file")
    {
        aInput.ReadValue(mActionFilename);
        return true;
    }

    if (command == "max_heading_delta_deg")
    {
        aInput.ReadValue(mMaxHeadingDeltaDeg);
        return true;
    }

    if (command == "action_wait_timeout_sec")
    {
        aInput.ReadValue(mActionWaitTimeoutSec);
        return true;
    }
   
   
   return WsfProcessor::ProcessInput(aInput);
    
}

// Update()是Processor最重要的函数。
// 每次仿真更新都会执行一次。
void WsfSimpleControllerProcessor::Update(double aSimTime)
{

    // 先执行父类Update。
    WsfProcessor::Update(aSimTime);

    WsfPlatform* ownship = GetPlatform();// 获取所属平台
    if (ownship == nullptr)// 如果Processor没有挂到任何平台上，就退出。
    {
        return;
    }

    if (mTargetName.empty())// 如果没有设置目标平台名，也没法控制，直接退出。
    {
        return;
    }

   WsfPlatform* target = nullptr;
    if (!mTargetName.empty()) 
    {
        target = GetSimulation()->GetPlatformByName(mTargetName);// 根据目标平台名字查找目标平台对象。
        if (target == nullptr)// 如果没有找到目标平台，打印提示并退出。
        {
            std::cout << "Target platform [" << mTargetName << "] not found!" << std::endl;
            return;
        }
    }

    RelativeState state =// 计算 ownship 和 target 之间的相对态势。
      CalculateRelativeState(
         aSimTime,
         ownship,
         target);

    if (!state.valid)// 如果相对态势无效，直接退出。
    {
        return;
    }

    //==========================================================
    // ① 获取 ownship 的 mover
    //==========================================================

    // 拿到 mover
    WsfMover* mover = ownship->GetMover();// mover 是真正负责平台运动的对象。
    if (mover == nullptr)
    {
        std::cout << "[Control] No mover on platform: "
              << ownship->GetName() << std::endl;
        return;
    }

    //==========================================================
    // ② 把 mover 转成 WsfWaypointMover
    //==========================================================

    // 由于当前使用的是 WSF_AIR_MOVER，
    // 而 WSF_AIR_MOVER 对应的 WsfAirMover 继承自 WsfWaypointMover，
    // 所以这里可以尝试 dynamic_cast。
    WsfWaypointMover* waypointMover = dynamic_cast<WsfWaypointMover*>(mover);
    // 如果转换失败，说明当前平台用的 mover 不是 WsfWaypointMover 类型，
    // 也就不能调用 TurnToHeading()。
    if (waypointMover == nullptr)
    {
        std::cout << "[Control] Mover is not WsfWaypointMover: "
                << ownship->GetName() << std::endl;
        return;
    }

    //==========================================================
    // ③ 生成控制命令：朝目标方向转向
    //==========================================================

    // // state.bearingDeg 是目标相对于正北方向的绝对方位角，单位是度。
    // // TurnToHeading() 需要的是弧度，所以这里要把度转成弧度。
    // double desiredHeadingRad = state.bearingDeg * UtMath::cRAD_PER_DEG;
    // // 径向加速度约束。
    // // 传入 -1.0 表示不额外指定，使用 mover 默认的转弯约束。
    // double radialAccel = -1.0;

    // // 转弯方向。
    // // cTURN_DIR_SHORTEST 表示选择最短方向转弯。
    // // 例如向左转 20° 比向右转 340° 更短，就会向左转。
    // WsfPath::TurnDirection turnDirection = WsfPath::cTURN_DIR_SHORTEST;
    // // 调用 mover 的转向接口。
    // // 这一步是真正影响 blue_1 运动的地方。
    // // 它会让 ownship 朝 desiredHeadingRad 指定的航向飞。
    // waypointMover->TurnToHeading(
    //     aSimTime,
    //     desiredHeadingRad,
    //     radialAccel,
    //     turnDirection
    // );

    // ==========================================================
    // ② 删除旧 action，准备等待 Python 写新 action
    // ==========================================================

    std::remove(mActionFilename.c_str());
    // ==========================================================
    // ① 写 observation 给 Python
    // ==========================================================

    // 当前 RL 步号
    int stepId = mRlStepIndex;

    // 为了避免 Python 读到半截文件，先写临时文件，再 rename 成正式文件。
    std::string obsTmpFilename = mObservationFilename + ".tmp";

    std::ofstream obsFile(obsTmpFilename.c_str(), std::ios::out);

    if (obsFile.is_open())
    {
        // observation 格式：
        // step_id time range relative_bearing elevation closing_speed course
        obsFile
            << stepId << " "
            << aSimTime << " "
            << state.range << " "
            << state.relativeBearingDeg << " "
            << state.elevationDeg << " "
            << state.closingSpeed << " "
            << state.courseDeg
            << std::endl;

        obsFile.close();

        // 删除旧 observation 文件
        std::remove(mObservationFilename.c_str());

        // 把 tmp 文件改名为正式 observation 文件
        std::rename(obsTmpFilename.c_str(), mObservationFilename.c_str());
    }
    else
    {
        std::cout << "[RL] Failed to write observation file: "
                << mObservationFilename << std::endl;
    }

    // ==========================================================
    // 从 Python 动作文件读取 heading_delta_deg
    // ==========================================================
    // headingDeltaDeg 是 Python 输出的动作。
    // 单位：度。
    // 含义：
    // 负数 = 向左修正
    // 正数 = 向右修正
    // 0    = 保持当前航向
    double headingDeltaDeg = 0.0;
    bool gotAction = false;

    const int sleepMs = 10;
    int waitedMs = 0;
    int maxWaitMs = static_cast<int>(mActionWaitTimeoutSec * 1000.0);

    while (waitedMs <= maxWaitMs)
    {
        std::ifstream actionFile(mActionFilename.c_str());

        if (actionFile.is_open())
        {
            int actionStepId = -1;
            double actionValue = 0.0;

            actionFile >> actionStepId >> actionValue;
            actionFile.close();

            // 只有 action 文件里的 step_id 和当前 stepId 一致，才接受这个动作。
            if (actionStepId == stepId)
            {
                headingDeltaDeg = actionValue;
                gotAction = true;
                break;
            }
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(sleepMs));
        waitedMs += sleepMs;
    }
    if (!gotAction)
    {
        std::cout << "[RL] Timeout waiting action for step "
                << stepId
                << ", use headingDeltaDeg = 0"
                << std::endl;

        headingDeltaDeg = 0.0;
    }

    // ==========================================================
    // 限制动作范围，防止 Python 输出过大的动作
    // ==========================================================

    if (headingDeltaDeg > mMaxHeadingDeltaDeg)
    {
        headingDeltaDeg = mMaxHeadingDeltaDeg;
    }

    if (headingDeltaDeg < -mMaxHeadingDeltaDeg)
    {
        headingDeltaDeg = -mMaxHeadingDeltaDeg;
    }
    // ==========================================================
    // 把相对转向量转换成绝对目标航向
    // ==========================================================
    // state.courseDeg 是当前自机航迹角，单位度。
    // headingDeltaDeg 是 Python 给的相对修正量。
    // desiredHeadingDeg 就是最终要飞向的绝对航向角。
    double desiredHeadingDeg = state.courseDeg + headingDeltaDeg;
    // 把角度归一化到 [0, 360)
    while (desiredHeadingDeg < 0.0)
    {
        desiredHeadingDeg += 360.0;
    }
    while (desiredHeadingDeg >= 360.0)
    {
        desiredHeadingDeg -= 360.0;
    }
    // TurnToHeading() 需要弧度，所以这里从度转弧度。
    double desiredHeadingRad = desiredHeadingDeg * UtMath::cRAD_PER_DEG;
    // 使用 mover 默认转弯约束。
    double radialAccel = -1.0;
    // 选择最短方向转弯。
    WsfPath::TurnDirection turnDirection = WsfPath::cTURN_DIR_SHORTEST;
    // ==========================================================
    // 真正控制飞机转向
    // ==========================================================
    waypointMover->TurnToHeading(
        aSimTime,
        desiredHeadingRad,
        radialAccel,
        turnDirection
    );

    //==========================================================
    // ④ 简化版决策名称
    //==========================================================
    std::string decision = "GUIDE_TO_TARGET";// GUIDE_TO_TARGET = 朝目标方向飞。

    //==========================================================
    // ⑤ 计算 nx, ny, nz
    //==========================================================

    // nx, ny, nz 这里暂时表示：
    // WCS 坐标系下，从 ownship 指向 target 的单位视线方向。
    //
    // 注意：
    // 这里的 nx, ny, nz 不是严格意义上的飞机机体系过载控制量，
    // 只是当前控制器输出的目标方向向量。
    double nx = 0.0;
    double ny = 0.0;
    double nz = 0.0;
    // 计算 LOS 向量的模长。
    // 理论上 state.losX/Y/Z 已经是单位向量，
    // 这里再归一化一次，是为了安全。
    double norm = sqrt(
        state.losX * state.losX +
        state.losY * state.losY +
        state.losZ * state.losZ
    );

    if (norm > 1e-6)// 如果模长不是 0，就归一化。
    {
        nx = state.losX / norm;
        ny = state.losY / norm;
        nz = state.losZ / norm;
    }
    //==========================================================
    // ⑥ 终端打印调试信息
    //==========================================================
    std::cout << "[Controller] t="
             << aSimTime
             << " ownship="
             << ownship->GetName()
             << " target="
             << target->GetName()
             << " range="
             << state.range
             << " rb="
             << state.relativeBearingDeg
            << " course="
            << state.courseDeg
            << " action_delta="
            << headingDeltaDeg
            << " desired_heading="
            << desiredHeadingDeg
            << " decision="
            << decision
            << std::endl;
    std::cout << "[Control] nx="
          << nx
          << " ny="
          << ny
          << " nz="
          << nz
          << std::endl;
    //==========================================================
    // ⑦ 写入 CSV 文件
    //==========================================================

    if (mDecisionFile.is_open())
    {
        mDecisionFile
            << aSimTime << ","
            << ownship->GetName() << ","
            << target->GetName() << ","
            << state.range << ","
            << state.relativeBearingDeg << ","
            << state.elevationDeg << ","
            << state.courseDeg << ","
            << headingDeltaDeg << ","
            << desiredHeadingDeg << ","
            << decision << ","
            << nx << ","
            << ny << ","
            << nz
            << '\n';
        // flush() 表示立即把缓冲区内容写入文件。
        // 好处是仿真中途停止时，已经写出的数据不容易丢。
        // 缺点是频繁 flush 会稍微降低性能。
        mDecisionFile.flush();
    }  
    mRlStepIndex++;

}

RelativeState
WsfSimpleControllerProcessor::CalculateRelativeState(
   double aSimTime,
   WsfPlatform* aOwnship,
   WsfPlatform* aTarget)
{
    RelativeState state;
    if ((aOwnship == nullptr) ||
       (aTarget == nullptr))
   {
      return state;
   }
   aTarget->Update(aSimTime);
   double ownLoc[3];
   double ownVel[3];

   double targetLoc[3];
   double targetVel[3];

   aOwnship->GetLocationWCS(ownLoc);
   aOwnship->GetVelocityWCS(ownVel);

   aTarget->GetLocationWCS(targetLoc);
   aTarget->GetVelocityWCS(targetVel);

   double ownLat;
   double ownLon;
   double ownAlt;

   double targetLat;
   double targetLon;
   double targetAlt;

   aOwnship->GetLocationLLA(
      ownLat,
      ownLon,
      ownAlt);

   aTarget->GetLocationLLA(
      targetLat,
      targetLon,
      targetAlt);

    //   计算速度
    state.ownSpeed = std::sqrt(
        ownVel[0] * ownVel[0] +
        ownVel[1] * ownVel[1] +
        ownVel[2] * ownVel[2]);

    state.targetSpeed = std::sqrt(
        targetVel[0] * targetVel[0] +
        targetVel[1] * targetVel[1] +
        targetVel[2] * targetVel[2]);

        // 计算相对位置
    double dx =
        targetLoc[0] - ownLoc[0];

    double dy =
        targetLoc[1] - ownLoc[1];

    double dz =
        targetLoc[2] - ownLoc[2];

        // 距离
    state.range = std::sqrt(
        dx * dx +
        dy * dy +
        dz * dz);
        // 高度差
    state.altitudeDifference = targetAlt - ownAlt;

    // LOS
    if (state.range > 1.0e-6)
    {
        state.losX = dx / state.range;
        state.losY = dy / state.range;
        state.losZ = dz / state.range;
        // 接近速度
        double relVx = targetVel[0] - ownVel[0];
        double relVy = targetVel[1] - ownVel[1];
        double relVz = targetVel[2] - ownVel[2];
        state.closingSpeed = -(relVx * state.losX + relVy * state.losY + relVz * state.losZ);
        
    }
    
    // ECEF → ENU
    constexpr double PI =    3.14159265358979323846;

    double latRad =    ownLat * PI / 180.0;

    double lonRad =    ownLon * PI / 180.0;

    double sinLat = std::sin(latRad);
    double cosLat = std::cos(latRad);

    double sinLon = std::sin(lonRad);
    double cosLon = std::cos(lonRad);

    state.east =
        -sinLon * dx
        +cosLon * dy;

    state.north =
        -sinLat * cosLon * dx
        -sinLat * sinLon * dy
        +cosLat * dz;

    state.up =
            cosLat * cosLon * dx
        +cosLat * sinLon * dy
        +sinLat * dz;

    // Bearing
    state.bearingDeg = std::atan2(state.east, state.north) * 180.0 / PI;
    if (state.bearingDeg < 0.0)
    {
        state.bearingDeg += 360.0;
    }
    // 水平距离和 elevation
    state.horizontalRange = std::sqrt(state.east * state.east + state.north * state.north);
    state.elevationDeg = std::atan2(state.up, state.horizontalRange) * 180.0 / PI;
    // 自机速度 ECEF → ENU
    double velEast =
        -sinLon * ownVel[0]
        +cosLon * ownVel[1];

    double velNorth =
        -sinLat * cosLon * ownVel[0]
        -sinLat * sinLon * ownVel[1]
        +cosLat * ownVel[2];
    // Course
    double courseRad =
        std::atan2(
            velEast,
            velNorth);

    state.courseDeg =
        courseRad * 180.0 / PI;

    if (state.courseDeg < 0.0)
    {
        state.courseDeg += 360.0;
    }
    // Relative bearing
    state.relativeBearingDeg = state.bearingDeg - state.courseDeg;
    while (state.relativeBearingDeg > 180.0)
    {
        state.relativeBearingDeg -= 360.0;
    }
    while (state.relativeBearingDeg <= -180.0)
    {
        state.relativeBearingDeg += 360.0;
    }

   state.valid = true;

   return state;

}

two_platform_logger.txt

end_time 60 sec

platform_type BLUE_AIRCRAFT WSF_PLATFORM
   mover WSF_AIR_MOVER
      update_interval 1 sec
      roll_rate_limit 1 rad/sec
      default_linear_acceleration 1.0 g
      default_radial_acceleration 6.5 g
      default_climb_rate 400 fps
      maximum_climb_rate 400 fps
      maximum_speed 600.0 knots
      minimum_speed 150.0 knots
      maximum_altitude 50000 ft
      minimum_altitude 50 ft
      maximum_linear_acceleration 9 g
      at_end_of_path extrapolate
      turn_rate_limit 4.0 deg/sec
   end_mover

   processor state_logger MY_STATE_LOGGER_PROCESSOR
      update_interval 10 sec
      filename /home/ubuntu/afsim_ws/project1/11-two-platform-logger/blue.csv
      target red_1
   end_processor
   
   processor simple_controller MY_SIMPLE_CONTROLLER_PROCESSOR
      update_interval 10 sec
      target red_1
      decision_file /home/ubuntu/afsim_ws/project1/11-two-platform-logger-2/controller_decision.csv
      observation_file /home/ubuntu/afsim_ws/project1/11-two-platform-logger-2/observation.txt
      action_file /home/ubuntu/afsim_ws/project1/11-two-platform-logger-2/action.txt
      max_heading_delta_deg 10
      action_wait_timeout_sec 30
   end_processor
   
end_platform_type

platform_type RED_AIRCRAFT WSF_PLATFORM
   mover WSF_AIR_MOVER
      update_interval 1 sec
      roll_rate_limit 1 rad/sec
      default_linear_acceleration 1.0 g
      default_radial_acceleration 6.5 g
      default_climb_rate 400 fps
      maximum_climb_rate 400 fps
      maximum_speed 600.0 knots
      minimum_speed 150.0 knots
      maximum_altitude 50000 ft
      minimum_altitude 50 ft
      maximum_linear_acceleration 9 g
      at_end_of_path extrapolate
      turn_rate_limit 4.0 deg/sec
   end_mover
   
end_platform_type

platform blue_1 BLUE_AIRCRAFT
   side blue
   position 38:49:12.88n 93:08:16.09w altitude 35000.00 ft msl
   heading 90 deg
   
   route
      position 38:49:12.88n 93:08:16.09w altitude 35000.00 ft msl speed 500 kts
      position 38:49:12.11n 89:52:40.60w altitude 35000.00 ft msl speed 500 kts
   end_route

   execute at_time 0.1 sec absolute
      writeln("time,name,lat,lon,alt,x,y,z,vx,vy,vz,speed");
   end_execute

end_platform

platform red_1 RED_AIRCRAFT
   side red
   position 38:49:12.88n 90:08:16.09w altitude 35000.00 ft msl
   heading 270 deg

   route
      position 38:49:12.88n 90:08:16.09w altitude 35000.00 ft msl speed 450 kts
      position 38:49:12.11n 93:52:40.60w altitude 35000.00 ft msl speed 450 kts
   end_route
end_platform

运行

先编译，编译完成后，先运行python

python3 /home/ubuntu/afsim_ws/project1/11-two-platform-logger-2/rl_policy.py

再运行mission

./buil/mission -sm /home/ubuntu/afsim_ws/project1/11-two-platform-logger-2/two_platform_logger.txt