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 interfacehardware_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 interfacehardware_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 interfacehardware_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 ardiunoROBOTHardwareInterface 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              10ros::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 ApinMode(encodPinB1, INPUT_PULLUP);                  // quadrature encoder input BattachInterrupt(0, encoder, FALLING);               // update encoder positionTCCR1B = 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 encoderposvel =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 cyclesif (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 CWanalogWrite(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=0def my_callback(msg):global pos, velpos=msg.data[0]vel=msg.data[1]#print "Pos: ", pos, "vel: ", veldef my_server(req):global pos, velres = Floats_arrayResponse() res.res=[pos, vel]return resrospy.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/JointStateControllerpublish_rate: 50# Position Controllers ---------------------------------------joint1_position_controller:type: effort_controllers/JointPositionControllerjoint: joint1pid: {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/JointVelocityControllerjoint: joint1pid: {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的一些心得，及时记录下来。偶尔回头看看是个好习惯。

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！