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

bobo 发表于 2015-4-21 16:43:55

HCR机器人实验2-ROS系统和HCR Arduino控制板通信

上一个实验 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 -1motor1 std_msgs/Int16 -- -100

控制电机1正转，转速100转每分
$ rostopic pub -1motor1 std_msgs/Int16 -- 100

控制电机2反转，转速100转每分
$ rostopic pub -1motor2 std_msgs/Int16 -- -100

控制电机2正转，转速100转每分
$ rostopic pub -1motor2 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.txtincludemainpage.doxMakefilemanifest.xmlsrc
进入到功能包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 -1M1EncoderClean std_msgs/Empty
$ rostopic pub -1M2EncoderClean 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

hnyzcj 发表于 2015-4-21 17:33:36

顶一个。

大连林海 发表于 2015-4-21 18:21:44

顶一个。:D

yoyojacky 发表于 2015-4-24 15:58:36

顶一下。

Jaky_wang 发表于 2015-10-28 08:13:24

这个教程节省了我们编写底层的精力和时间，而且也很难写的这么好，让我们可以花更多的时间来做上层的SLAM，很感谢博主。现在我遇到一个问题，就是采集编码器数据一般都是很正常，但是在我向下发送控制命令时，编码器1的数据会发生跳变，请问有什么解决办法吗？谢谢

bobo 发表于 2015-11-2 10:51:13

Jaky_wang 发表于 2015-10-28 08:13
这个教程节省了我们编写底层的精力和时间，而且也很难写的这么好，让我们可以花更多的时间来做上层的SLAM， ...

是如何跳变的?

lbbbo 发表于 2016-9-13 15:59:41

上传到arduino板，提示serial3未声明错误

喂_小刺 发表于 2020-2-1 22:42:35

666666666666666666

喂_小刺 发表于 2020-2-1 22:42:48

6666666666666666

DFHk-0ykaN8 发表于 2020-3-5 10:27:46

已经学习了楼主，感谢发帖。

页: [1]

DF创客社区 - 分享创造的喜悦's Archiver

HCR机器人实验2-ROS系统和HCR Arduino控制板通信