OwheelNT/main.cpp at master · ZzzzzzS/OwheelNT · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
/*****************************************************************/ /**
                                                                     * \file   main.cpp
                                                                     * \brief
                                                                     *
                                                                     * \author ZhouZishun
                                                                     * \date   March 2025
                                                                     *********************************************************************/
#ifdef BUILD_SIMULATION
#include "bitbot_mujoco/kernel/mujoco_kernel.hpp"
#else
#include "Bitbot_Encos/include/kernel/Encos_kernel.hpp"
#include "bitbot_kernel/utils/cpu_affinity.h"
#include "bitbot_kernel/utils/priority.h"
#include <sys/mman.h>
#endif // BUILD_SIMULATION
#include "user_func.h"
#include "user_command.h"

int main(int argc, char const* argv[])
{
    // NOTE: 注意将配置文件路径修改为自己的路径

#ifdef BUILD_SIMULATION
    std::string cfg_path = PROJECT_ROOT_DIR + std::string("/settings/bitbot_mujoco.xml");
    KernelType kernel(cfg_path);
#else
    // if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1)
    // {
    //     printf("mlockall failed: \n");
    //     if (errno == ENOMEM)
    //     {
    //         printf(
    //             "\nIt is likely your user does not have enough memory limits, you "
    //             "can change the limits by adding the "
    //             "following line to /etc/security/limits.conf:\n\n");
    //     }
    //     return -1;
    // }
    // if (!setProcessHighPriority(99))
    // {
    //     printf("Failed to set process scheduling policy\n");
    //     return -1;
    // }

    std::string cfg_path = PROJECT_ROOT_DIR + std::string("/settings/bitbot_owheel.xml");
    KernelType kernel(cfg_path);
#endif // BUILD_SIMULATION

    kernel.RegisterConfigFunc(ConfigFunc);
    kernel.RegisterFinishFunc(FinishFunc);

    // 注册状态切换事件
    kernel.RegisterEvent("system_test", static_cast<bitbot::EventId>(Events::SystemTest), &EventSystemTest);
    kernel.RegisterEvent("init_pose", static_cast<bitbot::EventId>(Events::InitPose), &EventInitPose);
    kernel.RegisterEvent("policy_run", static_cast<bitbot::EventId>(Events::PolicyRun), &EventPolicyRun);

    // 注册用户命令事件(键盘/手柄按钮)
    kernel.RegisterEvent("velo_x_increase", static_cast<bitbot::EventId>(UserCmdEvents::VeloxIncrease), &EventCmdNIncrease<0>);
    kernel.RegisterEvent("velo_y_increase", static_cast<bitbot::EventId>(UserCmdEvents::VeloyIncrease), &EventCmdNIncrease<1>);
    kernel.RegisterEvent("velo_yaw_increase", static_cast<bitbot::EventId>(UserCmdEvents::VeloYawIncrease), &EventCmdNIncrease<2>);
    kernel.RegisterEvent("pos_z_increase", static_cast<bitbot::EventId>(UserCmdEvents::PosZIncrease), &EventCmdNIncrease<3>);
    kernel.RegisterEvent("pos_pitch_increase", static_cast<bitbot::EventId>(UserCmdEvents::PosPitchIncrease), &EventCmdNIncrease<4>);
    kernel.RegisterEvent("pos_roll_increase", static_cast<bitbot::EventId>(UserCmdEvents::PosRollIncrease), &EventCmdNIncrease<5>);

    kernel.RegisterEvent("velo_x_decrease", static_cast<bitbot::EventId>(UserCmdEvents::VeloxDecrease), &EventCmdNDecrease<0>);
    kernel.RegisterEvent("velo_y_decrease", static_cast<bitbot::EventId>(UserCmdEvents::VeloyDecrease), &EventCmdNDecrease<1>);
    kernel.RegisterEvent("velo_yaw_decrease", static_cast<bitbot::EventId>(UserCmdEvents::VeloYawDecrease), &EventCmdNDecrease<2>);
    kernel.RegisterEvent("pos_z_decrease", static_cast<bitbot::EventId>(UserCmdEvents::PosZDecrease), &EventCmdNDecrease<3>);
    kernel.RegisterEvent("pos_pitch_decrease", static_cast<bitbot::EventId>(UserCmdEvents::PosPitchDecrease), &EventCmdNDecrease<4>);
    kernel.RegisterEvent("pos_roll_decrease", static_cast<bitbot::EventId>(UserCmdEvents::PosRollDecrease), &EventCmdNDecrease<5>);

    // 注册用连续命令事件(手柄摇杆)
    kernel.RegisterEvent("gamepad_x_move", static_cast<bitbot::EventId>(UserCmdEvents::GamepadXMove), &EventCmdNMove<0>);
    kernel.RegisterEvent("gamepad_y_move", static_cast<bitbot::EventId>(UserCmdEvents::GamepadYMove), &EventCmdNMove<1>);
    kernel.RegisterEvent("gamepad_yaw_move", static_cast<bitbot::EventId>(UserCmdEvents::GamepadYawMove), &EventCmdNMove<2>);
    kernel.RegisterEvent("gamepad_z_move", static_cast<bitbot::EventId>(UserCmdEvents::GamepadZMove), &EventCmdNMove<3>);
    kernel.RegisterEvent("gamepad_pitch_move", static_cast<bitbot::EventId>(UserCmdEvents::GamepadPitchMove), &EventCmdNMove<4>);
    kernel.RegisterEvent("gamepad_roll_move", static_cast<bitbot::EventId>(UserCmdEvents::GamepadRollMove), &EventCmdNMove<5>);

    // 注册 State
    kernel.RegisterState("waiting", static_cast<bitbot::StateId>(States::Waiting),
        &StateWaiting,
        { static_cast<bitbot::EventId>(Events::SystemTest), (Events::InitPose) });

    kernel.RegisterState("SystemTest", static_cast<bitbot::StateId>(States::PF2SystemTest), &StateSystemTest, {});

    kernel.RegisterState("init_pose",
        static_cast<bitbot::StateId>(States::PF2InitPose),
        &StateJointInitPose,
        { (Events::PolicyRun) });

    std::vector<bitbot::EventId> PolicyRuntEvents;
    for (size_t i = UserCmdEvents::VeloxIncrease; i < UserCmdEvents::USER_CMD_EVENT_END; i++)
    {
        PolicyRuntEvents.push_back(static_cast<bitbot::EventId>(i));
    }
    kernel.RegisterState("policy_run", static_cast<bitbot::StateId>(States::PF2PolicyRun), &StatePolicyRun, PolicyRuntEvents);

    kernel.SetFirstState(static_cast<bitbot::StateId>(States::Waiting));
    kernel.Run(); // Run the kernel
    return 0;
}