jazzy-dev

.gitignore

@@ -3,6 +3,9 @@ build/
 install/
 log/
 
+# clion
+**/cmake-build-debug
+**/.idea
 # vscode
 **/.vscode
 **/.devcontainer

mujoco_ros2_control/CMakeLists.txt

@@ -36,14 +36,23 @@ set(THIS_PACKAGE_DEPENDS
 )
 set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
 
-find_library(MUJOCO_LIB mujoco HINTS $ENV{MUJOCO_DIR}/lib)
+find_package(mujoco QUIET)
+if(mujoco_FOUND)
+  message(STATUS "Mujoco build from source")
+  set(MUJOCO_LIB mujoco::mujoco)
+  set(MUJOCO_INCLUDE_DIR ${MUJOCO_INCLUDE_DIR})
+else (mujoco_FOUND)
+  message(STATUS "Mujoco build from binary")
+  find_library(MUJOCO_LIB mujoco HINTS $ENV{MUJOCO_DIR}/lib)
+  set(MUJOCO_INCLUDE_DIR $ENV{MUJOCO_DIR}/include)
+endif (mujoco_FOUND)
 
 include_directories(include)
 
 add_library(mujoco_system_plugins SHARED src/mujoco_system.cpp)
 ament_target_dependencies(mujoco_system_plugins ${THIS_PACKAGE_DEPENDS})
 target_link_libraries(mujoco_system_plugins ${MUJOCO_LIB})
-target_include_directories(mujoco_system_plugins PUBLIC $ENV{MUJOCO_DIR}/include ${EIGEN3_INCLUDE_DIR})
+target_include_directories(mujoco_system_plugins PUBLIC ${MUJOCO_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR})
 
 install(TARGETS
   mujoco_system_plugins
@@ -56,7 +65,7 @@ install(TARGETS
 add_executable(mujoco_ros2_control src/mujoco_ros2_control_node.cpp src/mujoco_rendering.cpp src/mujoco_ros2_control.cpp)
 ament_target_dependencies(mujoco_ros2_control ${THIS_PACKAGE_DEPENDS})
 target_link_libraries(mujoco_ros2_control ${MUJOCO_LIB} glfw)
-target_include_directories(mujoco_ros2_control PUBLIC $ENV{MUJOCO_DIR}/include ${EIGEN3_INCLUDE_DIR})
+target_include_directories(mujoco_ros2_control PUBLIC ${MUJOCO_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR})
 
 install(TARGETS
   mujoco_ros2_control

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

@@ -12,27 +12,33 @@
 
 namespace mujoco_ros2_control
 {
+// declare in advance
+class MJResourceManager;
+
 class MujocoRos2Control
 {
 public:
-  MujocoRos2Control(rclcpp::Node::SharedPtr & node, 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_;  // TODO: delete node
-  mjModel* mj_model_;
-  mjData* mj_data_;
+  rclcpp::Node::SharedPtr node_;
+  mjModel* mj_model_ = nullptr;
+  mjData* mj_data_ = nullptr;
+  rclcpp::NodeOptions cm_node_option_;
 
   rclcpp::Logger logger_;
   std::shared_ptr<pluginlib::ClassLoader<MujocoSystemInterface>> robot_hw_sim_loader_;
 
   std::shared_ptr<controller_manager::ControllerManager> controller_manager_;
   rclcpp::executors::MultiThreadedExecutor::SharedPtr cm_executor_;
   std::thread cm_thread_;
-  bool stop_cm_thread_;
+  bool stop_cm_thread_ = false;
   rclcpp::Duration control_period_;
 
   rclcpp::Time last_update_sim_time_ros_;

mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system.hpp

@@ -79,6 +79,7 @@ class MujocoSystem : public MujocoSystemInterface
     SensorData<Eigen::Quaternion<double>> orientation;
     SensorData<Eigen::Vector3d> angular_velocity;
     SensorData<Eigen::Vector3d> linear_velocity;
+    SensorData<Eigen::Vector3d> linear_acceleration;
   };
 
 private:
@@ -102,7 +103,7 @@ class MujocoSystem : public MujocoSystemInterface
   mjModel* mj_model_;
   mjData* mj_data_;
 
-  rclcpp::Logger logger_;  // TODO: delete?
+  rclcpp::Logger logger_;
 };
 }  // namespace mujoco_ros2_control
 

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

@@ -6,100 +6,121 @@
 
 namespace mujoco_ros2_control
 {
-MujocoRos2Control::MujocoRos2Control(rclcpp::Node::SharedPtr & node, mjModel* mujoco_model, mjData* mujoco_data)
-  : node_(node), mj_model_(mujoco_model), mj_data_(mujoco_data), 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)
-{
-}
-
-MujocoRos2Control::~MujocoRos2Control()
-{
-  stop_cm_thread_ = true;
-  cm_executor_->remove_node(controller_manager_);
-  cm_executor_->cancel();
-  cm_thread_.join();
-}
-
-void MujocoRos2Control::init()
-{
-  clock_publisher_ = node_->create_publisher<rosgraph_msgs::msg::Clock>("/clock", 10);
-  // Read urdf from ros parameter server then
-  // setup actuators and mechanism control node.
-  std::string urdf_string;
-  std::vector<hardware_interface::HardwareInfo> control_hardware_info;
-  try
+class MJResourceManager : public hardware_interface::ResourceManager{
+public:
+  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")), mj_model_(mj_model), mj_data_(mj_data)
   {
-    urdf_string = node_->get_parameter("robot_description").as_string();
-    control_hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf_string);
-  }
-  catch (const std::runtime_error & ex)
-  {
-    RCLCPP_ERROR_STREAM(logger_, "Error parsing URDF : " << ex.what());
-    return;
-  }
-
-  try
-  {
-    robot_hw_sim_loader_.reset(
-      new pluginlib::ClassLoader<MujocoSystemInterface>(
-        "mujoco_ros2_control",
-        "mujoco_ros2_control::MujocoSystemInterface"));
-  }
-  catch (pluginlib::LibraryLoadException & ex)
-  {
-    RCLCPP_ERROR_STREAM(logger_, "Failed to create hardware interface loader:  " << ex.what());
-    return;
+    node_ = node;
   }
 
-  std::unique_ptr<hardware_interface::ResourceManager> resource_manager =
-    std::make_unique<hardware_interface::ResourceManager>();
+  MJResourceManager(const MJResourceManager&) = delete;
 
-  try
-  {
-    resource_manager->load_urdf(urdf_string, false, false);
-  }
-  catch (...)
+  ~MJResourceManager() override
   {
-    RCLCPP_ERROR(logger_, "Error while initializing URDF!");
+    // release resources when exit or failure
+    mj_deleteModel(mj_model_);
+    mj_deleteData(mj_data_);
   }
 
-  for (const auto& hardware : control_hardware_info)
+  /// Called from ControllerManager when {robot_description} is initialized from callback.
+  /**
+   * Override from hardware_interface::ResourceManager
+   * \param[in] urdf string containing the URDF.
+   * \param[in] update_rate Update rate of the controller manager to calculate calling frequency
+   * of async components.
+   */
+  bool load_and_initialize_components(const std::string& urdf, unsigned  int update_rate) override
   {
-    std::string robot_hw_sim_type_str_ = hardware.hardware_class_type;
-    std::unique_ptr<MujocoSystemInterface> mujoco_system;
-    try
-    {
-      mujoco_system = std::unique_ptr<MujocoSystemInterface>(
-        robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_));
+    components_are_loaded_and_initialized_ = true;
+
+    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);
     }
-    catch (pluginlib::PluginlibException & ex)
-    {
-      RCLCPP_ERROR_STREAM(logger_, "The plugin failed to load. Error: " << ex.what());
-      continue;
+    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_);
 
-    urdf::Model urdf_model;
-    urdf_model.initString(urdf_string);
-    if (!mujoco_system->init_sim(node_, mj_model_, mj_data_, urdf_model, hardware))
+    for (const auto& individual_hardware_info : hardware_info)
     {
-      RCLCPP_FATAL(logger_, "Could not initialize robot simulation interface");
-      return;
+      std::string robot_hw_sim_type_str_ = individual_hardware_info.hardware_plugin_name;
+      RCLCPP_DEBUG(logger_, "Load hardware interface %s ...", robot_hw_sim_type_str_.c_str());
+
+      // Load hardware
+      std::unique_ptr<MujocoSystemInterface> mjSimSystem;
+      std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_);
+      try
+      {
+        mjSimSystem = std::unique_ptr<MujocoSystemInterface>(
+          mj_system_loader_.createUnmanagedInstance(robot_hw_sim_type_str_));
+      } catch (pluginlib::PluginlibException & ex) {
+        RCLCPP_ERROR_STREAM(logger_, "The plugin failed to load. Error: " << ex.what());
+        continue;
+      }
+
+      // initialize simulation required resource from the hardware info.
+      urdf::Model urdf_model;
+      urdf_model.initString(urdf);
+      if(!mjSimSystem->init_sim(node_, mj_model_, mj_data_, urdf_model, individual_hardware_info))
+      {
+        RCLCPP_FATAL(logger_, "Could not initialize robot simulation interface");
+        components_are_loaded_and_initialized_ = false;
+        break;
+      }
+      RCLCPP_DEBUG(logger_, "Initialized hardware interface %s !", robot_hw_sim_type_str_.c_str());
+      import_component(std::move(mjSimSystem), individual_hardware_info);
     }
+    return components_are_loaded_and_initialized_;
+  }
 
-    resource_manager->import_component(std::move(mujoco_system), hardware);
+private:
+  std::shared_ptr<rclcpp::Node> node_;
+  pluginlib::ClassLoader<MujocoSystemInterface> mj_system_loader_;
+  rclcpp::Logger logger_;
+  mjModel*& mj_model_;
+  mjData*& mj_data_;
+};
+
+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)
+{
+}
 
-    rclcpp_lifecycle::State state(
-      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
-      hardware_interface::lifecycle_state_names::ACTIVE);
-    resource_manager->set_component_state(hardware.name, state);
-  }
+MujocoRos2Control::~MujocoRos2Control()
+{
+  stop_cm_thread_ = true;
+  cm_executor_->remove_node(controller_manager_);
+  cm_executor_->cancel();
+  cm_thread_.join();
+}
+
+void MujocoRos2Control::init()
+{
+  clock_publisher_ = node_->create_publisher<rosgraph_msgs::msg::Clock>("/clock", 10);
+  std::unique_ptr<hardware_interface::ResourceManager> resource_manager =
+    std::make_unique<mujoco_ros2_control::MJResourceManager>(node_, mj_model_, mj_data_);
 
   // Create the controller manager
   RCLCPP_INFO(logger_, "Loading controller_manager");
   cm_executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
   controller_manager_.reset(new controller_manager::ControllerManager(
       std::move(resource_manager), cm_executor_,
-      "controller_manager", node_->get_namespace()));
+      "controller_manager", node_->get_namespace(), cm_node_option_));
 
   cm_executor_->add_node(controller_manager_);
 
@@ -113,27 +134,32 @@ void MujocoRos2Control::init()
     std::chrono::duration_cast<std::chrono::nanoseconds>(
       std::chrono::duration<double>(1.0 / static_cast<double>(update_rate))));
 
-  // Force setting of use_sime_time parameter
+  // Force setting of use_sim_time parameter
   controller_manager_->set_parameter(rclcpp::Parameter("use_sim_time", rclcpp::ParameterValue(true)));
-
   stop_cm_thread_ = false;
   auto spin = [this]()
-    {
-      while (rclcpp::ok() && !stop_cm_thread_) {
-        cm_executor_->spin_once();
-      }
-    };
+  {
+    while (rclcpp::ok() && !stop_cm_thread_) {
+      cm_executor_->spin_once();
+    }
+  };
   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...");
+    std::this_thread::sleep_for(std::chrono::microseconds (2000000));
+  }
 }
 
 void MujocoRos2Control::update()
 {
   // Get the simulation time and period
-  auto sim_time = mj_data_->time;
-  int sim_time_sec = static_cast<int>(sim_time);
-  int sim_time_nanosec = static_cast<int>((sim_time - sim_time_sec)*1000000000);
+  std::chrono::duration<double> sim_time(static_cast<double>(mj_data_->time));
 
-  rclcpp::Time sim_time_ros(sim_time_sec, sim_time_nanosec, RCL_ROS_TIME);
+  rclcpp::Time sim_time_ros(std::chrono::duration_cast<std::chrono::nanoseconds>(sim_time).count(), RCL_ROS_TIME);
   rclcpp::Duration sim_period = sim_time_ros - last_update_sim_time_ros_;
 
   publish_sim_time(sim_time_ros);
@@ -145,13 +171,16 @@ void MujocoRos2Control::update()
     controller_manager_->update(sim_time_ros, sim_period);
     last_update_sim_time_ros_ = sim_time_ros;
   }
-
   // use same time as for read and update call - this is how it is done in ros2_control_node
   controller_manager_->write(sim_time_ros, sim_period);
 
   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