ROS教程

这是小弟的学习笔记，有错求请拍，多指教，谢谢

二 树莓派3+ROS-kinetic+mbed/开环二轮差分模型

二轮差分模型介绍

1.二轮差分模型

二轮差分运动模型是目前最为简单方便的机器人运动底盘设计，依靠两个动力轮的电机输出不同达到直线行走和左右转动的目的，左右电机的转向不同即可完成转动，常见于寻迹小车，扫地机器人，送餐机器人等，底盘较为灵活，编程，机械设计都很简单，但缺点是转弯时不能走出顺滑的弧线

2.H桥电机驱动

H桥通常是由4个三极管做成的直流电机控制电路，电路原理图十分像字母H。4根三极管分为左上，左下，右上，右下，对角的一对三极管控制电机的一个运动方向，一侧导通后电机顺时针转，另一侧导通则逆时针转

H-bridge/wiki
H-bridge/baidu

rosserial_mbed介绍

通常来说，ROS机器人操作系统是在树莓派，miniPC，或者是笔记本电脑等运行以linux为基础的系统的设备上运行的，这类型的设备通常没有GPIO或PWM输出引脚，或者IO功能很弱，比如树莓派，它有GPIO引脚，但需要用python语言去控制或者引入wiringC++库，引脚数目也很少。所以在设计这个移动底盘的时候选择了mbed单片机和树莓派3的组合，mbed单片机负责底层的电机驱动，传感器数据读取任务，树莓派上则运行ROS系统，负责传感器数据的处理及发送控制指令。这就涉及了一个问题，ROS和mbed如何通信呢？
答案是，使用rosserial_mbed库，这个新的接口是在jade和kinetic这两个15年之后才推出的新版本上发布的，旧版本的indigo是没有的。rosserial_mbed库强大的功能在于，我们可以把单片机当作一个节点来看，在上边编写发布者和订阅者，与ROS通过USB串口来通信，不需要使用单片机Tx/Rx引脚，而且USB驱动也无需更改，直接使用mbed的程序烧录数据线，不需要用USB转TTL线
rosserial_mbed/wiki

还有与arduino的库
rosserial_arduino/wiki

连接mbed与ROS

1.mbed上的发布者和订阅者

1）mbed的发布者代码

#include <mbed.h>//mbed程序必须的头文件
#include <ros.h>
#include <std_msgs/String.h>//所涉及的消息类型的头文件

ros::NodeHandle  nh;//实例化ROS，给ROS分配一个句柄

std_msgs::String str_msg;
ros::Publisher chatter("chatter", &str_msg);

char hello[13] = "hello world!";

DigitalOut led = LED1;

int main() {
    nh.initNode();
    nh.advertise(chatter);

    while (1) {
        led = !led;
        str_msg.data = hello;
        chatter.publish( &str_msg );
        nh.spinOnce();
        wait_ms(1000);
    }
}

从这段ROSwiki给出的例程可以看出，在mbed上用到的语法与在电脑编写发布者节点的语法是一致的，整体框架也一样，所以在mbed上写ROS代码的时候，当作是往ROS框架内添加mbed的代码

2）mbed的订阅者代码

#include <mbed.h>
#include <ros.h>
#include <std_msgs/Empty.h>

ros::NodeHandle nh;
DigitalOut myled(LED1);

void messageCb(const std_msgs::Empty& toggle_msg){
    myled = !myled;   // blink the led
}

ros::Subscriber<std_msgs::Empty> sub("toggle_led", &messageCb);

int main() {
    nh.initNode();
    nh.subscribe(sub);

    while (1) {
        nh.spinOnce();
        wait_ms(1);
    }
}

订阅者的语法规则以及函数格式也和在电脑上编写节点是一致的

rosserial-mbed/Tutorials

2.DC-motor库

1）mbed上有很多函数库，便于快速开发，motordriver函数库是一个直流电机驱动函数库，包括了speed函数，coast函数，stop函数等

2）motor库的功能是使用单片机的PWM输出来控制电机，PWM是一种脉宽调制，高电平的占空比也大，电机速度越快

3）例程解析

// Sweep the motor speed from full-speed reverse (-1.0) to full speed forwards (1.0)

#include "mbed.h"
#include "Motor.h"

Motor m(p23, p6, p5); 
//Motor是电机引脚定义函数
//Motor 变量名(pwm输出引脚，IO—fwd，IO—rev，break_val)
//IO—fwd是当电机向一个方向转动时的指示灯，IO—rev则相反
//break_val设定motor能否刹车，1代表可以，0代表不可以

int main() {
    for (float s= -1.0; s < 1.0 ; s += 0.01) {
       m.speed(s); //speed()函数用来调节PWM的占空比输出，从而调节电机转速
       wait(0.02);
    }
}

4）coast()函数是保持电机当前状态，stop()则是停下电机

mbed/DC-motor

3.mbed烧录程序

1）程序编写完成后，在当前项目的main文件窗口下，点击complie编译程序，如果程序没错通过了编译，会生成一个bin文件，并进行下载

2）插上mbed的数据线，把下载好的bin文件复制到mbed这个文件夹内

3）按mbed上的复位按钮2-3秒，松手后mbed指示灯闪烁三次，则说明程序烧录成功

4.USB串口通信

1）mbed和ROS的信息通过USB port来传输，所以需要先运行起一个serial节点(运行任何节点之前都要运行master节点)
$ rosrun rosserial_python serial_node.py /dev/ttyACM0
这个serial_node节点是ROS-kinetic里已经有的，所以不需要自己编写，/dev/ttyACM0 指的是当前通信串口的地址，一般只有一个USB连接单片机在使用的时候默认分配到ACM0，谨记是ACM，不是USB

2）这个时候如果没有root的可读可写权限，会出现permission denied的越权错误（+tu）
解决办法是，在终端输入指令
$ sudo chmod 777 /dev/ttyACM0
不过这需要每次连接上USB数据线的时候都要获取权限

5.RPC通信

1）RPC通信与rosserial_mbed库所用到的通信是不同的，RPC通信使用起来比rosserial通信复杂，因为这个通信格式使用里RPCVariable，不同与ROS框架内的任何消息类型，所以需要在ROS端写专门写一个bridge来负责传递参数的任务

2）用到的函数库
RPCInterface

3）参考
zumy_ros_bridge
mbed_rpc

6.编写电机控制代码

1）向工程项目中添加Motordriver库

选择需要添加的路径后点击Import确认添加

2）添加ros-kinetic库
步骤同上，库源码地址：
ros_lib_kinetic

3）电机控制代码

#include <mbed.h>
#include <ros.h>
#include <geometry_msgs/Twist.h>
#include <motordriver.h>

ros::NodeHandle nh;

Motor A_fwd(p24, p6, p5, 1); // pwm, fwd, rev, can brake 
Motor A_rev(p23, p6, p5 ,1);
Motor B_fwd(p22, p7, p8 ,1);
Motor B_rev(p21, p7, p8 ,1);

void messageCb(const geometry_msgs::Twist& msg) //速度的消息类型是Twist
{
  if (msg.angular.z == 0 && msg.linear.x == 0)
  {
    A_fwd.speed(0);
    A_rev.speed(0);
    B_fwd.speed(0);
    B_rev.speed(0);
    wait(0.5);
  }
  else
  {
    if (msg.angular.z < 0)
    {
      float speed = (float)(msg.angular.z/10);//angular数值与speed()实际需要的参数存在比例关系，否则当angular数据在数值很小的时候，也对应一个很大的速度值的话，电机就不能准确调速甚至无法调速，同样的道理使用于linear
      A_fwd.speed(speed);
      B_rev.speed(speed);
    }
    else if (msg.angular.z > 0)
    {
      float speed = (float)(msg.angular.z/10);
      A_rev.speed(speed);
      B_fwd.speed(speed);
    }
    else if (msg.linear.x < 0)
    {
      float speed = (float)(msg.linear.x);
      A_fwd.speed(speed);
      B_fwd.speed(speed);
    }
    else if (msg.linear.x > 0)
    {
      float speed = (float)(msg.linear.x);
      A_rev.speed(speed);
      B_rev.speed(speed);
    }
  }
}

ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &messageCb);

Timer t;

int main()
{
  wait_ms(10);
  t.start();
  long vel_timer = 0;
  nh.initNode();
  nh.subscribe(sub);

  while (1)
  {
    if (t.read_ms() > vel_timer)
    {
      //motorDriver.stop();
      vel_timer = t.read_ms() + 500;
    }
    nh.spinOnce();
    wait_ms(100);
  }
}

从电机控制代码可以看到，电机的指令是以Twist类型发出，mbed上有一个订阅者，当接收到这类型的消息后，调用回调函数，输出不同的PWM以控制电机运动
但这个Motordriver库可能不一定适用于所有种类的H桥，笔者使用的DRV8833，stop()函数无法让电机停下，所以选择了speed(0)来替代stop()

7.使用rqt_robot_steering控制电机

1）运行serial节点进行通信
$ rosrun rosserial_python serial_node.py /dev/ttyACM0
2）打开robot_steering
$ rosrun rqt_robot_steering rqt_robot_steering
3）使用wasd键控制运动方向，空格键是刹车

8.使用手柄控制底盘运动

使用的手柄类型：PS3
1）下载jstest-gtk，测试，校准手柄$ sudo apt-get jstest-gtk

2）查看手柄的ID $ ls /dev/input/

插入手柄后再查看一次，多出来的则是当前使用的手柄的ID

3）安装ROS的joy接口
$ sudo apt-get install ros-kinetic-joy

4）创建learning_joy软件包，并编写learning_joy.cpp文件

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Joy.h>

class Teleop
{
public:
  Teleop();

private:
  void joyCallback(const sensor_msgs::Joy::ConstPtr& joy);

  ros::NodeHandle nh_;

  int linear_, angular_;
  double l_scale_, a_scale_;
  ros::Publisher vel_pub_;
  ros::Subscriber joy_sub_;
};

Teleop::Teleop():
  linear_(1),
  angular_(2)
{
  nh_.param("axis_linear", linear_, linear_);
  nh_.param("axis_angular", angular_, angular_);
  nh_.param("scale_angular", a_scale_, a_scale_);
  nh_.param("scale_linear", l_scale_, l_scale_);

  vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
  joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy", 10, &Teleop::joyCallback, this);
}

void Teleop::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
{
  geometry_msgs::Twist twist;
  twist.angular.z = a_scale_*joy->axes[angular_];
  twist.linear.x = l_scale_*joy->axes[linear_];
  vel_pub_.publish(twist);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "teleop");
  Teleop teleop;

  ros::spin();
}

5）修改CMakeLists.txt文件
在最后添加
add_executable(learning_joy src/learning_joy.cpp)
target_link_libraries(learning_joy ${catkin_LIBRARIES})

6）编写launch文件
首先在learning_joy软件包目录下创建一个launch文件夹，在launch文件夹内编辑learning_joy.launch文件
$ gedit learning_joy.launch

<launch>

 <!-- joy node -->
  <node respawn="true" pkg="joy"
        type="joy_node" name="learning_joy" >
    <param name="dev" type="string" value="/dev/input/js1" />
    <param name="deadzone" value="0.12" />
  </node>

 <!-- Axes -->
  <param name="axis_linear" value="1" type="int"/>
  <param name="axis_angular" value="0" type="int"/>
  <param name="scale_linear" value="1" type="double"/>
  <param name="scale_angular" value="5" type="double"/>
  <node pkg="learning_joy" type="turtle_teleop_joy" name="teleop"/>
</launch>

7）编译软件包$ catkin_make

8）运行手柄控制节点$ roslaunch learning_joy learning_joy.launch

9）参考
TeleopNode/wiki
ConfiguringALinuxJoystick