摘要: 原创博客:转载请表明出处: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 
     
      int main(int argc,char** argv){    ros::init(argc,argv,"publish_zhouxuewei");           //初始化ros节点    ros::NodeHandle node_handle;    ros::Publisher pub = node_handle.advertise
      
       ("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移动)

roscorerosrun turtlesim turtlesim_noderosrun zxwtest_package hello_node

查看节点 框图：

rqt_graph

3.0.下面教你如何订阅节点在话题上发布的消息，下面的代码订阅了turtle1/Pose话题上的消息。并用ros日志输出。

3.1.在zxwtest_package/src新建listen_node.cpp

代码如下：

#include 
     
      #include 
      
       #include 
       
        void poseMessageReceived(const turtlesim::Pose& msg){    ROS_INFO_STREAM(std::setprecision(2) << std::fixed << "position=("<< msg.x <<","<< msg.y <<")" <<" *direction="<
       
      
     

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.测试代码：

roscorerosrun turtlesim turtlesim_node rosrun turtlesim turtle_teteop_keyrosrun 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 
     
      #include "geometry_msgs/Twist.h"#include 
      
       #include 
       
        #include 
        
         #include 
         
          #include 
          
           #include 
           
            #include 
            
             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.测试代码：

roscorerosrun turtlesim turtlesim_node rosrun turtlesim turtle_teteop_keyrosrun zxwtest_package send_serial_node

将你的电脑和你的机器人通过串口连接，在键盘控制节点终端移动机器人，pc端输出如下：

机器人端收到数据后也相应移动。

查看节点 框图：

rqt_graph