170 bool initialize(rclcpp::Node::SharedPtr node,
const std::string& model_path,
const std::string& mujoco_model_topic,
171 double sim_speed_factor,
bool headless);
250 bool set_free_joint_states(
const std::vector<mujoco_ros2_control_msgs::msg::FreeJointState>& free_joints,
251 std::string& error_message);
306 std::vector<mjtNum> qpos;
307 std::vector<mjtNum> qvel;
308 std::vector<mjtNum> act;
309 std::vector<mjtNum> qfrc_actuator;
310 std::vector<mjtNum> sensordata;
311 std::vector<mjtNum> ctrl;
339 RCLCPP_WARN_EXPRESSION(logger_, sim_mutex_ ==
nullptr,
"Sim recursive mutex is still nullptr");
350 return step_count_.load();
359 void apply_staged_control_inputs();
367 void publish_control_state();
376 void refresh_data_snapshot();
386 void publish_clock();
391 void update_sim_display();
403 void reset_world_state(
bool fill_initial_state,
const mujoco_ros2_control_msgs::msg::SimulationState& state_overrides);
412 int frame_body_id(
const std::string& frame_id)
const;
420 bool validate_frame_id(
const std::string& frame_id,
const std::string& field_label, std::string& error_message)
const;
425 int find_free_joint_id(
int body_id)
const;
435 bool validate_free_joint_states(
const std::vector<mujoco_ros2_control_msgs::msg::FreeJointState>& free_joints,
436 std::string& error_message);
446 void apply_free_joint_states(
const std::vector<mujoco_ros2_control_msgs::msg::FreeJointState>& free_joints);
458 bool validate_joint_state_overrides(
const sensor_msgs::msg::JointState& joint_state, std::string& error_message);
466 void apply_joint_state_overrides(
const sensor_msgs::msg::JointState& joint_state);
469 void reset_world_callback(
const std::shared_ptr<mujoco_ros2_control_msgs::srv::ResetWorld::Request> request,
470 std::shared_ptr<mujoco_ros2_control_msgs::srv::ResetWorld::Response> response);
471 void set_pause_callback(
const std::shared_ptr<mujoco_ros2_control_msgs::srv::SetPause::Request> request,
472 std::shared_ptr<mujoco_ros2_control_msgs::srv::SetPause::Response> response);
473 void step_simulation_callback(
const std::shared_ptr<mujoco_ros2_control_msgs::srv::StepSimulation::Request> request,
474 std::shared_ptr<mujoco_ros2_control_msgs::srv::StepSimulation::Response> response);
476 set_free_joint_state_callback(
const std::shared_ptr<mujoco_ros2_control_msgs::srv::SetFreeJointState::Request> request,
477 std::shared_ptr<mujoco_ros2_control_msgs::srv::SetFreeJointState::Response> response);
479 rclcpp::Logger get_logger()
const
485 rclcpp::Logger logger_ = rclcpp::get_logger(
"MujocoSimulation");
488 rclcpp::Node::SharedPtr node_;
491 std::string model_path_;
492 std::string mujoco_model_topic_;
496 mjModel* mj_model_{
nullptr };
500 mjData* mj_data_{
nullptr };
507 mjData* snapshot_write_{
nullptr };
508 mjData* snapshot_read_{
nullptr };
509 bool snapshot_ready_{
false };
512 std::vector<mjtNum> ctrl_staged_;
513 std::vector<mjtNum> qfrc_applied_staged_;
517 bool control_inputs_staged_{
false };
521 std::mutex data_exchange_mutex_;
526 std::atomic<bool> snapshot_refresh_requested_{
true };
533 std::mutex control_staging_mutex_;
540 std::mutex control_state_mutex_;
549 double sim_speed_factor_{ -1.0 };
552 bool headless_{
false };
555 std::unique_ptr<mujoco::Simulate> sim_;
558 std::thread physics_thread_;
559 std::thread ui_thread_;
561 std::atomic<bool> explicit_shutdown_requested_{
false };
564 std::shared_ptr<rclcpp::Publisher<rosgraph_msgs::msg::Clock>> clock_publisher_;
565 realtime_tools::RealtimePublisher<rosgraph_msgs::msg::Clock>::SharedPtr clock_realtime_publisher_;
572 std::recursive_mutex* sim_mutex_{
nullptr };
575 rclcpp::CallbackGroup::SharedPtr reset_world_cb_group_;
576 rclcpp::Service<mujoco_ros2_control_msgs::srv::ResetWorld>::SharedPtr reset_world_service_;
579 rclcpp::CallbackGroup::SharedPtr set_pause_cb_group_;
580 rclcpp::Service<mujoco_ros2_control_msgs::srv::SetPause>::SharedPtr set_pause_service_;
583 rclcpp::CallbackGroup::SharedPtr step_simulation_cb_group_;
584 rclcpp::Service<mujoco_ros2_control_msgs::srv::StepSimulation>::SharedPtr step_simulation_service_;
587 rclcpp::CallbackGroup::SharedPtr set_free_joint_state_cb_group_;
588 rclcpp::Service<mujoco_ros2_control_msgs::srv::SetFreeJointState>::SharedPtr set_free_joint_state_service_;
591 std::atomic<uint32_t> pending_steps_{ 0 };
592 std::atomic<bool> step_diverged_{
false };
593 std::atomic<bool> steps_interrupted_{
false };
594 std::atomic<bool> keyboard_step_requested_{
false };
595 std::atomic<uint64_t> step_count_{ 0 };
596 std::mutex steps_cv_mutex_;
597 std::condition_variable steps_cv_;
600 std::vector<mjtNum> initial_qpos_;
601 std::vector<mjtNum> initial_qvel_;
602 std::vector<mjtNum> initial_ctrl_;