controller_plugin.h

#pragma once
 
#include <control_msgs/JointControllerState.h>
#include <control_toolbox/pid.h>
#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <quad_msgs/LegCommand.h>
#include <quad_msgs/LegCommandArray.h>
#include <quad_msgs/MotorCommand.h>
#include <quad_utils/ros_utils.h>
#include <realtime_tools/realtime_buffer.h>
#include <realtime_tools/realtime_publisher.h>
#include <ros/node_handle.h>
#include <urdf/model.h>
 
namespace effort_controllers {
class QuadController : public controller_interface::Controller<
                           hardware_interface::EffortJointInterface> {
  typedef std::vector<quad_msgs::LegCommand> BufferType;
 
 public:
  QuadController();
  ~QuadController();
 
  bool init(hardware_interface::EffortJointInterface* hw, ros::NodeHandle& n);
  void update(const ros::Time& /*time*/, const ros::Duration& /*period*/);
 
  std::vector<std::string> joint_names_;
  std::vector<hardware_interface::JointHandle> joints_;
  realtime_tools::RealtimeBuffer<BufferType> commands_buffer_;
  unsigned int n_joints_;
 
 private:
  ros::Subscriber sub_command_;
 
  std::vector<urdf::JointConstSharedPtr> joint_urdfs_;
 
  std::map<int, std::pair<int, int>> leg_map_;
 
  std::vector<double> torque_lims_;
  std::vector<double> speed_lims_;
 
  void commandCB(const quad_msgs::LegCommandArrayConstPtr& msg);
  void enforceJointLimits(double& command, unsigned int index);
};  // class
 
}  // namespace effort_controllers