ros control

ros hardware_interface

参考链接：https://github.com/RethinkRobotics/ros_control/wiki/hardware_interface

利用官方写好的硬件接口：

　　假设要控制两个joint，假定控制的是joint的位置（位置环），这时我们需要提供两个借口：一个是输出接口（JointPositionInterface），用来控制joint的位置（position）；另一个是控制的输入接口（JointStateInterface）即joint的状态（state）输入（状态反馈）。

代码实现：

　　

#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>

class MyRobot : public hardware_interface::RobotHW
{
public:
  MyRobot() 
 { 
   // connect and register the joint state interface
   hardware_interface::JointStateHandle state_handle_a("A", &pos[0], &vel[0], &eff[0]);
   jnt_state_interface.registerHandle(state_handle_a);

   hardware_interface::JointStateHandle state_handle_b("B", &pos[1], &vel[1], &eff[1]);
   jnt_state_interface.registerHandle(state_handle_b);

   registerInterface(&jnt_state_interface);

   // connect and register the joint position interface
   hardware_interface::JointHandle pos_handle_a(jnt_state_interface.getHandle("A"), &cmd[0]);
   jnt_pos_interface.registerHandle(pos_handle_a);

   hardware_interface::JointHandle pos_handle_b(jnt_state_interface.getHandle("B"), &cmd[1]);
   jnt_pos_interface.registerHandle(pos_handle_b);

   registerInterface(&jnt_pos_interface);
  }

private:
  hardware_interface::JointStateInterface jnt_state_interface;
  hardware_interface::PositionJointInterface jnt_pos_interface;
  double cmd[2];
  double pos[2];
  double vel[2];
  double eff[2];
};

运行的原理：

控制器（controller）或者控制器管理器（controller manger）可以通过上面的代码可以读取控制对象joint的状态，也可以发送控制指令给控制对象。控制器（controller）在运行时候可以通过读取pos,vel,eff三个变量获得joint的状态，控制指令cmd变量里面。设计者需要重写read()、write()两个函数。read更新pos、vel、eff三个变量，write需要将cmd传给底层控制。

　　主函数的实现DEMO：

main()
{
  MyRobot robot;
  controller_manager::ControllerManager cm(&robot);

  while (true)
  {
     robot.read();
     cm.update(robot.get_time(), robot.get_period());
     robot.write();
     sleep();
  }
}

 Controllers

　　joint_state_controller  既 publish joint state