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)
+add_library(mujoco_system_plugins SHARED src/mujoco_system.cpp src/mujoco_sensor.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_sensor.hpp

@@ -0,0 +1,65 @@
+#ifndef MUJOCO_ROS2_CONTROL_MUJOCO_SENSOR_HPP
+#define MUJOCO_ROS2_CONTROL_MUJOCO_SENSOR_HPP
+
+#include <string>
+#include <memory>
+#include <Eigen/Dense>
+#include "hardware_interface/system_interface.hpp"
+#include "mujoco/mujoco.h"
+
+namespace mujoco_ros2_control
+{
+template <typename T>
+struct SensorData
+{
+  std::string name;
+  T data;
+  int mj_sensor_index;
+};
+
+struct FTSensorData
+{
+  std::string name;
+  SensorData<Eigen::Vector3d> force;
+  SensorData<Eigen::Vector3d> torque;
+};
+
+class SensorBase{
+public:
+  std::string name_;
+
+  SensorBase() = default;
+
+  virtual bool init(const hardware_interface::ComponentInfo& sensor_info, const mjModel* mj_model) = 0;
+  virtual void registerStateIface(std::vector<hardware_interface::StateInterface>& state_interface) = 0;
+  virtual void read(const mjData* mj_data) = 0;
+};
+
+class IMUSensor final : public SensorBase{
+public:
+  IMUSensor() : SensorBase(){};
+  bool init(const hardware_interface::ComponentInfo& sensor_info, const mjModel* mj_model) override;
+  void registerStateIface(std::vector<hardware_interface::StateInterface>& state_interface) override;
+  void read(const mjData* mj_data) override;
+private:
+  SensorData<Eigen::Quaternion<double>> orientation;
+  SensorData<Eigen::Vector3d> angular_velocity;
+  SensorData<Eigen::Vector3d> linear_velocity;
+  SensorData<Eigen::Vector3d> linear_acceleration;
+};
+
+class FTSensor final : public SensorBase{
+public:
+  FTSensor() : SensorBase(){};
+  bool init(const hardware_interface::ComponentInfo& sensor_info, const mjModel* mj_model) override;
+  void registerStateIface(std::vector<hardware_interface::StateInterface>& state_interface) override;
+  void read(const mjData* mj_data) override;
+private:
+  SensorData<Eigen::Vector3d> force;
+  SensorData<Eigen::Vector3d> torque;
+  std::vector<std::string> iface_lists;
+};
+
+}  // namespace mujoco_ros2_control
+
+#endif  // MUJOCO_ROS2_CONTROL_MUJOCO_SENSOR_HPP

mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system.hpp

@@ -3,6 +3,7 @@
 
 #include <Eigen/Dense>
 #include "mujoco_ros2_control/mujoco_system_interface.hpp"
+#include "mujoco_ros2_control/mujoco_sensor.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "joint_limits/joint_limits.hpp"
 #include "control_toolbox/pid.hpp"
@@ -58,29 +59,6 @@ class MujocoSystem : public MujocoSystemInterface
     int mj_vel_adr;
   };
 
-  template <typename T>
-  struct SensorData
-  {
-    std::string name;
-    T data;
-    int mj_sensor_index;
-  };
-
-  struct FTSensorData
-  {
-    std::string name;
-    SensorData<Eigen::Vector3d> force;
-    SensorData<Eigen::Vector3d> torque;
-  };
-
-  struct IMUSensorData
-  {
-    std::string name;
-    SensorData<Eigen::Quaternion<double>> orientation;
-    SensorData<Eigen::Vector3d> angular_velocity;
-    SensorData<Eigen::Vector3d> linear_velocity;
-  };
-
 private:
   void register_joints(const urdf::Model& urdf_model, const hardware_interface::HardwareInfo & hardware_info);
   void register_sensors(const urdf::Model& urdf_model, const hardware_interface::HardwareInfo & hardware_info);
@@ -96,13 +74,13 @@ class MujocoSystem : public MujocoSystemInterface
   std::vector<hardware_interface::CommandInterface> command_interfaces_;
 
   std::vector<JointState> joint_states_;
-  std::vector<FTSensorData> ft_sensor_data_;
-  std::vector<IMUSensorData> imu_sensor_data_;
+  std::vector<IMUSensor> imu_sensors_;
+  std::vector<FTSensor> ft_sensors_;
 
   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()