base_velocity_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__BASE_VELOCITY_PLUGIN_HPP_
#define MUJOCO_ROS2_CONTROL_PLUGINS__BASE_VELOCITY_PLUGIN_HPP_
 
#include <limits>
#include <mutex>
#include <string>
 
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <rclcpp/rclcpp.hpp>
 
#include "mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp"
 
namespace mujoco_ros2_control_plugins
{
 
class BaseVelocityPlugin : public MuJoCoROS2ControlPluginBase
{
public:
  BaseVelocityPlugin() = default;
  ~BaseVelocityPlugin() override = default;
 
  bool init(rclcpp::Node::SharedPtr node, const mjModel* model, mjData* data) override;
  void pre_step(mjData* data) override;
  void cleanup() override;
 
private:
  void twistCallback(const geometry_msgs::msg::Twist& msg);
  void twistStampedCallback(const geometry_msgs::msg::TwistStamped& msg);
  void storeCommand(double vx, double vy, double wz);
 
  // ROS interfaces
  rclcpp::Node::SharedPtr node_;
  rclcpp::Logger logger_{ rclcpp::get_logger("BaseVelocityPlugin") };
  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_sub_;
  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr twist_stamped_sub_;
 
  // Model/body/joint lookup
  const mjModel* model_{ nullptr };
  int body_id_{ -1 };
  int qvel_adr_{ -1 };
 
  // Clamp parameters
  double max_linear_velocity_{ std::numeric_limits<double>::infinity() };
  double max_yaw_rate_{ std::numeric_limits<double>::infinity() };
  rclcpp::Duration cmd_timeout_{ 0, 0 };
 
  struct CommandState
  {
    double vx{ 0.0 };
    double vy{ 0.0 };
    double wz{ 0.0 };
    rclcpp::Time time{ 0, 0, RCL_ROS_TIME };
  };
 
  // Latest command, written by the subscription callback (ROS executor thread) under
  // cmd_mutex_. pre_step() (physics thread) copies it into cached_cmd_ via try_lock, so
  // it never blocks on the subscription callback.
  std::mutex cmd_mutex_;
  CommandState latest_cmd_;
 
  // Local copy of the command, only ever touched from pre_step() (physics thread).
  CommandState cached_cmd_;
};
 
}  // namespace mujoco_ros2_control_plugins
 
#endif  // MUJOCO_ROS2_CONTROL_PLUGINS__BASE_VELOCITY_PLUGIN_HPP_