jazzy-dev

mujoco_ros2_control/include/mujoco_ros2_control/mujoco_rendering.hpp

@@ -12,6 +12,7 @@ class MujocoRendering
 public:
   MujocoRendering(const MujocoRendering& obj) = delete;
   void operator=(const MujocoRendering &) = delete;
+  ~MujocoRendering();
 
   static MujocoRendering* get_instance();
   void init(rclcpp::Node::SharedPtr & node, mjModel* mujoco_model, mjData* mujoco_data);

mujoco_ros2_control/include/mujoco_ros2_control/mujoco_ros2_control.hpp

@@ -18,16 +18,18 @@ class MJResourceManager;
 class MujocoRos2Control
 {
 public:
-  MujocoRos2Control(const rclcpp::Node::SharedPtr & node, const rclcpp::NodeOptions & cm_node_option, mjModel* mujoco_model, mjData* mujoco_data);
+  MujocoRos2Control(const rclcpp::Node::SharedPtr & node, const rclcpp::NodeOptions & cm_node_option);
   ~MujocoRos2Control();
   void init();
   void update();
+  mjData* getMjData();
+  mjModel* getMjModel();
 
 private:
   void publish_sim_time(rclcpp::Time sim_time);
   rclcpp::Node::SharedPtr node_;
-  mjModel* mj_model_;
-  mjData* mj_data_;
+  mjModel* mj_model_ = nullptr;
+  mjData* mj_data_ = nullptr;
   rclcpp::NodeOptions cm_node_option_;
 
   rclcpp::Logger logger_;

mujoco_ros2_control/src/mujoco_rendering.cpp

@@ -2,6 +2,11 @@
 
 namespace mujoco_ros2_control
 {
+MujocoRendering::~MujocoRendering()
+{
+  close();
+}
+
 MujocoRendering* MujocoRendering::instance_ = nullptr;
 
 MujocoRendering* MujocoRendering::get_instance()

mujoco_ros2_control/src/mujoco_ros2_control.cpp

@@ -8,17 +8,23 @@ namespace mujoco_ros2_control
 {
 class MJResourceManager : public hardware_interface::ResourceManager{
 public:
-  MJResourceManager(rclcpp::Node::SharedPtr& node, mjModel* mj_model, mjData* mj_data)
+  MJResourceManager(rclcpp::Node::SharedPtr& node, mjModel*& mj_model, mjData*& mj_data)
   : hardware_interface::ResourceManager(node->get_node_clock_interface(), node->get_node_logging_interface()),
         mj_system_loader_("mujoco_ros2_control", "mujoco_ros2_control::MujocoSystemInterface"),
-        logger_(node->get_logger().get_child("MJResourceManager"))
+        logger_(node->get_logger().get_child("MJResourceManager")), mj_model_(mj_model), mj_data_(mj_data)
   {
-    mj_model_ = mj_model;
-    mj_data_ = mj_data;
     node_ = node;
   }
+
   MJResourceManager(const MJResourceManager&) = delete;
 
+  ~MJResourceManager() override
+  {
+    // release resources when exit or failure
+    mj_deleteModel(mj_model_);
+    mj_deleteData(mj_data_);
+  }
+
   /// Called from ControllerManager when {robot_description} is initialized from callback.
   /**
    * Override from hardware_interface::ResourceManager
@@ -32,6 +38,22 @@ class MJResourceManager : public hardware_interface::ResourceManager{
 
     const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf);
 
+    // generate mj_data_ and mj_model_ for SystemInterface
+    auto model_path = node_->get_parameter("mujoco_model_path").as_string();
+    // load and compile model
+    char error[1000] = "Could not load binary model";
+    if (std::strlen(model_path.c_str())>4 && !std::strcmp(model_path.c_str()+std::strlen(model_path.c_str())-4, ".mjb")) {
+      mj_model_ = mj_loadModel(model_path.c_str(), 0);
+    } else {
+      mj_model_ = mj_loadXML(model_path.c_str(), 0, error, 1000);
+    }
+    if (!mj_model_) {
+      mju_error("Load model error: %s", error);
+      return !components_are_loaded_and_initialized_;
+    }
+    RCLCPP_INFO_STREAM(logger_, "Mujoco model has been successfully loaded !");
+    mj_data_ = mj_makeData(mj_model_);
+
     for (const auto& individual_hardware_info : hardware_info)
     {
       std::string robot_hw_sim_type_str_ = individual_hardware_info.hardware_plugin_name;
@@ -68,12 +90,12 @@ class MJResourceManager : public hardware_interface::ResourceManager{
   std::shared_ptr<rclcpp::Node> node_;
   pluginlib::ClassLoader<MujocoSystemInterface> mj_system_loader_;
   rclcpp::Logger logger_;
-  mjModel* mj_model_;
-  mjData* mj_data_;
+  mjModel*& mj_model_;
+  mjData*& mj_data_;
 };
 
-MujocoRos2Control::MujocoRos2Control(const rclcpp::Node::SharedPtr & node, const rclcpp::NodeOptions & cm_node_option, mjModel* mujoco_model, mjData* mujoco_data)
-  : node_(node), mj_model_(mujoco_model), mj_data_(mujoco_data), cm_node_option_(cm_node_option),
+MujocoRos2Control::MujocoRos2Control(const rclcpp::Node::SharedPtr & node, const rclcpp::NodeOptions & cm_node_option)
+  : node_(node), cm_node_option_(cm_node_option),
       logger_(rclcpp::get_logger(node_->get_name() + std::string(".mujoco_ros2_control"))),
       control_period_(rclcpp::Duration(1, 0)), last_update_sim_time_ros_(0, 0, RCL_ROS_TIME)
 {
@@ -124,6 +146,7 @@ void MujocoRos2Control::init()
   cm_thread_ = std::thread(spin);
 
   // Waiting RM to be initialized through topic robot_description
+  // mj_data_ and mj_model_ will be allocated in the RM simultaneously
   while(!controller_manager_->is_resource_manager_initialized())
   {
     RCLCPP_WARN(logger_, "Waiting RM to load and initialize hardware...");
@@ -154,6 +177,10 @@ void MujocoRos2Control::update()
   mj_step2(mj_model_, mj_data_);
 }
 
+mjData* MujocoRos2Control::getMjData() {return mj_data_;}
+
+mjModel* MujocoRos2Control::getMjModel() {return mj_model_;}
+
 void MujocoRos2Control::publish_sim_time(rclcpp::Time sim_time)
 {
   // TODO

mujoco_ros2_control/src/mujoco_ros2_control_node.cpp

@@ -5,10 +5,6 @@
 #include "mujoco_ros2_control/mujoco_ros2_control.hpp"
 #include "mujoco_ros2_control/mujoco_rendering.hpp"
 
-// MuJoCo data structures
-mjModel* mujoco_model = nullptr;
-mjData* mujoco_data = nullptr;
-
 // main function
 int main(int argc, const char** argv) {
   rclcpp::init(argc, argv);
@@ -24,30 +20,15 @@ int main(int argc, const char** argv) {
   }
   cm_node_options.arguments(node_arguments);
 
-  RCLCPP_INFO_STREAM(node->get_logger(), "Initializing mujoco_ros2_control node...");
-  auto model_path = node->get_parameter("mujoco_model_path").as_string();
-
-  // load and compile model
-  char error[1000] = "Could not load binary model";
-  if (std::strlen(model_path.c_str())>4 && !std::strcmp(model_path.c_str()+std::strlen(model_path.c_str())-4, ".mjb")) {
-    mujoco_model = mj_loadModel(model_path.c_str(), 0);
-  } else {
-    mujoco_model = mj_loadXML(model_path.c_str(), 0, error, 1000);
-  }
-  if (!mujoco_model) {
-    mju_error("Load model error: %s", error);
-  }
-
-  RCLCPP_INFO_STREAM(node->get_logger(), "Mujoco model has been successfully loaded !");
-  // make data
-  mujoco_data = mj_makeData(mujoco_model);
-
   // initialize mujoco control
-  auto control = mujoco_ros2_control::MujocoRos2Control(node, cm_node_options, mujoco_model, mujoco_data);
+  RCLCPP_INFO_STREAM(node->get_logger(), "Initializing mujoco_ros2_control node...");
+  auto control = mujoco_ros2_control::MujocoRos2Control(node, cm_node_options);
   control.init();
   RCLCPP_INFO_STREAM(node->get_logger(), "Mujoco ros2 controller has been successfully initialized !");
 
-  // initialize mujoco redering
+  // initialize mujoco rendering
+  mjData* mujoco_data = control.getMjData();
+  mjModel* mujoco_model = control.getMjModel();
   auto rendering = mujoco_ros2_control::MujocoRendering::get_instance();
   rendering->init(node, mujoco_model, mujoco_data);
   RCLCPP_INFO_STREAM(node->get_logger(), "Mujoco rendering has been successfully initialized !");
@@ -65,12 +46,6 @@ int main(int argc, const char** argv) {
     rendering->update();
   }
 
-  rendering->close();
-
-  // free MuJoCo model and data
-  mj_deleteData(mujoco_data);
-  mj_deleteModel(mujoco_model);
   RCLCPP_INFO(node->get_logger(), "Mujoco Sim Stop ...");
-
   return 0;
 }