（7）ROS2-MUJOCO联合仿真环境迁移优化

前言：

之前在这篇帖子（5）ROS：MUJOCO仿真环境迁移教程_mujoco文件用gezabo打开的方法和注意事项-CSDN博客中介绍过如何将ros2的仿真环境由gazebo迁移到mujoco，但是当时使用的ros官方源码的迁移程序中，对mujoco的渲染是使用的自定义的mujoco_rendering.cpp，相当于实现了极简的纯仿真环境，但是没有mujoco原版那样默认丰富的侧边可交互UI，本篇介绍如何使用mujoco官方定义的simulate仿真UI替换原来的mujoco_rendering.cpp。

正文：

1. 找到mujoco的源码路径，找到simulate文件夹

2. 把simulate文件夹完整copy到/your_workspace/src/mujoco_ros2_control/mujoco_ros2_control下，目录结构如图

3. 下载lodepng依赖，是simulate包编译需要的一个依赖程序，主要用于处理png，mujoco官方包里没有这个https://github.com/lvandeve/lodepng/  

git下来之后解压找到lodepng.cpp和lodepng.h，单独把这两个复制到simulate文件夹下就可以，simulate完整版长这样

4.  配置主循环，打开main.cc，因为现在是希望用这个main.cc代替原来的主循环mujoco_ros2_control_node.cpp，也就是它除了要实现官方原定义的UI渲染，还要实现ros2 controller manager的加载和仿真步进，因此做三处修改：

第一步，头部导入mujoco_ros2_control.hpp，并创建将来用于加载控制器的静态指针

#if defined(mjUSEUSD)
#include <mujoco/experimental/usd/usd.h>
#endif
#include <mujoco/mujoco.h>
#include "glfw_adapter.h"
#include "simulate.h"
#include "array_safety.h"

// 增加如下两行，用于 PhysicsLoop 访问 mujoco_control
#include "mujoco_ros2_control/mujoco_ros2_control.hpp"
static mujoco_ros2_control::MujocoRos2Control* g_mujoco_control = nullptr;  

第二步，修改void PhysicsLoop函数，确保每次步进我们的控制器也被更新，完整的函数程序如下，主要就是加了两行g_mujoco_control->update和executor.spin_some(); 保证ros2、mujoco仿真环境、mujoco仿真UI的时钟保持一致

void PhysicsLoop(mj::Simulate& sim, rclcpp::executors::MultiThreadedExecutor& executor) {
  // cpu-sim syncronization point
  std::chrono::time_point<mj::Simulate::Clock> syncCPU;
  mjtNum syncSim = 0;

  // run until asked to exit
  while (!sim.exitrequest.load()) {
    if (sim.droploadrequest.load()) {
      sim.LoadMessage(sim.dropfilename);
      mjModel* mnew = LoadModel(sim.dropfilename, sim);
      sim.droploadrequest.store(false);

      mjData* dnew = nullptr;
      if (mnew) dnew = mj_makeData(mnew);
      if (dnew) {
        sim.Load(mnew, dnew, sim.dropfilename);

        // lock the sim mutex
        const std::unique_lock<std::recursive_mutex> lock(sim.mtx);

        mj_deleteData(d);
        mj_deleteModel(m);

        m = mnew;
        d = dnew;
        mj_forward(m, d);

      } else {
        sim.LoadMessageClear();
      }
    }

    if (sim.uiloadrequest.load()) {
      sim.uiloadrequest.fetch_sub(1);
      sim.LoadMessage(sim.filename);
      mjModel* mnew = LoadModel(sim.filename, sim);
      mjData* dnew = nullptr;
      if (mnew) dnew = mj_makeData(mnew);
      if (dnew) {
        sim.Load(mnew, dnew, sim.filename);

        // lock the sim mutex
        const std::unique_lock<std::recursive_mutex> lock(sim.mtx);

        mj_deleteData(d);
        mj_deleteModel(m);

        m = mnew;
        d = dnew;
        mj_forward(m, d);

      } else {
        sim.LoadMessageClear();
      }
    }

    // sleep for 1 ms or yield, to let main thread run
    //  yield results in busy wait - which has better timing but kills battery life
    if (sim.run && sim.busywait) {
      std::this_thread::yield();
    } else {
      std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }

    // lock the sim mutex for the entire physics/control/ros2 update
    const std::unique_lock<std::recursive_mutex> lock(sim.mtx);

    // run only if model is present
    if (m) {
      // running
      if (sim.run) {
        bool stepped = false;

        // record cpu time at start of iteration
        const auto startCPU = mj::Simulate::Clock::now();

        // elapsed CPU and simulation time since last sync
        const auto elapsedCPU = startCPU - syncCPU;
        double elapsedSim = d->time - syncSim;

        // requested slow-down factor
        double slowdown = 100 / sim.percentRealTime[sim.real_time_index];

        // misalignment condition: distance from target sim time is bigger than syncMisalign
        bool misaligned =
            std::abs(Seconds(elapsedCPU).count()/slowdown - elapsedSim) > syncMisalign;

        // out-of-sync (for any reason): reset sync times, step
        if (elapsedSim < 0 || elapsedCPU.count() < 0 || syncCPU.time_since_epoch().count() == 0 ||
            misaligned || sim.speed_changed) {
          // re-sync
          syncCPU = startCPU;
          syncSim = d->time;
          sim.speed_changed = false;

          // run single step, let next iteration deal with timing
          mj_step(m, d);
          // --- 保证控制器 update 被周期性调用 ---
          if (g_mujoco_control) {
            g_mujoco_control->update();
          }
          // ROS2 spin_some 也在锁内
          if (rclcpp::ok()) {
            executor.spin_some();
          }
          const char* message = Diverged(m->opt.disableflags, d);
          if (message) {
            sim.run = 0;
            mju::strcpy_arr(sim.load_error, message);
          } else {
            stepped = true;
          }
        }

        // in-sync: step until ahead of cpu
        else {
          bool measured = false;
          mjtNum prevSim = d->time;

          double refreshTime = simRefreshFraction/sim.refresh_rate;

          // step while sim lags behind cpu and within refreshTime
          while (Seconds((d->time - syncSim)*slowdown) < mj::Simulate::Clock::now() - syncCPU &&
                 mj::Simulate::Clock::now() - startCPU < Seconds(refreshTime)) {
            // measure slowdown before first step
            if (!measured && elapsedSim) {
              sim.measured_slowdown =
                  std::chrono::duration<double>(elapsedCPU).count() / elapsedSim;
              measured = true;
            }

            // inject noise
            sim.InjectNoise();

            // call mj_step
            mj_step(m, d);
            // --- 保证控制器 update 被周期性调用 ---
            if (g_mujoco_control) {
              g_mujoco_control->update();
            }
            // ROS2 spin_some 也在锁内
            if (rclcpp::ok()) {
              executor.spin_some();
            }
            const char* message = Diverged(m->opt.disableflags, d);
            if (message) {
              sim.run = 0;
              mju::strcpy_arr(sim.load_error, message);
            } else {
              stepped = true;
            }

            // break if reset
            if (d->time < prevSim) {
              break;
            }
          }
        }

        // save current state to history buffer
        if (stepped) {
          sim.AddToHistory();
        }
      }

      // paused
      else {
        // run mj_forward, to update rendering and joint sliders
        mj_forward(m, d);
        sim.speed_changed = true;
      }
    }
  }
}

第三步，修改main函数启动循环及ros2节点，创建node节点来启动controller_manager，分离ros2控制线程和UI渲染主线程，可直接复制

int main(int argc, char** argv) {

  // display an error if running on macOS under Rosetta 2
#if defined(__APPLE__) && defined(__AVX__)
  if (rosetta_error_msg) {
    DisplayErrorDialogBox("Rosetta 2 is not supported", rosetta_error_msg);
    std::exit(1);
  }
#endif

  // print version, check compatibility
  std::printf("MuJoCo version %s\n", mj_versionString());
  if (mjVERSION_HEADER!=mj_version()) {
    mju_error("Headers and library have different versions");
  }

  // scan for libraries in the plugin directory to load additional plugins
  scanPluginLibraries();

#if defined(mjUSEUSD)
  // If USD is used, print the version.
  std::printf("OpenUSD version v%d.%02d\n", PXR_MINOR_VERSION, PXR_PATCH_VERSION);
#endif

  mjvCamera cam;
  mjv_defaultCamera(&cam);

  mjvOption opt;
  mjv_defaultOption(&opt);

  mjvPerturb pert;
  mjv_defaultPerturb(&pert);

  // simulate object encapsulates the UI
  auto sim = std::make_unique<mj::Simulate>(
      std::make_unique<mj::GlfwAdapter>(),
      &cam, &opt, &pert, /* is_passive = */ false
  );



  // ROS2 初始化
  rclcpp::init(argc, argv);
  auto ros_node = rclcpp::Node::make_shared(
    "mujoco_ros2_control_node",
    rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));

  // 读取 launch 传入参数
  std::string mujoco_model_path;
  if (ros_node->has_parameter("mujoco_model")) {
    mujoco_model_path = ros_node->get_parameter("mujoco_model").as_string();
  }
  if (mujoco_model_path.empty()) {
    for (int i = 1; i < argc; ++i) {
      std::string arg = argv[i];
      if (arg.size() > 4 && (arg.substr(arg.size()-4) == ".xml" || arg.substr(arg.size()-4) == ".mjb")) {
        mujoco_model_path = arg;
        break;
      }
    }
  }
  if (mujoco_model_path.empty()) {
    std::cerr << "ERROR: No MuJoCo model path found! Please set 'mujoco_model' ROS2 parameter, or pass .xml/.mjb file as argument." << std::endl;
    return 1;
  }
  const char* filename = mujoco_model_path.c_str();

  // 1. 先定义 executor，后面传递给物理线程
  rclcpp::executors::MultiThreadedExecutor executor;
  // 2. 启动物理线程加载模型和数据和推进仿真
  std::thread physicsthreadhandle([&]() {
    PhysicsThread(sim.get(), filename, executor);
  });

  // 2. 主线程等待 d 就绪
  std::cout << "[MuJoCo] 主线程: 等待物理线程加载模型和数据..." << std::endl;
  while (!d && rclcpp::ok() && !sim->exitrequest.load()) {
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
  }
  if (!d) {
    std::cerr << "[MuJoCo] 主线程: 等待 d 超时或被中断，退出" << std::endl;
    if (physicsthreadhandle.joinable()) physicsthreadhandle.join();
    return 1;
  }
  std::cout << "[MuJoCo] 主线程: m/d 已就绪，初始化控制器和 ROS2 executor" << std::endl;

  // 3. m/d 就绪后再初始化控制器和 executor
  mujoco_ros2_control::MujocoRos2Control mujoco_control(ros_node, m, d);
  g_mujoco_control = &mujoco_control;
  mujoco_control.init();
  executor.add_node(ros_node);

  // 4. 主线程只负责 UI 渲染
  std::cout << "[MuJoCo] 主线程: 进入 sim->RenderLoop() (UI 事件循环)" << std::endl;
  sim->RenderLoop();

  physicsthreadhandle.join();
  mj_deleteData(d);
  mj_deleteModel(m);
  return 0;
}

5.  打开Cmakelist.txt，设置新主程序的依赖，这里叫它simulate_main

file(GLOB SIMULATE_SRC
  "${CMAKE_CURRENT_SOURCE_DIR}/simulate/*.cc"
  "${CMAKE_CURRENT_SOURCE_DIR}/simulate/lodepng.cpp"
)

add_executable(simulate_main
  ${SIMULATE_SRC}
  src/mujoco_ros2_control.cpp
  src/mujoco_rendering.cpp
  src/mujoco_cameras.cpp
)
ament_target_dependencies(simulate_main ${THIS_PACKAGE_DEPENDS})
target_link_libraries(simulate_main
  ${MUJOCO_LIB}
  glfw
)
target_include_directories(simulate_main
  PUBLIC
    ${CMAKE_CURRENT_SOURCE_DIR}/simulate
    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
    $<INSTALL_INTERFACE:include>
    ${MUJOCO_INCLUDE_DIR}
    ${EIGEN3_INCLUDE_DIR}
    ${CMAKE_CURRENT_SOURCE_DIR}/src
    ${CMAKE_CURRENT_SOURCE_DIR}/simulate
)
install(TARGETS
  simulate_main
  DESTINATION lib/${PROJECT_NAME})

6. 最后，launch的时候的启动改成用simulate_main就好了

node_mujoco_ros2_control = Node(
        package='mujoco_ros2_control',
        executable='simulate_main',
        output='screen',
        parameters=[
            robot_description,
            controller_config_file,
            {'mujoco_model_path': os.path.join(mujoco_ros2_control_demos_path, 'mujoco_xml', 'my_mujoco_arm.xml')},
        ]
    )

最后你的仿真环境就能达到这种原生态效果：