[原创] ros控制小车运行编程

1、 roscore

2、 sudo chmod a+rw /dev/ttyACM0

3、 rosrun rosserial_python serial_node.py /dev/ttyACM0

4、 rosrun turtlesim turtle_teleop_key

代码：：：

#include

#include

#include

//定义五中运动状态

#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;

//x轴方向的速度

double lin_vel = 0.0;

//y轴方向的速度

double ang_vel = 0.0;

//定义接受的键

int cmd_ctrl = 0;

//注册ROS节点

ros::NodeHandle nh;

//回调函数

void messageCb(constgeometry_msgs::Twist& vel) {

lin_vel = vel.linear.x;

ang_vel = vel.angular.z;

cmd_ctrl = 1 * lin_vel + 3 * ang_vel;

  }

//设置订阅的消息类型和发布的主题

ros::Subscribersub("/turtle1/cmd_vel", 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();

switch(cmd_ctrl)

  {

     case 2:

       motorRun(4);

       delay(2000);//每个命令执行2s

       motorRun(5);

       break;

      case -2:

        motorRun(3);

        delay(2000);//每个命令执行2s

        motorRun(5);

        break;

      case 6:

        motorRun(1);

         delay(2000);//每个命令执行2s

         motorRun(5);

        break;

      case -6:

        motorRun(2);

        delay(2000);//每个命令执行2s

         motorRun(5);

        break;

     default:

       motorRun(5);

       break;

    }

   cmd_ctrl = 0;

}

//运动控制函数

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);

  }

}