HCR机器人实验2-ROS系统和HCR Arduino控制板通信 DF创客社区

上一个实验 HCR机器人实验1-建立底层传感器平台

这个实验我们将建立ROS系统和底层Arduino控制板的通信，使用主题发送电机控制消息、订阅电机编码器计数值，及其发送消息使编码器清零。

1、首先在你的电脑安装ubuntu及其ROS系统，如何安装和基本的使用可以参考 ROS的中文wiki
https://wiki.ros.org/cn
在这里我们推荐一个直接安装易科机器人的 Ubuntu 14.04.1 for ROS(indigo) by ExBot iso 发行版
这个版本包含最新的Ubuntu系统和最新的ROS indigo系统，省去一大把配置和下载更新的时间。

2、将SensorBoard的Arduino代码下载到Arduino mega控制板
代码中配置了4个订阅器用于接收电机的速度控制命令和编码器清零命令，
同时定义了两个发布器，用于发布编码器计数值。
如下代码添加在void setup()函数

  nh.initNode();    //初始化节点
  nh.subscribe(sub1);  //初始化订阅器1 控制电机1
  nh.subscribe(sub2);  //初始化订阅器2 控制电机2 
  nh.subscribe(sub3);  //初始化订阅器3 电机1编码器清零 
  nh.subscribe(sub4);  //初始化订阅器4 电机2编码器清零

  nh.advertise(pub1);   //初始化发布器1 电机2编码器清零 
  nh.advertise(pub2); 
复制代码

为订阅器编写回调函数处理命令：

ros::NodeHandle nh;   //节点句柄

/*--------------------------------------------------------------------------------------------
函数说明:  电机1控制函数
参数说明:  无
返回值  :  无
---------------------------------------------------------------------------------------------*/
void ROS_M1Control(const std_msgs::Int16 & cmd_msg)
{
  M1Speed = cmd_msg.data;  //设置M1电机的速度
  motorControl(M1Speed,M2Speed); 
        digitalWrite(13,HIGH-digitalRead(13));  
}
ros::Subscriber<std_msgs::Int16> sub1("motor1",ROS_M1Control);

/*--------------------------------------------------------------------------------------------
函数说明:  电机2控制函数
参数说明:  无
返回值  :  无
---------------------------------------------------------------------------------------------*/
void ROS_M2Control(const std_msgs::Int16 & cmd_msg)
{
  M2Speed = cmd_msg.data;  //设置M1电机的速度
  motorControl(M1Speed,M2Speed); 
        digitalWrite(13,HIGH-digitalRead(13));  
}
ros::Subscriber<std_msgs::Int16> sub2("motor2",ROS_M2Control);

/*--------------------------------------------------------------------------------------------
函数说明:  电机1编码器清零
参数说明:  无
返回值  :  无
---------------------------------------------------------------------------------------------*/
void ROS_M1EncoderClean(const std_msgs::Empty & cmd_msg)
{
  motorCleanEncoder(1);
        digitalWrite(13,HIGH-digitalRead(13));  
}
ros::Subscriber<std_msgs::Empty> sub3("M1EncoderClean",ROS_M1EncoderClean);

/*--------------------------------------------------------------------------------------------
函数说明:  电机2编码器清零
参数说明:  无
返回值  :  无
---------------------------------------------------------------------------------------------*/
void ROS_M2EncoderClean(const std_msgs::Empty & cmd_msg)
{
  motorCleanEncoder(2);
        digitalWrite(13,HIGH-digitalRead(13));  
}
ros::Subscriber<std_msgs::Empty> sub4("M2EncoderClean",ROS_M2EncoderClean);

std_msgs::Int32 M1EncoderCount;          //定义编码器消息变量                        
std_msgs::Int32 M2EncoderCount;
ros::Publisher pub1("M1EncoderCount", &M1EncoderCount);     //定义发布器pub1，主题名称 M1EncoderCount
ros::Publisher pub2("M2EncoderCount", &M2EncoderCount);     //定义发布器pub1，主题名称 M1EncoderCount
复制代码

在void readSensor(void) 函数中定期通过发布器发布编码器数据：

  motorGetEncoder(&EncoderCount1,&EncoderCount2);  //获取编码器累计脉冲数

  M1EncoderCount.data = EncoderCount1;  //将编码器数据通过发布器发送出去
  M2EncoderCount.data = EncoderCount2;
  pub1.publish(&M1EncoderCount);
  pub2.publish(&M2EncoderCount);
复制代码

3、通过主题的发布功能对电机进行控制，此时可以观察到电机转动
运行ROS内核

$ roscore
复制代码

运行ROS串口节点

$ rosrun rosserial_python serial_node.py /dev/ttyACM0
复制代码

通过另外一个终端发布主题消息
控制电机1反转，转速100转每分

$ rostopic pub -1  motor1 std_msgs/Int16 -- -100
复制代码

控制电机1正转，转速100转每分

$ rostopic pub -1  motor1 std_msgs/Int16 -- 100
复制代码

控制电机2反转，转速100转每分

$ rostopic pub -1  motor2 std_msgs/Int16 -- -100
复制代码

控制电机2正转，转速100转每分

$ rostopic pub -1  motor2 std_msgs/Int16 -- 100
复制代码

3、通过在PC上编写发布器节点来控制电机

首先要建立自己的工作空间
查询当前正在使用的空间

$ echo $ROS_PACKAGE_PATH
/opt/ros/indigo/share:/opt/ros/indigo/stacks
复制代码

创建自己用的空间
$ cd ~ $ mkdir -p dev/roscoding $ echo "export ROS_PACKAGE_PATH=~/dev/roscoding:${ROS_PACKAGE_PATH}" >>~/.bashrc $ . ~/.bashrc 复制代码

创建新的个功能包 hcr_robot

$ cd ~/dev/roscoding $ roscreate-pkg hcr_robot std_msgs rospy roscpp 复制代码

查找功能包地址

$ rospack find hcr_robot 
/home/exbot/dev/roscoding/hcr_robot
复制代码

查看依赖关系

$ rospack depends hcr_robot 
cpp_common
rostime
roscpp_traits
roscpp_serialization
genmsg
genpy
message_runtime
std_msgs
catkin
gencpp
genlisp
message_generation
rosbuild
rosconsole
rosgraph_msgs
xmlrpcpp
roscpp
rosgraph
rospack
roslib
rospy
复制代码

查看内容

$ rosls hcr_robot
CMakeLists.txt  include  mainpage.dox  Makefile  manifest.xml  src
复制代码

进入到功能包hcr_robot路径

$ roscd hcr_robot
复制代码

进入src目录在目录新建hcrControl.cpp

#include "ros/ros.h"                //ROS必要的文件
#include "std_msgs/Int16.h"  //16位数组消息类型
#include <sstream>
/*----------------------------------------------------------------------------------------------------------------------
函数说明: 建立一个hcrControl 节点，发布 电机控制 motor1 和 motor2两个消息
                  这两个消息被HCR的arduoino主板订阅，用于控制电机
参数说明:  无
返回值  :  无
----------------------------------------------------------------------------------------------------------------------*/
int main(int argc, char **argv)
{
    int i;
    ros::init(argc, argv, "hcrControl");     //设置唯一的节点名称 hcrControl
    ros::NodeHandle n;      //节点句柄
        //将节点设置为发布者，并将发布的主题的类型和名称告知节点管理器，消息名称motor，缓冲区1000
    ros::Publisher pub1 = n.advertise<std_msgs::Int16>("motor1", 1000);
    ros::Publisher pub2 = n.advertise<std_msgs::Int16>("motor2", 1000);
        ros::Rate loop_rate(10);  //发送速率为10hz

    std_msgs::Int16 msg1;   //创建一个 16位 消息变量
    std_msgs::Int16 msg2;   //创建一个 16位 消息变量
    msg1.data = 0;
    msg2.data = 0;

    while (ros::ok())        //收到Ctrl+C 后停止
        {
        for(i=0;i<164;i++)  //HCR的两个轮子从0速度到前进最大速度
        {
            msg1.data = i;
            msg2.data = i;
            pub1.publish(msg1);                //发布消息
            pub2.publish(msg2);                //发布消息
            ros::spinOnce();    //如果有订阅者出现，ROS就会更新和读取所有主题
            loop_rate.sleep();        //按照10hz的频率挂起
        }
        for(i=0;i>-164;i--)  //HCR的两个轮子从0速度到后退最大速度
        {
            msg1.data = i;
            msg2.data = i;
            pub1.publish(msg1);                //发布消息
            pub2.publish(msg2);                //发布消息
            ros::spinOnce();    //如果有订阅者出现，ROS就会更新和读取所有主题
            loop_rate.sleep();        //按照10hz的频率挂起
        }
    }
        return 0;
}
复制代码

修改 CMakeLists.txt 文件，在文件末尾增加待编译的源文件

rosbuild_add_executable(hcrControl src/hcrControl.cpp)
复制代码

执行编译命令，对节点进行编译

$ rosmake hcr_robot
复制代码

如果编译没有错误我们就可以运行节点了。

4、测试PC端控制节点
打开一个终端执行ROS内核

$ roscore
复制代码

在另外一种终端执行串口服务节点

$ rosrun rosserial_python serial_node.py /dev/ttyACM0
复制代码

打开另外一个终端执行刚才我们编写的HCR控制节点

$ rosrun hcr_robot hcrControl
复制代码

此时可以观察HCR机器人两个轮子做匀速变速转动，首先正传从最慢到最快然后反转从最慢到最快。

打开另外一个终端通过主题读取发布器发出来的电机1编码器计数值

$ rostopic echo M1EncoderCount
复制代码

打开另外一个终端通过主题读取发布器发出来的电机2编码器计数值

$ rostopic echo M2EncoderCount
复制代码

打开另外一个终端通过发布器对电机的编码器计数值清零，可以切换到编码器计数值显示终端看看编码器值有没有改变

$ rostopic pub -1  M1EncoderClean std_msgs/Empty
$ rostopic pub -1  M2EncoderClean std_msgs/Empty
复制代码

我们还可以通过 rxgraph命令来在线监视节点状态图：

$ rosrun rqt_graph rqt_graph
复制代码

另外通过串口监控软件监控发现通过 ROS发送给Arduino int16 100 发现发送的数据是如下格式，包含了帧头、数据长度、数据和校验，说明ROS会将数据封包后再发送。
FF FE 02 00 FD 64 00 64 00 37
在运行rosserial_python 节点后，Arduino节点还会定期对自动发送一串字符串出来用于注册节点管理
FF FE 08 00 F7 0A 00 D2 2A 26 55 34 88 EB 0A CD FF FE 02 00 FD 64 00 64 00 37 FF FE 08 00 F7 0A 00 D4 2A 26 55 56 A3 D6 2C 81

hcr, 机器人, ros系统, Arduino, 通信