【从零开始的ROS四轴机械臂控制】(七)- ROS与arduino连接

163
0
2020年12月28日 09时06分

从零开始的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

 

1.虚拟机与arduino的连接

 

(1)arduino连接与IDE

 

首先将Arduino的连接到虚拟机,只要在Vmware打开的情况下,它都会询问是否连接到虚拟机,或者直接在Player菜单–>可移动设备中连接也可。

 

然后在虚拟机中安装arduino IDE,在Linux安装的具体方法可以百度或者Google上,在此不做详细说明。安装之后可以打开如下。

 

2

 

(2)PCA9685模块支持与测试

 

3

 

以上为原理图,连接好线后我们要下载关于PCA9685模块的支持库进行测试。

在arudino IDE中打开库管理,搜索Adafruit pwm Servo Driver Library,然后进行安装

 

4

 

在该库的示例下打开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 

 

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

若正确接线,效果如下,

 

5

 

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库

 

6

 

可以打开示例如下,

 

7

 

(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 查看节点图

 

8

 

9

 

效果如下,

 

11

 

12

 

十一、小结

 

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

 

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

发表评论

后才能评论