jazzy-dev

.gitignore

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

mujoco_ros2_control/CMakeLists.txt

@@ -52,7 +52,7 @@ else()
   message(FATAL_ERROR "Failed to find mujoco with find_package. Either build and install mujoco from source or set the MUJOCO_DIR environment variable to tell CMake where to find the binary install. ")
 endif (mujoco_FOUND)
 
-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

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,17 +12,23 @@
 
 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
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::NodeOptions cm_node_option_;
   mjModel* mj_model_;
   mjData* mj_data_;
 
@@ -32,7 +38,7 @@ class MujocoRos2Control
   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()