free_joint_state_publisher_plugin.hpp

// Copyright 2026 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#ifndef MUJOCO_ROS2_CONTROL_PLUGINS__FREE_JOINT_STATE_PUBLISHER_PLUGIN_HPP_
#define MUJOCO_ROS2_CONTROL_PLUGINS__FREE_JOINT_STATE_PUBLISHER_PLUGIN_HPP_
 
#include <memory>
#include <string>
#include <vector>
 
#include <mujoco_ros2_control_msgs/msg/free_joint_state_array.hpp>
#include <rclcpp/rclcpp.hpp>
#include <realtime_tools/realtime_publisher.hpp>
 
#include "mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp"
 
namespace mujoco_ros2_control_plugins
{
 
class FreeJointStatePublisherPlugin : public MuJoCoROS2ControlPluginBase
{
public:
  FreeJointStatePublisherPlugin() = default;
  ~FreeJointStatePublisherPlugin() override = default;
 
  bool init(rclcpp::Node::SharedPtr node, const mjModel* model, mjData* data) override;
  void update_write(const mjModel* model, mjData* data) override;
  void cleanup() override;
 
private:
  using FreeJointStateArray = mujoco_ros2_control_msgs::msg::FreeJointStateArray;
 
  struct FreeJointEntry
  {
    int body_id{ -1 };
    std::string body_name;
    int qpos_adr{ -1 };  
    int qvel_adr{ -1 };  
  };
 
  bool collect_free_joint_entries(const std::vector<std::string>& body_names);
 
  int resolve_frame_id(const std::string& frame_id) const;
 
  rclcpp::Node::SharedPtr node_;
  rclcpp::Logger logger_{ rclcpp::get_logger("FreeJointStatePublisherPlugin") };
 
  const mjModel* model_{ nullptr };
 
  rclcpp::Publisher<FreeJointStateArray>::SharedPtr publisher_raw_;
  std::unique_ptr<realtime_tools::RealtimePublisher<FreeJointStateArray>> publisher_;
 
  std::vector<FreeJointEntry> entries_;
 
  int frame_body_id_{ -1 };
  std::string effective_frame_id_;
 
  rclcpp::Duration publish_period_{ 0, 0 };
  rclcpp::Time last_publish_time_{ 0, 0, RCL_ROS_TIME };
 
  FreeJointStateArray message_;
};
 
}  // namespace mujoco_ros2_control_plugins
 
#endif  // MUJOCO_ROS2_CONTROL_PLUGINS__FREE_JOINT_STATE_PUBLISHER_PLUGIN_HPP_