标签：
摘要: 原创博客:转载请表明出处:http://www.cnblogs.com/zxouxuewei/
1.0.本教程教你写实际的ros程序，控制自己的机器人。采用的是PC端的ubuntu+ros。终端为了能够使用低成本的里程计 ，陀螺仪 ，加速度计，超声波等传感器，采用STM32控制器。两者通过串口通信，交互数据。所有代码都有认真学习的必要。前提是你以经在ros.wiki或者书本上了解了ros 下的 话题，消息，节点等名词。
2.0.由于ros官方采用tuitlesim仿真器，我们再次也按照这种方式进行。首先在ros工作空间中创建自己的功能包：
catkin_creat_pkg zxwtest_package std_msgs rospy roscpp(功能包依赖std_msgs rospy roscpp)
然后在zxwtest_package/src下创建一个hello_node.cpp
2.1.修改catkin_make所需的编译选项配置。通过vim或者自己喜欢的编辑器打开zxwtest_package目录下的CMakeLists.txt文件，加载自己的编译选项，修改过后如下：
add_executable(hello_node src/hello.cpp)
target_link_libraries(hello_node ${catkin_LIBRARIES})
2.2.现在就要开始写代码了。一个属于自己的ros代码。
vim hello_node.cpp
代码如下：代码比较简单 希望各位认真消化
#include "ros/ros.h" //添加ros核心的头文件 #include "geometry_msgs/Twist.h" //包含geometry_msgs::Twist消息头文件 #include <stdlib.h> int main(int argc,char** argv) { ros::init(argc,argv,"publish_zhouxuewei"); //初始化ros节点 ros::NodeHandle node_handle; ros::Publisher pub = node_handle.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1000); ros::Rate loop_rate(10); while(ros::ok()){ geometry_msgs::Twist msg; msg.linear.x = double(rand())/double(RAND_MAX); //给消息中的变量赋值 msg.angular.z = 2*double(rand())/double(RAND_MAX)-1; pub.publish(msg); //发布消息 ROS_INFO_STREAM("Sending random velocity command!"); //ros可调式日志输出 loop_rate.sleep(); } }
2.3.编译代码：到你的工作空间的顶层目录下：
catkin_make
如果没有错误就一切正常，你也会看到相应的输出：
Built target hello_node
2.4.测试代码：(此程序正如你所看到的会产生-1到1之间的随机数，控制turtlesim移动)
roscore
rosrun turtlesim turtlesim_node
rosrun zxwtest_package hello_node
查看节点 框图：
rqt_graph
3.0.下面教你如何订阅节点在话题上发布的消息，下面的代码订阅了turtle1/Pose话题上的消息。并用ros日志输出。
3.1.在zxwtest_package/src新建listen_node.cpp
代码如下：
#include <ros/ros.h> #include <turtlesim/Pose.h> #include <iomanip> void poseMessageReceived(const turtlesim::Pose& msg) { ROS_INFO_STREAM(std::setprecision(2) << std::fixed << "position=("<< msg.x <<","<< msg.y <<")" <<" *direction="<<msg.theta); } int main(int argv,char** argc) { ros::init(argv,argc,"listener_pose"); ros::NodeHandle node_Handle; ros::Subscriber sub = node_Handle.subscribe("turtle1/pose",1000,&poseMessageReceived); ros::spin(); return 0; }
3.2.编译代码。首先还是要修改编译属性，通过vim或者自己喜欢的编辑器打开zxwtest_package目录下的CMakeLists.txt文件，加载自己的编译选项，修改过后如下：
add_executable(listen_node src/listen_node.cpp)
target_link_libraries(listen_node.cpp ${catkin_LIBRARIES})
到你的工作空间的顶层目录下：
catkin_make
如果没有错误就一切正常，你也会看到相应的输出：
Built target listen_node
3.3.测试代码：
roscore
rosrun turtlesim turtlesim_node
rosrun turtlesim turtle_teteop_key
rosrun zxwtest_package listen_node
当你通过键盘控制节点控制turtlrsim时，相应的输出如下：
查看节点 框图：
rqt_graph
4.0.以下程序在ros机器人当中应用非常多，pc端通过串口下发数据到终端执行。我们订阅/turtle1/cmd_vel话题上的turtlesim移动的角速度和线速度信息，下发到我们的机器人上，让他也跟着turtlesim通过键盘控制移动。
4.1.在zxwtest_package/src新建send_serial_node.cpp
代码如下：
#include <ros/ros.h> #include "geometry_msgs/Twist.h" #include <iomanip> #include <stdio.h> #include <string.h> #include <unistd.h> #include <fcntl.h> #include <errno.h> #include <termios.h> void Serial_Send_Data(const geometry_msgs::Twist& msg) { int i,fd,iRet; struct termios options_old, options; char buf[2]; buf[0] = (char)msg.linear.x; buf[1] = (char)msg.angular.z; printf("buf[0] = %d\n",buf[0]); printf("buf[1] = %d\n",buf[1]); fd = open("/dev/ttyUSB0", O_RDWR | O_NOCTTY | O_NDELAY); if (fd < 0) { printf("[%s-%d] open error!!!\n", __FILE__, __LINE__); } fcntl(fd, F_SETFL, 0); /*********************************************************/ tcgetattr(fd, &options); options_old = options; options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); /*Input*/ options.c_oflag &= ~OPOST; /** 选择原始输出 **/ /*** Set the new options for the port... **/ tcsetattr(fd, TCSANOW, &options); /*********************************************************/ iRet = write(fd, &buf,sizeof(buf)); if (iRet < 0) { printf("[%s-%d] write error!!!\n", __FILE__, __LINE__); } tcsetattr(fd, TCSANOW, &options_old); close(fd); } int main(int argv,char** argc) { ros::init(argv,argc,"serial_send"); ros::NodeHandle node_Handle; ros::Subscriber sub = node_Handle.subscribe("/turtle1/cmd_vel",1000,&Serial_Send_Data); ros::spin(); return 0; }
4.2.编译代码。首先还是要修改编译属性，通过vim或者自己喜欢的编辑器打开zxwtest_package目录下的CMakeLists.txt文件，加载自己的编译选项，修改过后如下：
add_executable(send_serial_node src/send_serial_node.cpp)
target_link_libraries(send_serial_node.cpp ${catkin_LIBRARIES})
到你的工作空间的顶层目录下：
catkin_make
如果没有错误就一切正常，你也会看到相应的输出：
Built target send_serial_node
4.3.测试代码：
roscore
rosrun turtlesim turtlesim_node
rosrun turtlesim turtle_teteop_key
rosrun zxwtest_package send_serial_node
将你的电脑和你的机器人通过串口连接，在键盘控制节点终端移动机器人，pc端输出如下：
机器人端收到数据后也相应移动。
查看节点 框图：
rqt_graph
监听turtlesim仿真器，发送数据到实际的机器人--20
标签：
原文地址：http://www.cnblogs.com/zxouxuewei/p/5281644.html
