ros_control控制真实电机的方法

ros_control控制真实电机的方法

声明：参考 此项目对电机驱动方法进行总结。

需要的硬件

1. 用于下载程序的单片机，此处使用arduino uno
2. 电机驱动板，此处使用常规的L298N
3. 直流电机带编码器/霍尔元件

软件部分

1. 和硬件交互的robot_hardware_interface.h，作用：声明接口类，构造器添加publisher用于发布关节角度，client用于接收返回角度。需要继承hardware_interface::RobotHW。
   代码如下：

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

class ROBOTHardwareInterface : public hardware_interface::RobotHW 
{
	public:
        ROBOTHardwareInterface(ros::NodeHandle& nh);
        ~ROBOTHardwareInterface();
        void init();
        void update(const ros::TimerEvent& e);
        void read();
        void write(ros::Duration elapsed_time);
        ros::Publisher pub;
        ros::ServiceClient client;
        rospy_tutorials::Floats joints_pub;
        three_dof_planar_manipulator::Floats_array joint_read;
        
    protected:
        hardware_interface::JointStateInterface joint_state_interface_;
        hardware_interface::PositionJointInterface position_joint_interface_;
        hardware_interface::EffortJointInterface     effort_joint_interface_;

        
        joint_limits_interface::EffortJointSaturationInterface effortJointSaturationInterface;
        
        int num_joints_;
        std::string joint_name_;  
        double joint_position_;
        double joint_velocity_;
        double joint_effort_;
        double joint_position_command_;
        double joint_effort_command_;
        double joint_velocity_command_;
        
        ros::NodeHandle nh_;
        ros::Timer non_realtime_loop_;
        ros::Duration elapsed_time_;
        double loop_hz_;
        boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;
};

2. 类的实现：robot_hardware_interface_node.cpp,在这里向ros注册控制器名称，通过服务的方式实现read方法：在cpp文件中声明客户端向服务器发起请求获得角度返回值。write方法：发布joint_effort_command_（目标值）到/joints_to_arduino话题。此处是定义功能，实际角度发布是在terminal使用rostopic pub发布的，当然也可以直接用脚本发布。注意joint_effort_command_表示目标的effor:

#include 

ROBOTHardwareInterface::ROBOTHardwareInterface(ros::NodeHandle& nh) : nh_(nh) {
    init();
    controller_manager_.reset(new controller_manager::ControllerManager(this, nh_));
    loop_hz_=5;
    ros::Duration update_freq = ros::Duration(1.0/loop_hz_);
	
	pub = nh_.advertise<rospy_tutorials::Floats>("/joints_to_aurdino",10);
	client = nh_.serviceClient<three_dof_planar_manipulator::Floats_array>("/read_joint_state");
	
    non_realtime_loop_ = nh_.createTimer(update_freq, &ROBOTHardwareInterface::update, this);
}

ROBOTHardwareInterface::~ROBOTHardwareInterface() {
}

void ROBOTHardwareInterface::init() {
    
    
	joint_name_="joint1";
    
// Create joint state interface
    hardware_interface::JointStateHandle jointStateHandle(joint_name_, &joint_position_, &joint_velocity_, &joint_effort_);
    joint_state_interface_.registerHandle(jointStateHandle);

// Create position joint interface
    //hardware_interface::JointHandle jointPositionHandle(jointStateHandle, &joint_position_command_);
    //position_joint_interface_.registerHandle(jointPositionHandle);
    
// Create velocity joint interface
	//hardware_interface::JointHandle jointVelocityHandle(jointStateHandle, &joint_velocity_command_);
    //effort_joint_interface_.registerHandle(jointVelocityHandle);
    
// Create effort joint interface
    hardware_interface::JointHandle jointEffortHandle(jointStateHandle, &joint_effort_command_);
	effort_joint_interface_.registerHandle(jointEffortHandle);
	
// Create Joint Limit interface   
    joint_limits_interface::JointLimits limits;
    joint_limits_interface::getJointLimits("joint1", nh_, limits);
	joint_limits_interface::EffortJointSaturationHandle jointLimitsHandle(jointEffortHandle, limits);
	effortJointSaturationInterface.registerHandle(jointLimitsHandle);
	
/*
If you have more joints then,
    joint_name_= "joint2"
    
// Create joint state interface
    hardware_interface::JointStateHandle jointStateHandle2(joint_name_, &joint_position_2, &joint_velocity_2, &joint_effort_2);
    joint_state_interface_.registerHandle(jointStateHandle);
//create the position/velocity/effort interface according to your actuator 
    hardware_interface::JointHandle jointPositionHandle2(jointStateHandle2, &joint_position_command_2);
    position_joint_interface_.registerHandle(jointPositionHandle2);
    
    hardware_interface::JointHandle jointVelocityHandle2(jointStateHandle2, &joint_velocity_command_2);
    effort_joint_interface_.registerHandle(jointVelocityHandle2);
    
    hardware_interface::JointHandle jointEffortHandle2(jointStateHandle2, &joint_effort_command_2);
	effort_joint_interface_.registerHandle(jointEffortHandle2);
	
//create joint limit interface.
    joint_limits_interface::getJointLimits("joint2", nh_, limits);
	joint_limits_interface::EffortJointSaturationHandle jointLimitsHandle2(jointEffortHandle2, limits);
	effortJointSaturationInterface.registerHandle(jointLimitsHandle2);
	
	Repeat same for other joints
*/
	

// Register all joints interfaces    
    registerInterface(&joint_state_interface_);
    registerInterface(&position_joint_interface_);
    registerInterface(&effort_joint_interface_);
    registerInterface(&effortJointSaturationInterface);
}

void ROBOTHardwareInterface::update(const ros::TimerEvent& e) {
    elapsed_time_ = ros::Duration(e.current_real - e.last_real);
    read();
    controller_manager_->update(ros::Time::now(), elapsed_time_);
    write(elapsed_time_);
}

void ROBOTHardwareInterface::read() {

	joint_read.request.req=1.0;
	
	if(client.call(joint_read))
	{
	    joint_position_ = angles::from_degrees(joint_read.response.res[0]);
	    joint_velocity_ = angles::from_degrees(joint_read.response.res[1]);
	    ROS_INFO("Current Pos: %.2f, Vel: %.2f",joint_position_,joint_velocity_);
/*
if more than one joint,
        get values for joint_position_2, joint_velocity_2,......
*/	    
	    
	}
	else
	{
	    joint_position_ = 0;
	    joint_velocity_ = 0;
	}
        

}

void ROBOTHardwareInterface::write(ros::Duration elapsed_time) {
   
    effortJointSaturationInterface.enforceLimits(elapsed_time);    
	joints_pub.data.clear();
	joints_pub.data.push_back(joint_effort_command_);
	
/*
if more than one joint,
    publish values for joint_effort_command_2,......
*/	
	
	ROS_INFO("PWM Cmd: %.2f",joint_effort_command_);
	pub.publish(joints_pub);
		
}



int main(int argc, char** argv)
{
    ros::init(argc, argv, "single_joint_hardware_interface");
    ros::NodeHandle nh;
    //ros::AsyncSpinner spinner(4);  
    ros::MultiThreadedSpinner spinner(2); // Multiple threads for controller service callback and for the Service client callback used to get the feedback from ardiuno
    ROBOTHardwareInterface ROBOT(nh);
    //spinner.start();
    spinner.spin();
    //ros::spin();
    return 0;
}

3. 电机的驱动代码部署在arduino上:arduino_code.ino，通过订阅/joints_to_arduino话题获取关节角度命令，执行之后将关节状态（编码器读取）在/joint_states_from_arduino话题发布。

#include 
#include 
#define encodPinA1      2                       // Quadrature encoder A pin
#define encodPinB1      8                       // Quadrature encoder B pin
#define M1              9                       // PWM outputs to motor driver module
#define M2              10

ros::NodeHandle  nh;

double pos = 0, vel= 0, output = 0, temp=0;
unsigned long lastTime,now,lasttimepub;
volatile long encoderPos = 0,last_pos=0;
rospy_tutorials::Floats joint_state;

void set_angle_cb( const rospy_tutorials::Floats& cmd_msg){
  output= cmd_msg.data[0]; 
}


ros::Subscriber<rospy_tutorials::Floats> sub("/joints_to_aurdino", set_angle_cb);
ros::Publisher pub("/joint_states_from_arduino", &joint_state);

void setup(){
  nh.initNode();
  nh.subscribe(sub);
  nh.advertise(pub);
  pinMode(encodPinA1, INPUT_PULLUP);                  // quadrature encoder input A
  pinMode(encodPinB1, INPUT_PULLUP);                  // quadrature encoder input B
  attachInterrupt(0, encoder, FALLING);               // update encoder position
  TCCR1B = TCCR1B & 0b11111000 | 1;                   // set 31KHz PWM to prevent motor noise  
}

void loop(){
  pos = (encoderPos*360)/2200 ;
  now = millis();
  int timeChange = (now - lastTime);
  if(timeChange>=500 )
  {
      temp = (360.0*1000*(encoderPos-last_pos)) /(2200.0*(now - lastTime));
      if ((encoderPos < -2 || encoderPos > 2) && temp >= -60 && temp <=60 ) // to gaurd encoderPos at boundary i.e., after max limit it will rest. Then lastPos will be greater than encoderpos
          vel =temp;
      lastTime=now;
      last_pos=encoderPos;
  }

  pwmOut(output);
  
  if ((now - lasttimepub)> 100)
  {
    joint_state.data_length=2;
    joint_state.data[0]=pos;
    joint_state.data[1]=vel;
    pub.publish(&joint_state);
    lasttimepub=now;
  }

  nh.spinOnce();

}

void encoder()  {                                     // pulse and direction, direct port reading to save cycles

  if (encoderPos > 2250 || encoderPos < -2250)
    encoderPos=0; 
  if (PINB & 0b00000001)    encoderPos++;             // if(digitalRead(encodPinB1)==HIGH)   count ++;
  else                      encoderPos--;             // if(digitalRead(encodPinB1)==LOW)   count --;
}

void pwmOut(float out) {                                
  if (out > 0) {
    analogWrite(M2, out);                             // drive motor CW
    analogWrite(M1, 0);
  }
  else {
    analogWrite(M2, 0);
    analogWrite(M1, abs(out));                        // drive motor CCW
  }
}

4. python节点订阅/joint_states_from_arduino话题获取当前电机状态，启动/read_joint_state服务端，返回值就是电机状态包括pos和vel即角度和角速度。代码实现：

#!/usr/bin/env python
import rospy
from rospy_tutorials.msg import Floats
from three_dof_planar_manipulator.srv import Floats_array, Floats_arrayResponse, Floats_arrayRequest
pos=0
vel=0

def my_callback(msg):
    global pos, vel
    pos=msg.data[0]
    vel=msg.data[1]
    #print "Pos: ", pos, "vel: ", vel

def my_server(req):
    global pos, vel
    res = Floats_arrayResponse() 
    res.res=[pos, vel]
    return res

rospy.init_node('subscriber_py') #initialzing the node with name "subscriber_py"

rospy.Subscriber("/joint_states_from_arduino", Floats, my_callback, queue_size=10) 

rospy.Service('/read_joint_state', Floats_array, my_server)

rospy.spin() 

5. 控制器配置文件controllers.yaml:定义控制器类型和pid参数。注意type使用effort_controllers/JointPositionController输出的接口为effort，输入为position。

single_joint_actuator:
    # Publish all joint states -----------------------------------
    joints_update:
      type: joint_state_controller/JointStateController
      publish_rate: 50
    
    # Position Controllers ---------------------------------------
    joint1_position_controller:
      type: effort_controllers/JointPositionController
      joint: joint1
      pid: {p: 1300.0, i: 0.0, d: 5, i_clamp_min: -130.0, i_clamp_max: 130, antiwindup: True}
      
    # Velocity Controllers ---------------------------------------
    joint1_velocity_controller:
      type: effort_controllers/JointVelocityController
      joint: joint1
      pid: {p: 100.0, i: 100.0, d: 0.0, i_clamp_min: -255.0, i_clamp_max: 255, antiwindup: True}

6. 通过launch文件加载控制器,spawner的参数args和yaml文件要对应。

<launch>
    
  <rosparam file="$(find ros_control_example)/config/controllers.yaml" command="load"/>
  <rosparam file="$(find ros_control_example)/config/joint_limits.yaml" command="load"/>
   
  <arg name="model" default="$(find ros_control_example)/urdf/single_joint_actuator.urdf.xacro"/>
  <arg name="gui" default="true" />
  
  <param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />
  <param name="use_gui" value="$(arg gui)"/>
  
  <node name="robot_hardware_interface" pkg="ros_control_example" type="single_joint_hardware_interface" output="screen"/>
  
  <node name="subscriber_py" pkg="ros_control_example" type="joints_receive_from_arduino.py" output="screen"/> 

  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" >
  node>
  
  <node name="rviz" pkg="rviz" type="rviz"/>
    
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"
        args="
        	/single_joint_actuator/joints_update
            /single_joint_actuator/joint1_position_controller
        "/>
launch>

7. 启动launch文件，在terminal中发布/single_joint_actuator/joint1_position_controller/command 角度值可以看到电机转动rviz中电机模型也更新状态。
8. 可以使用rqt_gui 连续发布角度并观察/single_joint_actuator/joint1_position_controller/state/set_point和/single_joint_actuator/joint1_position_controller/state/process_value变化，使用动态pid调节调整pid参数。

以上是学习ros的一些心得，及时记录下来。偶尔回头看看是个好习惯。