ROS下语音控制移动机器人代码

#include<ros/ros.h>
#include<geometry_msgs/Twist.h>
#include<std_msgs/String.h>
#include<pthread.h>
#include<iostream>
#include<stdio.h>

using namespace std;

ros::Publisher pub;
geometry_msgs::Twist vel_cmd;
pthread_t pth_[4];

void* vel_ctr(void* arg)
{
    while(true)
    {
        usleep(50000);
        pub.publish(vel_cmd);    
        ros::spinOnce();
    }
    return 0;
}

void callback(const std_msgs::String::ConstPtr& msg)
{

    ROS_INFO("I said: %s",msg->data.c_str());

    if(msg->data == "go")
    {
        vel_cmd.linear.x = 1;
        vel_cmd.angular.z = 0;
        pthread_create(&pth_[0],NULL,vel_ctr,NULL);
    }
    if(msg->data == "back")
    {
        vel_cmd.linear.x = -1;
        vel_cmd.angular.z = 0;
        pthread_create(&pth_[1],NULL,vel_ctr,NULL);
    }
    if(msg->data == "left")
    {
        vel_cmd.linear.x = 0;
        vel_cmd.angular.z = 1;
        pthread_create(&pth_[2],NULL,vel_ctr,NULL);
    }
    if(msg->data == "right")
    {
        vel_cmd.linear.x = 0;
        vel_cmd.angular.z = -1;
        pthread_create(&pth_[3],NULL,vel_ctr,NULL);
    }

}

int main(int argc, char** argv)
{
    ros::init(argc,argv,"voice_vel");
    ros::NodeHandle n;

    pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",10);
    ros::Subscriber sub = n.subscribe("/recognizer/output",10,callback);
    ros::spin();
}