監聽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\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