ros控制小车运行代码

#include <ros.h>
#include <std_msgs/String.h>
#include <Servo.h>

//定义五中运动状态
#define STOP      0
#define FORWARD   1
#define BACKWARD  2
#define TURNLEFT  3
#define TURNRIGHT 4
//定义需要用到的引脚
int leftMotor1 = 4;
int leftMotor2 = 5;
int rightMotor1 = 6;
int rightMotor2 = 7;

ros::NodeHandle nh;

void messageCb(const std_msgs::String& cat_msg) {
     switch(cat_msg.data[0]) {
        case 'w':
            motorRun(1);
            delay(2000);//每个命令执行2s
            motorRun(5);
            break;
        case 's':
            motorRun(2);
            delay(2000);//每个命令执行2s
            motorRun(5);
            break;
         case 'a':
            motorRun(3);
            delay(2000);//每个命令执行2s
            motorRun(5);
            break;
         case 'd':
           motorRun(4);
           delay(2000);//每个命令执行2s
            motorRun(5);
           break;    
      }
  }

ros::Subscriber<std_msgs::String> sub("cat_run", &messageCb);

void setup() {
  // put your setup code here, to run once:
  //设置控制电机的引脚为输出状态
  pinMode(leftMotor1, OUTPUT);
  pinMode(leftMotor2, OUTPUT);
  pinMode(rightMotor1, OUTPUT);
  pinMode(rightMotor2, OUTPUT);

  nh.initNode();
  nh.subscribe(sub);
}

void loop() {
  // put your main code here, to run repeatedly:
  nh.spinOnce();
  delay(1);
}
//运动控制函数
void motorRun(int cmd)
{
  switch(cmd){
    case FORWARD:
      digitalWrite(leftMotor1, LOW);
      digitalWrite(leftMotor2, HIGH);
      digitalWrite(rightMotor1, LOW);
      digitalWrite(rightMotor2, HIGH);
      break;
     case BACKWARD:
      digitalWrite(leftMotor1, HIGH);
      digitalWrite(leftMotor2, LOW);
      digitalWrite(rightMotor1, HIGH);
      digitalWrite(rightMotor2, LOW);
      break;
     case TURNLEFT:
      digitalWrite(leftMotor1, HIGH);
      digitalWrite(leftMotor2, LOW);
      digitalWrite(rightMotor1, LOW);
      digitalWrite(rightMotor2, HIGH);
      break;
     case TURNRIGHT:
      digitalWrite(leftMotor1, LOW);
      digitalWrite(leftMotor2, HIGH);
      digitalWrite(rightMotor1, HIGH);
      digitalWrite(rightMotor2, LOW);
      break;
     default:
      digitalWrite(leftMotor1, LOW);
      digitalWrite(leftMotor2, LOW);
      digitalWrite(rightMotor1, LOW);
      digitalWrite(rightMotor2, LOW);
  }
}