mujoco_system_interface.hpp

 
#pragma once
 
#include <condition_variable>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <vector>
 
#include <hardware_interface/version.h>
#include <atomic>
#include <hardware_interface/handle.hpp>
#include <hardware_interface/hardware_info.hpp>
#include <hardware_interface/system_interface.hpp>
#include <hardware_interface/types/hardware_interface_return_values.hpp>
#include <mujoco_ros2_control_msgs/srv/reset_world.hpp>
#include <mujoco_ros2_control_msgs/srv/set_pause.hpp>
#include <mujoco_ros2_control_msgs/srv/step_simulation.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/macros.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp>
#include <rclcpp_lifecycle/state.hpp>
#include <realtime_tools/realtime_publisher.hpp>
#include <rosgraph_msgs/msg/clock.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
 
#include <mujoco/mujoco.h>
 
// Pull in the Simulate class and PhysicsThread/RenderLoop declarations:
#include "glfw_adapter.h"  // for mj::GlfwAdapter
#include "simulate.h"      // must be on your include path, handled by CMake
 
#include "mujoco_ros2_control/data.hpp"
#include "mujoco_ros2_control/mujoco_cameras.hpp"
#include "mujoco_ros2_control/mujoco_lidar.hpp"
 
#include <pluginlib/class_list_macros.hpp>
#include <pluginlib/class_loader.hpp>
#include "mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp"
#include "transmission_interface/transmission.hpp"
#include "transmission_interface/transmission_interface_exception.hpp"
#include "transmission_interface/transmission_loader.hpp"
 
#define ROS_DISTRO_HUMBLE (HARDWARE_INTERFACE_VERSION_MAJOR < 3)
 
// defining these for Humble, because they are defined elsewhere in future versions, and we use them in this file
#if ROS_DISTRO_HUMBLE
namespace hardware_interface
{
constexpr char HW_IF_TORQUE[] = "torque";
constexpr char HW_IF_FORCE[] = "force";
}  // namespace hardware_interface
#endif
 
namespace mujoco_ros2_control
{
class MujocoSystemInterface : public hardware_interface::SystemInterface
{
public:
  MujocoSystemInterface();
  ~MujocoSystemInterface() override;
 
  hardware_interface::CallbackReturn
// Jazzy introduces a new HarwareComponentInterfaceParams object which doesn't exist in humble. This adds
// compatibility by switching to the old interface, which behaves similarly
#if ROS_DISTRO_HUMBLE
  on_init(const hardware_interface::HardwareInfo& info) override;
#else
  on_init(const hardware_interface::HardwareComponentInterfaceParams& params) override;
#endif
  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
 
  hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
  hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
 
  hardware_interface::return_type perform_command_mode_switch(const std::vector<std::string>& start_interfaces,
                                                              const std::vector<std::string>& stop_interfaces) override;
 
  hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override;
  hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override;
 
// In humble this method doesn't exist, so we just add it back in with the implementation
#if ROS_DISTRO_HUMBLE
  const hardware_interface::HardwareInfo& get_hardware_info() const
  {
    return info_;
  }
#endif
  void actuator_state_to_joint_state();
 
  void joint_command_to_actuator_command();
  void get_model(mjModel*& dest);
 
  void get_data(mjData*& dest);
 
  void set_data(mjData* mj_data);
 
protected:
  rclcpp::Logger get_logger() const;
 
private:
  bool register_mujoco_actuators();
 
  void register_urdf_joints(const hardware_interface::HardwareInfo& info);
 
  bool register_transmissions(const hardware_interface::HardwareInfo& info);
 
  bool initialize_initial_positions(const hardware_interface::HardwareInfo& info);
 
  bool apply_keyframe(const std::string& keyframe_name);
 
  void register_sensors(const hardware_interface::HardwareInfo& info);
 
  bool set_override_start_positions(const std::string& override_start_position_file);
 
  void set_initial_pose();
 
  void reset_simulation_state(bool fill_initial_state);
 
  void reset_world_callback(const std::shared_ptr<mujoco_ros2_control_msgs::srv::ResetWorld::Request> request,
                            std::shared_ptr<mujoco_ros2_control_msgs::srv::ResetWorld::Response> response);
 
  void set_pause_callback(const std::shared_ptr<mujoco_ros2_control_msgs::srv::SetPause::Request> request,
                          std::shared_ptr<mujoco_ros2_control_msgs::srv::SetPause::Response> response);
 
  void step_simulation_callback(const std::shared_ptr<mujoco_ros2_control_msgs::srv::StepSimulation::Request> request,
                                std::shared_ptr<mujoco_ros2_control_msgs::srv::StepSimulation::Response> response);
 
  void PhysicsLoop();
 
  void publish_clock();
 
  void update_sim_display();
 
 
  rclcpp::Node::SharedPtr get_node() const;
 
 
  void load_mujoco_plugins();
 
  // System information
  std::string model_path_;
 
  // MuJoCo data pointers
  mjModel* mj_model_{ nullptr };
  mjData* mj_data_{ nullptr };
 
  // Data container for control data
  mjData* mj_data_control_{ nullptr };
 
  // For rendering
  mjvCamera cam_;
  mjvOption opt_;
  mjvPerturb pert_;
 
  // Logger
  rclcpp::Logger logger_ = rclcpp::get_logger("MujocoSystemInterface");
 
  // Speed scaling parameter. if set to >0 then we ignore the value set in the simulate app and instead
  // attempt to loop at whatever this is set to. If this is <0, then we use the value from the app.
  double sim_speed_factor_;
 
  // True when running without a display (no GLFW window)
  bool headless_{ false };
 
  // Primary simulate object
  std::unique_ptr<mujoco::Simulate> sim_;
 
  // Threads for rendering physics, the UI simulation, and the ROS node
  std::thread physics_thread_;
  std::thread ui_thread_;
 
  // Provides access to ROS interfaces for elements that require it
  std::shared_ptr<rclcpp::Node> mujoco_node_;
  std::unique_ptr<rclcpp::executors::MultiThreadedExecutor> executor_;
  std::thread executor_thread_;
 
  // Primary clock publisher for the world
  std::shared_ptr<rclcpp::Publisher<rosgraph_msgs::msg::Clock>> clock_publisher_;
  realtime_tools::RealtimePublisher<rosgraph_msgs::msg::Clock>::SharedPtr clock_realtime_publisher_;
 
  // Actuators state publisher
  std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::JointState>> actuator_state_publisher_ = nullptr;
  realtime_tools::RealtimePublisher<sensor_msgs::msg::JointState>::SharedPtr actuator_state_realtime_publisher_ =
      nullptr;
  sensor_msgs::msg::JointState actuator_state_msg_;
 
  // Floating base state publisher
  std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> floating_base_publisher_ = nullptr;
  realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>::SharedPtr floating_base_realtime_publisher_ = nullptr;
  nav_msgs::msg::Odometry floating_base_msg_;
 
  // Free joint data
  int free_joint_id_ = -1;
  int free_joint_qpos_adr_ = -1;
  int free_joint_qvel_adr_ = -1;
 
  // Containers for RGB-D cameras
  std::unique_ptr<MujocoCameras> cameras_;
 
  // Containers for LIDAR sensors
  std::unique_ptr<MujocoLidar> lidar_sensors_;
 
  // Mutex used inside simulate.h for protecting model/data, we keep a reference
  // here to protect access to shared data.
  // TODO: It would be far better to put all relevant data into a single container with accessors
  //       in a common location rather than passing around the raw pointer to the mutex, but it would
  //       require more work to pull it out of simulate.h.
  std::recursive_mutex* sim_mutex_{ nullptr };
 
  // Data containers for the HW interface
  std::unordered_map<std::string, hardware_interface::ComponentInfo> joint_hw_info_;
  std::unordered_map<std::string, hardware_interface::ComponentInfo> sensors_hw_info_;
 
  // Data containers for the MuJoCo Actuators
  std::vector<MuJoCoActuatorData> mujoco_actuator_data_;
 
  // Data containers for the URDF joints
  std::vector<URDFJointData> urdf_joint_data_;
 
  // Transmission instances
  std::unique_ptr<pluginlib::ClassLoader<transmission_interface::TransmissionLoader>> transmission_loader_ = nullptr;
  std::vector<std::shared_ptr<transmission_interface::Transmission>> transmission_instances_;
 
  // Plugin loader and instances
  std::unique_ptr<pluginlib::ClassLoader<mujoco_ros2_control_plugins::MuJoCoROS2ControlPluginBase>> plugin_loader_ =
      nullptr;
  std::vector<std::shared_ptr<mujoco_ros2_control_plugins::MuJoCoROS2ControlPluginBase>> plugin_instances_;
 
  std::vector<FTSensorData> ft_sensor_data_;
  std::vector<IMUSensorData> imu_sensor_data_;
 
  bool override_mujoco_actuator_positions_{ false };
  bool override_urdf_joint_positions_{ false };
 
  // Reset world service
  rclcpp::CallbackGroup::SharedPtr reset_world_cb_group_;
  rclcpp::Service<mujoco_ros2_control_msgs::srv::ResetWorld>::SharedPtr reset_world_service_;
 
  // Set pause service
  rclcpp::CallbackGroup::SharedPtr set_pause_cb_group_;
  rclcpp::Service<mujoco_ros2_control_msgs::srv::SetPause>::SharedPtr set_pause_service_;
 
  // Step simulation service
  rclcpp::CallbackGroup::SharedPtr step_simulation_cb_group_;
  rclcpp::Service<mujoco_ros2_control_msgs::srv::StepSimulation>::SharedPtr step_simulation_service_;
 
  // Pending steps to execute while paused, and synchronization for blocking callers
  std::atomic<uint32_t> pending_steps_{ 0 };
  std::atomic<bool> step_diverged_{ false };
  std::atomic<bool> steps_interrupted_{ false };
  std::atomic<bool> keyboard_step_requested_{ false };
  std::atomic<uint64_t> step_count_{ 0 };
  std::mutex steps_cv_mutex_;
  std::condition_variable steps_cv_;
 
  // Storage for initial state (used for reset_world)
  std::vector<mjtNum> initial_qpos_;
  std::vector<mjtNum> initial_qvel_;
  std::vector<mjtNum> initial_ctrl_;
  std::string initial_keyframe_ = "";
};
 
}  // namespace mujoco_ros2_control