监听turtlesim仿真器，发送数据到实际的机器人--20

摘要: 原创博客:转载请表明出处: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
",buf[0]);
        printf("buf[1] = %d
",buf[1]);

        fd = open("/dev/ttyUSB0", O_RDWR | O_NOCTTY | O_NDELAY);
        if (fd < 0) { 
            printf("[%s-%d] open error!!!
", __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!!!
", __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