制作ROS小车系列（二）——开始ROS上位机导航前的准备
一、系统准备
1、上位机系统的安装
本项目采用的系统配置为ubuntu 16.04+ROS Kinetic，采用的是工控机（大家也可以使用树莓派，这部分的安装参考博客：树莓派安装教程）
ubuntu16.04的安装比较简单这里不再赘述，系统镜像自行下载：云盘资料提取码：5vyc
安装好了ubuntu后大家就可以安装ros系统了。具体步骤如下：
1)、配置密钥
sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
2)、更新
sudo apt-get update && sudo apt-get upgrade
3)、安装
sudo apt-get install ros-kinetic-desktop-full
4)、初始化rosdep（这一步大部分人可能会出现问题，解决办法参考我这篇博客的内容：看rosdep初始化那个位置）
sudo rosdep init
然后再执行
sudo rosdep init
接下来输入
rosdep update
5)、安装依赖包
sudo apt-get install python-rosinstall python-rosinstall-generator python-wstool build-essential
6)、测试ROS是否搭建成功
先运行
roscore
再开新终端
rosrun turtlesim turtlesim_node
如果结果正常，此时会出现一个图形界面，里面有只乌龟
ROS系统安装完成！！！
二、上位机和下位机通讯方式选择
本项目所用的下位机为arduino所有其与上位机ros之间的通讯方式有大致两种一种是rosserial_arduino和ros_arduino_bridge，两种通讯方式各有优缺点。本项目选择用rosserial_arduino作为通讯方式（此方式相当于把arduino下位机当成一个ros节点，接收发发布话题进行数据的传输）
1、在ubuntu中安装arduino 18.0
这部分安装过程可以参考这篇文章:arduino 1.8安装教程
2、rosserial_arduino安装
这部分参考：rosserial_arduino安装教程
3、下位机的程序
这部分程序和之前说的大部分是一样的，只是这部分加了<ros.h>头文件将以前的arduino程序变成了ros的一个节点，具备一个订阅cmd_vel和发布里程计odom话题的能力。具体代码如下：
#if (ARDUINO >= 100)
#include <Arduino.h>
#else
#include <WProgram.h>
#endif
#include <ros.h>
#include <std_msgs/Int16.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Float32.h>
#define DIR1 4 //motor1方向控制引脚、pwm速度控制引脚定义
#define pwm1 5
#define DIR2 6 //motor2方向控制引脚、pwm速度控制引脚定义
#define pwm2 7
#define DIR3 8 //motor3方向控制引脚、pwm速度控制引脚定义
#define pwm3 9
#define DIR4 10 //motor4方向控制引脚、pwm速度控制引脚定义
#define pwm4 11
#define motorcount1A 19 //motor1编码器A、B引脚定义
#define motorcount1B 15
#define motorcount2A 18 //motor2编码器A、B引脚定义
#define motorcount2B 14
#define motorcount3A 21 //motor3编码器A、B引脚定义
#define motorcount3B 17
#define motorcount4A 20 //motor4编码器A、B引脚定义
#define motorcount4B 16
int max_v=50;
float a=0.36/2,b=0.265/2;
float car_vx=0; //linear x
float car_vy=0;
float car_yaw=0; //anglear z
float pub_car_vx=0; //linear x
float pub_car_vy=0;
float pub_car_yaw=0; //anglear z
float PotBuffer1=0; //各轮的速度
float PotBuffer2=0;
float PotBuffer3=0;
float PotBuffer4=0;
int PotBuffer=0; //车子的速度矢量
volatile float motor1=0; //中断变量，1号轮子脉冲计数
float v_motor_1=0;
volatile float motor2=0; //中断变量，2号轮子脉冲计数
float v_motor_2=0;
volatile float motor3=0; //中断变量，3号轮子脉冲计数
float v_motor_3=0;
volatile float motor4=0; //中断变量，4号轮子脉冲计数
float v_motor_4=0;
float kp=1.6,ki=0.8,kd=0;
int dir1=0,dir2=0,dir3=0,dir4=0;
int P1=0,P2=0,P3=0,P4=0;
ros::NodeHandle nh; //节点句柄
void generate_motor_v()
{
PotBuffer1=car_vx-car_vy+car_yaw*(a+b);
PotBuffer2=car_vx+car_vy-car_yaw*(a+b);
PotBuffer3=car_vx-car_vy-car_yaw*(a+b);
PotBuffer4=car_vx+car_vy+car_yaw*(a+b);
P1=Incremental_Pi(v_motor_1,PotBuffer1);
// P2=Incremental_Pi(v_motor_2,PotBuffer2);
// P3=Incremental_Pi(v_motor_3,PotBuffer3);
// P4=Incremental_Pi(v_motor_4,PotBuffer4);
if(PotBuffer1>=max_v)PotBuffer1=max_v;
else if(PotBuffer1<-max_v)PotBuffer1=-max_v;
if(PotBuffer2>=max_v)PotBuffer2=max_v;
else if(PotBuffer2<-max_v)PotBuffer2=-max_v;
if(PotBuffer3>=max_v)PotBuffer3=max_v;
else if(PotBuffer3<-max_v)PotBuffer3=-max_v;
if(PotBuffer4>=max_v)PotBuffer4=max_v;
else if(PotBuffer4<-max_v)PotBuffer4=-max_v;
if(PotBuffer1>=0){digitalWrite(DIR1,LOW);dir1=0;}
else if(PotBuffer1<0){digitalWrite(DIR1,HIGH);dir1=1;}
if(PotBuffer2>=0){digitalWrite(DIR2,HIGH);dir2=0;}
else if(PotBuffer2<0){digitalWrite(DIR2,LOW);dir2=1;}
if(PotBuffer3>=0){digitalWrite(DIR3,HIGH);dir3=0;}
else if(PotBuffer3<0){digitalWrite(DIR3,LOW);dir3=1;}
if(PotBuffer4>=0){digitalWrite(DIR4,LOW);dir4=0;}
else if(PotBuffer4<0){digitalWrite(DIR4,HIGH);dir4=1;}
}
void motor_control(const geometry_msgs::Twist& cmd_vel) //订阅话题时触发打中断函数
{
car_vx = cmd_vel.linear.x*100; // Cm/s
car_yaw=cmd_vel.angular.z*100;
car_vy=cmd_vel.linear.y;
read_motor_v();
generate_motor_v();
}
//ros::Subscriber<std_msgs::Int16> sub("motor_vel", motor_cmd); //订阅“motor_vel”话题的数据（速度话题）
ros::Subscriber<geometry_msgs::Twist> sub1("/cmd_vel", motor_control); //订阅cmd_vel话题的数据（方向话题）
geometry_msgs::Twist vel;
ros::Publisher chatter("vel_motor",&vel); //发布车的速度话题
void read_motor_v()
{
unsigned long nowtime=0; //机时时间定义
motor1=0; //脉冲初始化
motor2=0;
motor3=0;
motor4=0;
nowtime=millis()+50;
while(millis()<nowtime){
attachInterrupt(digitalPinToInterrupt(motorcount1A),read_motor_1,RISING); //子50ms内进行脉冲计数（调用中断函数）
attachInterrupt(digitalPinToInterrupt(motorcount2A),read_motor_2,RISING);
attachInterrupt(digitalPinToInterrupt(motorcount3A),read_motor_3,RISING);
attachInterrupt(digitalPinToInterrupt(motorcount4A),read_motor_4,RISING);
}
detachInterrupt(digitalPinToInterrupt(motorcount1A)); //关闭中断计数
detachInterrupt(digitalPinToInterrupt(motorcount2A));
detachInterrupt(digitalPinToInterrupt(motorcount3A));
detachInterrupt(digitalPinToInterrupt(motorcount4A));
if(dir1==0)v_motor_1=((motor1/350)*15*PI)/0.05; //根据脉冲数、时间、轮子打半径计算速度
else if(dir1==1)v_motor_1=-((motor1/350)*15*PI)/0.05;
if(dir2==0)v_motor_2=((motor2/350)*15*PI)/0.05;
else if(dir2==1)v_motor_2=-((motor2/350)*15*PI)/0.05;
if(dir3==0)v_motor_3=((motor3/350)*15*PI)/0.05;
else if(dir3==1)v_motor_3=-((motor3/350)*15*PI)/0.05;
if(dir4==0)v_motor_4=((motor4/350)*15*PI)/0.05; // CM/S
else if(dir4==1)v_motor_4=-((motor4/350)*15*PI)/0.05;
pub_car_vx=(v_motor_4+v_motor_1+v_motor_2+v_motor_3)/4;
pub_car_vy=(-v_motor_1+v_motor_2-v_motor_3+v_motor_4)/4;
pub_car_yaw=(v_motor_1-v_motor_2-v_motor_3+v_motor_4)/(4*(a+b)*100); //计算车子的速度
}
void read_motor_1(){ //motor脉冲计数中断函数
motor1++;
}
void read_motor_2(){
motor2++;
}
void read_motor_3(){
motor3++;
}
void read_motor_4(){
motor4++;
}
int Incremental_Pi(int current_speed,int target_speed){ //PID
static float pwm,bias,last_bias,prev_bias; //静态变量存在程序全周期：pwm:增量输出,bias:本次偏差,last_bias:上次偏差,prev_bais_:上上次偏差
bias=current_speed-target_speed; //计算本次偏差e(k)
pwm-=(kp*(bias-last_bias)+ki*bias); //增量式PID控制器
prev_bias=last_bias; //保存上上次偏差
last_bias=bias; //保存上一次偏差
// if(pwm<-max_v){
// pwm=-max_v;
// }
// if(pwm>=max_v){
// pwm=max_v;
// }
return pwm; //增量输出
}
void setup(){
pinMode(13, OUTPUT);
pinMode(motorcount1A,INPUT); //编码器引脚设置
pinMode(motorcount1B,INPUT);
pinMode(motorcount2A,INPUT);
pinMode(motorcount2B,INPUT);
pinMode(motorcount3A,INPUT);
pinMode(motorcount3B,INPUT);
pinMode(motorcount4A,INPUT);
pinMode(motorcount4B,INPUT);
nh.initNode(); //节点句柄
nh.advertise(chatter); //发布消息的准备
//nh.subscribe(sub); //订阅消息
nh.subscribe(sub1);
pinMode(12,OUTPUT);
pinMode(pwm1,OUTPUT); //motor方向和速度引脚设置
pinMode(DIR1,OUTPUT);
pinMode(pwm2,OUTPUT);
pinMode(DIR2,OUTPUT);
pinMode(pwm3,OUTPUT);
pinMode(DIR3,OUTPUT);
pinMode(pwm4,OUTPUT);
pinMode(DIR4,OUTPUT);
digitalWrite(12,HIGH);
}
void loop(){
read_motor_v();
vel.linear.x=pub_car_vx; //获取发布打数据
vel.linear.y=pub_car_vy;
vel.linear.z=0.0;
vel.angular.z=pub_car_yaw;
vel.angular.x=0.0;
vel.angular.y=0.0;
chatter.publish(&vel); //发布消息
nh.spinOnce();
analogWrite(pwm1,abs(PotBuffer1)); //发送脉冲给电机
analogWrite(pwm2,abs(PotBuffer2));
analogWrite(pwm3,abs(PotBuffer3));
analogWrite(pwm4,abs(PotBuffer4));
delay(1);
}
代码上传以后，需要对串口进行授权
sudo chmod 777 /dev/ttyACM0
末尾的端口号大家根据自己的串口情况来修改
然后运行
roscore
再开一个新的终端运行
rosrun rosserial_paython serial_node.py /dev/ttyACM0
如果出现以下结果说明连接成功！！！
三、到达的效果演示
这部分完成后大家就可以通过发布cmd_vel话题来控制自己的小车了。
大家可以运行
rosrun turtlesim turtle_teleop_key
使用键盘来发布cmd_vel速度话题控制小车。
这部分结束，下一章我们将开始我们的ros导航之旅！！！
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
