十、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后，上载程序，极大可能报错

[Errno 13] Permission denied: '/dev/ttyACM0'

说明没有权限，更改权限就可以了

$ sudo chmod 777 /dev/ttyACM0 

程序顺利上传，不过这种方法只是临时处理，在下次启动时需要重新输入。

若正确接线，效果如下，

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后，打开一个终端运行命令，

$ roscore

接下来，运行rosserial客户端应用程序，它将Arduino消息转发给其他ROS。确保使用正确的串口:

rosrun rosserial_python serial_node.py /dev/ttyACM0

用rostopic切换LED:

rostopic pub toggle_led std_msgs/Empty --once

若运行正常，则每输入一次命令，灯会进行一次操作。

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

$ source ~/catkin_ws/devel/setup.bash
$ roslaunch arm1 gazebo.launch

新建终端,

./setup.sh

就可以像之前一样运行程序了，打开rqt_graph 查看节点图

效果如下，

十一、小结

不知为何，最近虚拟机非常卡顿，以至于无法完成正常的图像处理，不过所有预想的功能都已经实现了，所以这个arm0项目就算是完成了吧。毕竟初衷就是一个练手项目，有很多优化没有时间去做。而且虚拟机还因为win10强制更新而导致的强制关闭出错了，估计也需要重新安装一遍系统。

接下来就是SLAM了，等后面学习完SLAM将其结合起来，这又是下一个项目了。