diff --git a/simulate/src/main.cc b/simulate/src/main.cc index 3368d916..75e2b6af 100755 --- a/simulate/src/main.cc +++ b/simulate/src/main.cc @@ -102,6 +102,11 @@ namespace // control noise variables mjtNum *ctrlnoise = nullptr; + // Pause/resume handshake for safe model reload: PhysicsLoop signals the + // bridge to stop before freeing m/d, bridge confirms before Physics proceeds. + std::atomic g_bridge_pause_request{false}; + std::atomic g_bridge_paused{false}; + using Seconds = std::chrono::duration; @@ -350,6 +355,10 @@ namespace { sim.Load(mnew, dnew, sim.dropfilename); + g_bridge_pause_request.store(true); + while (!g_bridge_paused.load()) + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + mj_deleteData(d); mj_deleteModel(m); @@ -357,6 +366,8 @@ namespace d = dnew; mj_forward(m, d); + g_bridge_pause_request.store(false); + // allocate ctrlnoise free(ctrlnoise); ctrlnoise = (mjtNum *)malloc(sizeof(mjtNum) * m->nu); @@ -587,23 +598,32 @@ void *UnitreeSdk2BridgeThread(void *arg) unitree::robot::ChannelFactory::Instance()->Init(param::config.domain_id, param::config.interface); - int body_id = mj_name2id(m, mjOBJ_BODY, "torso_link"); - if (body_id < 0) { - body_id = mj_name2id(m, mjOBJ_BODY, "base_link"); - } - param::config.band_attached_link = 6 * body_id; - - std::unique_ptr interface = nullptr; - if (m->nu > NUM_MOTOR_IDL_GO) { - interface = std::make_unique(m, d); - } else { - interface = std::make_unique(m, d); - } - interface->start(); - while (true) { - sleep(1); + int body_id = mj_name2id(m, mjOBJ_BODY, "torso_link"); + if (body_id < 0) { + body_id = mj_name2id(m, mjOBJ_BODY, "base_link"); + } + param::config.band_attached_link = 6 * body_id; + + std::unique_ptr interface = nullptr; + if (m->nu > NUM_MOTOR_IDL_GO) { + interface = std::make_unique(m, d); + } else { + interface = std::make_unique(m, d); + } + interface->start(); + + while (!g_bridge_pause_request.load()) + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + + interface->pre_destroy(); + interface.reset(); + g_bridge_paused.store(true); + + while (g_bridge_pause_request.load()) + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + g_bridge_paused.store(false); } } //------------------------------------------ main -------------------------------------------------- diff --git a/simulate/src/unitree_sdk2_bridge.h b/simulate/src/unitree_sdk2_bridge.h index 7c13289a..e1e10615 100644 --- a/simulate/src/unitree_sdk2_bridge.h +++ b/simulate/src/unitree_sdk2_bridge.h @@ -10,6 +10,9 @@ #include #include +#include +#include +#include #include "param.h" #include "physics_joystick.h" @@ -40,6 +43,7 @@ class UnitreeSDK2BridgeBase } virtual void start() {} + virtual void pre_destroy() {} void printSceneInformation() { @@ -165,18 +169,35 @@ using WirelessController_t = unitree::robot::go2::publisher::WirelessController; wireless_controller->joystick = joystick; } + ~RobotBridge() + { + pre_destroy(); + } + void start() { - thread_ = std::make_shared( - "unitree_bridge", UT_CPU_ID_NONE, 1000, [this]() { this->run(); }); + stop_flag_.store(false); + run_thread_ = std::thread([this]() { + while (!stop_flag_.load()) { + this->run(); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + }); + } + + void pre_destroy() override + { + stop_flag_.store(true); + if (run_thread_.joinable()) + run_thread_.join(); } virtual void run() { if(!mj_data_) return; if(lowstate->joystick) { lowstate->joystick->update(); } - // lowcmd - { + // lowcmd — only apply when a real command has been received + if (!lowcmd->isTimeout()) { std::lock_guard lock(lowcmd->mutex_); for(int i(0); imsg_.motor_cmd()[i]; @@ -251,7 +272,8 @@ using WirelessController_t = unitree::robot::go2::publisher::WirelessController; std::unique_ptr lowstate; private: - unitree::common::RecurrentThreadPtr thread_; + std::atomic stop_flag_{false}; + std::thread run_thread_; }; using Go2Bridge = RobotBridge;