ExternalForce.h

Go to the documentation of this file.

#pragma once
 
#include <rclcpp/rclcpp.hpp>
 
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mjvisualize.h>
 
#include <mujoco_ros_utils/msg/external_force.hpp>
#include <string>
 
namespace MujocoRosUtils
{
 
class ExternalForce
{
public:
  static void RegisterPlugin();
 
  static ExternalForce * Create(const mjModel * m, mjData * d, int plugin_id);
 
public:
  ExternalForce(ExternalForce &&) = default;
 
  void reset(const mjModel * m, int plugin_id);
 
  void compute(const mjModel * m, mjData * d, int plugin_id);
 
  void visualize(const mjModel * m, mjData * d, const mjvOption * opt, mjvScene * scn, int plugin_id);
 
protected:
  ExternalForce(const mjModel * m, mjData * d, int body_id, const std::string & topic_name, mjtNum vis_scale);
 
  void callback(const mujoco_ros_utils::msg::ExternalForce::SharedPtr msg);
 
protected:
  rclcpp::Node::SharedPtr nh_;
 
  rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
 
  rclcpp::Subscription<mujoco_ros_utils::msg::ExternalForce>::SharedPtr sub_;
 
  int body_id_ = -1;
 
  std::string topic_name_;
 
  std::shared_ptr<mujoco_ros_utils::msg::ExternalForce> msg_;
 
  mjtNum end_time_ = -1;
 
  mjtNum vis_scale_;
};
 
} // namespace MujocoRosUtils