从零开始的ROS四轴机械臂控制（七）
-
- 十、ROS与arduino连接
-
- 1.虚拟机与arduino的连接
-
- （1）arduino连接与IDE
- （2）PCA9685模块支持与测试
- 2.ROS与arduino连接测试
-
- （1）Arduino安装rosserial
- （2）rosserial测试
- 3.ROS与arduino连接
-
- （1）ros2ard节点
- （2）arm_command节点
- （3）arduino订阅
- （4）批处理
- （5）程序运行
- 十一、小结
十、ROS与arduino连接
回到许久没有动的硬件上来，考虑到我使用的是虚拟机，所以首先要测试的是虚拟机与arduino的连接，然后才能考虑ROS与arduino的连接。 写这部分的时候不知道为什么markdown编辑器“炸”掉了，编辑好的文本成了一堆乱码，关键这堆乱码还被保存了。由于之前没有出现过这种情况，我习惯性的用编辑器做记录，现在基本上都没了。于是就导致了一个问题，我可能在重新编辑的时候忘记某些步骤，还是希望能注意一下。 本部分完成效果如下
1.虚拟机与arduino的连接
（1）arduino连接与IDE
首先将Arduino的连接到虚拟机，只要在Vmware打开的情况下，它都会询问是否连接到虚拟机，或者直接在Player菜单–>可移动设备中连接也可。 然后在虚拟机中安装arduino IDE，在Linux安装的具体方法可以百度或者Google上，在此不做详细说明。安装之后可以打开如下。
（2）PCA9685模块支持与测试
以上为原理图，连接好线后我们要下载关于PCA9685模块的支持库进行测试。 在arudino IDE中打开库管理，搜索Adafruit pwm Servo Driver Library,然后进行安装
在该库的示例下打开servo示例如下，
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
// you can also call it with a different address you want
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x41);
// you can also call it with a different address and I2C interface
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40, Wire);
// Depending on your servo make, the pulse width min and max may vary, you
// want these to be as small/large as possible without hitting the hard stop
// for max range. You'll have to tweak them as necessary to match the servos you
// have!
#define SERVOMIN 150 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 600 // This is the 'maximum' pulse length count (out of 4096)
#define USMIN 600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150
#define USMAX 2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates
// our servo # counter
uint8_t servonum = 0;
void setup() {
Serial.begin(9600);
Serial.println("8 channel Servo test!");
pwm.begin();
// In theory the internal oscillator is 25MHz but it really isn't
// that precise. You can 'calibrate' by tweaking this number till
// you get the frequency you're expecting!
pwm.setOscillatorFrequency(27000000); // The int.osc. is closer to 27MHz
pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates
delay(10);
}
// You can use this function if you'd like to set the pulse length in seconds
// e.g. setServoPulse(0, 0.001) is a ~1 millisecond pulse width. It's not precise!
void setServoPulse(uint8_t n, double pulse) {
double pulselength;
pulselength = 1000000; // 1,000,000 us per second
pulselength /= SERVO_FREQ; // Analog servos run at ~60 Hz updates
Serial.print(pulselength); Serial.println(" us per period");
pulselength /= 4096; // 12 bits of resolution
Serial.print(pulselength); Serial.println(" us per bit");
pulse *= 1000000; // convert input seconds to us
pulse /= pulselength;
Serial.println(pulse);
pwm.setPWM(n, 0, pulse);
}
void loop() {
// Drive each servo one at a time using setPWM()
Serial.println(servonum);
for (uint16_t pulselen = SERVOMIN; pulselen < SERVOMAX; pulselen++) {
pwm.setPWM(servonum, 0, pulselen);
}
delay(500);
for (uint16_t pulselen = SERVOMAX; pulselen > SERVOMIN; pulselen--) {
pwm.setPWM(servonum, 0, pulselen);
}
delay(500);
// Drive each servo one at a time using writeMicroseconds(), it's not precise due to calculation rounding!
// The writeMicroseconds() function is used to mimic the Arduino Servo library writeMicroseconds() behavior.
for (uint16_t microsec = USMIN; microsec < USMAX; microsec++) {
pwm.writeMicroseconds(servonum, microsec);
}
delay(500);
for (uint16_t microsec = USMAX; microsec > USMIN; microsec--) {
pwm.writeMicroseconds(servonum, microsec);
}
delay(500);
servonum++;
if (servonum > 7) servonum = 0; // Testing the first 8 servo channels
}
一个比较好的建议是，把以下部分的范围改小，否则可能会损坏模型
#define SERVOMIN 150 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 600 // This is the 'maximum' pulse length count (out of 4096)
#define USMIN 600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150
#define USMAX 2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600
使用IDE连接好arduino UNO后，上载程序，极大可能报错
<code class="has-numbering">[Errno 13] Permission denied: '/dev/ttyACM0' </code>
说明没有权限，更改权限就可以了
<code class="has-numbering">$ sudo chmod 777 /dev/ttyACM0 </code>
程序顺利上传，不过这种方法只是临时处理，在下次启动时需要重新输入。 若正确接线，效果如下，
2.ROS与arduino连接测试
（1）Arduino安装rosserial
运行命令安装rosserial_arduino包，
$ sudo apt-get install ros-indigo-rosserial-arduino
$ sudo apt-get install ros-indigo-rosserial
在库管理中添加Rosserial Arduino库
可以打开示例如下，
（2）rosserial测试
使用blink程序进行测试，打开示例程序，
#include <ros.h>
#include <std_msgs/Empty.h>
ros::NodeHandle nh;
void messageCb( const std_msgs::Empty& toggle_msg){
digitalWrite(13, HIGH-digitalRead(13)); // blink the led
}
ros::Subscriber<std_msgs::Empty> sub("toggle_led", &messageCb );
void setup()
{
pinMode(13, OUTPUT);
nh.initNode();
nh.subscribe(sub);
}
void loop()
{
nh.spinOnce();
delay(1);
}
将程序上载到arduino后，打开一个终端运行命令，
<code class="has-numbering">$ roscore </code>
接下来，运行rosserial客户端应用程序，它将Arduino消息转发给其他ROS。确保使用正确的串口:
<code class="has-numbering">rosrun rosserial_python serial_node.py /dev/ttyACM0 </code>
用rostopic切换LED:
<code class="has-numbering">rostopic pub toggle_led std_msgs/Empty --once </code>
若运行正常，则每输入一次命令，灯会进行一次操作。
3.ROS与arduino连接
在完成了所有准备工作之后我们就可以进行下一步了
（1）ros2ard节点
因为舵机的控制信息和我们计算使用的角度信息不相同，所以我们就需要一个转换节点，接收节点信息，并进行转换，发布给arduino
#!/usr/bin/env python
import math
import rospy
from std_msgs.msg import UInt16MultiArray, Float64MultiArray
from sensor_msgs.msg import JointState
from arm1.srv import *
class ros2ard(object):
def __init__(self):
node_name = 'ros2ard'
rospy.init_node(node_name)
# init Publisher
self.pub1 = rospy.Publisher('/ros2ard/joint_msg', UInt16MultiArray, queue_size=10)
# init Subscriber
rospy.Subscriber('/arm_command/joint_val', Float64MultiArray, self.callback)
# 注意这里的各种参数是经过测试计算之后得出的，不具有普适性
def cal(self, angle):
joint_value = int(1.89*91.80/1.57*angle)
return joint_value
def callback(self, joint_msg):
print(joint_msg.data)
# 注意这里的各种参数是经过测试之后得出的，+600是因为我们使用的是UInt,为了确保角度不为负数
joint0_msg = self.cal(joint_msg.data[0])
joint1_msg = 390 - self.cal(joint_msg.data[1])+ 600
joint2_msg = 210 - self.cal(joint_msg.data[2])+ 600
joint3_msg = 230 - self.cal(joint_msg.data[3])+ 600
joint4_msg = 300+ 600
joint_value = [joint0_msg, joint1_msg, joint2_msg, joint3_msg, joint4_msg]
joint_message = UInt16MultiArray(data=joint_value)
print('joint_msg', joint_message.data)
self.pub1.publish(joint_message)
if __name__ == '__main__':
try:
ros2ard()
rospy.spin()
except rospy.ROSInterruptException:
pass
（2）arm_command节点
为了给ros2ard节点发布角度信息，所以我们需要修改一下arm_command，使其发布节点信息
#!/usr/bin/env python
import math
import rospy
from std_msgs.msg import Float64, Bool, Float64MultiArray
from sensor_msgs.msg import JointState
from arm1.srv import *
class ArmCommand(object):
def __init__(self):
...
self.pub3 = rospy.Publisher('/arm_command/joint_val', Float64MultiArray, queue_size=10)
...
# set all positions of joints with this function
def arm_pos_set(self, joint1=0.0, joint2=0.52,
joint3=-1.57,
joint4=0.98,
right_joint=1.0,
left_joint=1.0,
pos_init=False):
self.msg.joint1 = joint1
self.msg.joint2 = self.value_translation(joint2, 'joint2', 'cal2gaz')
self.msg.joint3 = self.value_translation(joint3, 'joint3', 'cal2gaz')
self.msg.joint4 = self.value_translation(joint4, 'joint4', 'cal2gaz')
self.msg.right_joint = right_joint
self.msg.left_joint = left_joint
response = self.arm_mover(self.msg)
joint_value = [joint1, joint2, joint3, joint4, right_joint]
joint_msg = Float64MultiArray(data=joint_value)
self.pub3.publish(joint_msg)
if pos_init:
rospy.loginfo('init all joints position')
...
...
（3）arduino订阅
我们需要使arduino订阅节点信息，并控制舵机移动，程序编写如下，
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include <WProgram.h>
#endif
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <ros.h>
#include <std_msgs/UInt16MultiArray.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
#define SERVO_FREQ 50
ros::NodeHandle nh;
void servo_set(const std_msgs::UInt16MultiArray& cmd_msg) {
pwm.setPWM(0, 0, cmd_msg.data[0]-600);
pwm.setPWM(1, 0, cmd_msg.data[1]-600);
pwm.setPWM(2, 0, cmd_msg.data[2]-600);
pwm.setPWM(3, 0, cmd_msg.data[3]-600);
pwm.setPWM(4, 0, cmd_msg.data[4]-600);
digitalWrite(13, HIGH - digitalRead(13));
}
ros::Subscriber<std_msgs::UInt16MultiArray> sub("/ros2ard/joint_msg", servo_set);
void setup() {
pinMode(13, OUTPUT);
pwm.begin();
pwm.setOscillatorFrequency(27000000);
pwm.setPWMFreq(SERVO_FREQ);
pwm.setPWM(0, 0, 400);
pwm.setPWM(1, 0, 270);
pwm.setPWM(2, 0, 450);
pwm.setPWM(3, 0, 130);
pwm.setPWM(4, 0, 300);
digitalWrite(13, HIGH - digitalRead(13));
nh.initNode();
nh.subscribe(sub);
}
void loop() {
nh.spinOnce();
delay(1000);
}
（4）批处理
由于需要启动的东西有点多了，所以一个比较好的方法使利用批处理命令，建立setup.sh文件，
#!/bin/bash
source ~/catkin_ws/devel/setup.bash
{
gnome-terminal -t "img_process" -x bash -c "rosrun arm1 image_process;exec bash"
}&
sleep 5s
{
gnome-terminal -t "arm_command" -x bash -c "rosrun arm1 arm_command;exec bash"
}&
sleep 5s
{
gnome-terminal -t "ros2ard" -x bash -c "rosrun arm1 ros2ard;exec bash"
}&
sleep 5s
{
gnome-terminal -t "ros2ard" -x bash -c "rosrun rosserial_python serial_node.py /dev/ttyACM0;exec bash"
}&
这样我们比较方便的运行命令了
（5）程序运行
运行命令打开gazebo
<code class="has-numbering">$ source ~/catkin_ws/devel/setup.bash $ roslaunch arm1 gazebo.launch</code>
<code class="has-numbering"></code>
新建终端,
<code class="has-numbering">./setup.sh </code>
就可以像之前一样运行程序了，打开rqt_graph 查看节点图
效果如下，
十一、小结
不知为何，最近虚拟机非常卡顿，以至于无法完成正常的图像处理，不过所有预想的功能都已经实现了，所以这个arm0项目就算是完成了吧。毕竟初衷就是一个练手项目，有很多优化没有时间去做。而且虚拟机还因为win10强制更新而导致的强制关闭出错了，估计也需要重新安装一遍系统。 接下来就是SLAM了，等后面学习完SLAM将其结合起来，这又是下一个项目了。
开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明【从零开始的ROS四轴机械臂控制】（七）- ROS与arduino连接！
