user_func.cpp

#include "user_func.h"

#include <chrono>
#define _USE_MATH_DEFINES
#include <cmath>
#include <ctime>
#include <memory>
#include <thread>
#include <iostream> // std::cout
#include <nlohmann/json.hpp>
#include <fstream>
#include "types.hpp"

template <typename Container, typename Func>
void Apply(Container& container, Func func)
{
    for (size_t i = 0; i < container.size(); ++i)
    {
        func(&container[i], i);
    }
}

void ConfigFunc(const KernelBus& bus, UserData& d)
{
    // 读取json配置文件,并初始化各个worker
    nlohmann::json cfg_root;
    nlohmann::json cfg_workers;
    {
        // NOTE: 注意将配置文件路径修改为自己的路径
        std::string path = PROJECT_ROOT_DIR + std::string("/settings/CtrlConfig.json");
        std::ifstream cfg_file(path);
        cfg_root = nlohmann::json::parse(cfg_file, nullptr, true, true);
        cfg_workers = cfg_root["Workers"];
    }

    // 对齐硬件颗粒度
    d.ImuPtr = bus.GetDevice<DeviceImu>(IMU_ID_MAP).value();
    Apply(d.JointsPtr, [&bus](DeviceJoint** joint, size_t i)
        { *joint = bus.GetDevice<DeviceJoint>(JOINT_ID_MAP[i]).value(); });

    // 创建调度器
    d.TaskScheduler = SchedulerType::Create(cfg_root["Scheduler"]);

    // 初始化各个worker
    d.ImuWorker = d.TaskScheduler->template CreateWorker<ImuWorkerType>(d.ImuPtr, cfg_workers["ImuProcess"]);
    d.MotorWorker = d.TaskScheduler->template CreateWorker<MotorWorkerType>(cfg_workers["MotorControl"], d.JointsPtr);
    d.Logger = d.TaskScheduler->template CreateWorker<LoggerWorkerType>(cfg_workers["AsyncLogger"]);
    d.CmdWorker = d.TaskScheduler->template CreateWorker<CmdWorkerType>(cfg_workers["Commander"]);
    d.DDSCommWorker = d.TaskScheduler->template CreateWorker<DDSCommWorkerType>(d.CmdWorker, cfg_workers["DDSComm"]);
    d.SmartDriverWorker = d.TaskScheduler->template CreateWorker<SmartDriverWorkerType>(cfg_workers["SmartDriver"]);
    d.ActionManagementWorker = d.TaskScheduler->template CreateWorker<ActionManagementWorkerType>(cfg_workers["ActionManager"]);
    d.ActionManagementWorker->template SetActionRemapFunction<NetActionPair>([](SchedulerType::Ptr scheduler, const MotorVec val)
        {
            MotorVec target_pos = val;
            target_pos[3] = 0;
            target_pos[7] = 0;

            MotorVec target_vel = MotorVec::zeros();
            target_vel[3] = val[3];
            target_vel[7] = val[7];
            scheduler->template SetData<"TargetMotorPosition">(target_pos);
            scheduler->template SetData<"TargetMotorVelocity">(target_vel); });

    // 创建主任务列表，并添加worker
    d.TaskScheduler->CreateTaskList("MainTask", 1, true);
    d.TaskScheduler->AddWorkers("MainTask",
        { d.ImuWorker,
         d.MotorWorker });

    // 创建推理任务列表，并添加worker，设置推理任务频率
    d.NetInferWorker = d.TaskScheduler->template CreateWorker<PlainInferWorkerType>(cfg_workers["NN"], cfg_workers["MotorControl"]);
    d.TaskScheduler->CreateTaskList("InferTask", cfg_root["Scheduler"]["InferTask"]["PolicyFrequency"]);
    d.TaskScheduler->AddWorkers("InferTask",
        {
            d.CmdWorker,
            d.DDSCommWorker,
            d.SmartDriverWorker,
            d.NetInferWorker,
         d.ActionManagementWorker,
         d.Logger });

    // 创建复位任务列表，并添加worker，设置复位任务频率为主任务频率的1/10
    d.MotorResetWorker = d.TaskScheduler->template CreateWorker<MotorResetWorkerType>(cfg_workers["MotorControl"], cfg_workers["ResetPosition"]);
    d.TaskScheduler->CreateTaskList("ResetTask", 10);
    d.TaskScheduler->AddWorker("ResetTask", d.MotorResetWorker);

#ifdef BUILD_ROS2_EXT
    //给ros2 创建单独的tasklist
    d.RobotStatusPubWorker = d.TaskScheduler->template CreateWorker<RobotStatusPubWorkerType>(cfg_workers["RobotStatusPub"]);
    d.DigitalTwinCmdWorker = d.TaskScheduler->template CreateWorker<Ros2CmdWorkerType>(cfg_workers["TGHPlannerNode"]);
    d.DigitalTwinCmdWorker->RegisterVelCmdCallback(
        [&d](SchedulerType::Ptr scheduler, const z::math::Vector<RealNumber, 3>& linear, const z::math::Vector<RealNumber, 3>& angular)
        {
            d.CmdWorker->SetCmd(0, linear[0], 1);
            d.CmdWorker->SetCmd(2, angular[2], 1);
        });



    d.TaskScheduler->CreateTaskList("Ros2PubTask", 10);
    d.TaskScheduler->AddWorker("Ros2PubTask", d.DigitalTwinCmdWorker);
    d.TaskScheduler->AddWorker("Ros2PubTask", d.RobotStatusPubWorker);
    d.TaskScheduler->EnableTaskList("Ros2PubTask"); //启动ros2 数据发送
#endif

    // 开始调度器
    d.TaskScheduler->Start();
}

void FinishFunc(UserData& d)
{
}

void StateWaiting(const bitbot::KernelInterface& kernel,
    bitbot::ExtraData& extra_data, UserData& d)
{
    // 在具体状态中只需要调用调度器的SpinOnce函数即可，调度器会根据设置自动调度任务
    d.MotorWorker->SetCurrentPositionAsTargetPosition(); // 设置当前位置为目标位置,使其保持当前位置
    d.TaskScheduler->SpinOnce();
}

void StateSystemTest(const bitbot::KernelInterface& kernel,
    bitbot::ExtraData& extra_data, UserData& user_data)
{
#ifndef BUILD_SIMULATION
    user_data.JointsPtr[1]->SetMode(bitbot::EncosJointMode::Position);
    user_data.JointsPtr[5]->SetMode(bitbot::EncosJointMode::Position);
    user_data.JointsPtr[1]->SetTargetPosition(0);
    user_data.JointsPtr[5]->SetTargetPosition(0);

    user_data.JointsPtr[0]->SetMode(bitbot::EncosJointMode::Position);
    user_data.JointsPtr[4]->SetMode(bitbot::EncosJointMode::Position);
    user_data.JointsPtr[0]->SetTargetPosition(0);
    user_data.JointsPtr[4]->SetTargetPosition(0);

    user_data.JointsPtr[2]->SetMode(bitbot::EncosJointMode::Position);
    user_data.JointsPtr[6]->SetMode(bitbot::EncosJointMode::Position);
    user_data.JointsPtr[2]->SetTargetPosition(0);
    user_data.JointsPtr[6]->SetTargetPosition(0);

    user_data.JointsPtr[3]->SetMode(bitbot::EncosJointMode::Velocity);
    user_data.JointsPtr[7]->SetMode(bitbot::EncosJointMode::Velocity);
    user_data.JointsPtr[3]->SetTargetVelocity(0);
    user_data.JointsPtr[7]->SetTargetVelocity(0);

#endif // BUILD_SIMULATION
}



void StatePolicyRun(const bitbot::KernelInterface& kernel,
    bitbot::ExtraData& extra_data, UserData& d)
{
    d.TaskScheduler->SpinOnce();
};

void StateJointInitPose(const bitbot::KernelInterface& kernel,
    bitbot::ExtraData& extra_data, UserData& d)
{
    d.TaskScheduler->SpinOnce();
}