ROS机器人小车底盘DIY有何难？不枉做一个程序猿，躯壳码上有功能

前文的躯壳

arduino stm32F103c8t6系统板的编程任务：
驱动TB6612
驱动GY85
驱动ps2遥控接收器
编码器的检测
PID速位控制
运动学算法
rosserial通讯协议

arduino好处是库比较多，上手快捷。
可以找一些开源的资料，按需整合，移植，调试。
但是有些还会遇到困难，需要自己创作。

问题：

比如编码器的库，可能找了很多都不太合适，需要改成中断的方案。
stm32使用arduino encoder库的改造草案https://blog.csdn.net/qq_38288618/article/details/106839319
比如rosserial的通讯协议探索。

ROS中rosserial通讯协议初探https://blog.csdn.net/qq_38288618/article/details/102931684
需要用ros配合来生成arduino用的库遇到问题。
ROS与Arduino硬件之rosserial_arduino（win10）https://blog.csdn.net/qq_38288618/article/details/104082877

等等吧，相信花点时间都能攻克。

上主文件的码：

#define USE_STM32_HW_SERIAL
#include <wirish_debug.h>
//#include <ros.h>
#include "zr_ros.h"
//header file for publishing velocities for odom
#include <zr_movebase_msgs/Velocities.h>

//header file for cmd_subscribing to "cmd_vel"
#include <geometry_msgs/Twist.h>

//header file for pid server
#include <zr_movebase_msgs/PID.h>

//header files for imu
#include <ros_arduino_msgs/RawImu.h>
#include <geometry_msgs/Vector3.h>
#include <ros/time.h>

//#include <zr_protocol/hw_info.h>
#include <zr_protocol/hw_cmd_.h>
#include "zr_info.h"

#include "config.h"
#include "Motor.h"
#include "PID.h"
#include "Kinematics.h"

#include <Wire.h>
#include "imu_configuration.h"

#include <PS2X_lib.h>  //for v1.6
#include "PS2X_zzz.h"

PS2X_ZZZ PS2X_ez(_PS2_CLK, _PS2_CMD, _PS2_SEL, _PS2_DAT, true, true);

//Kinematics kinematics(MAX_RPM, WHEEL_DIAMETER, BASE_WIDTH, PWM_BITS);
Kinematics kinematics(Kinematics::ZR_BASE, MAX_RPM, WHEEL_DIAMETER, FR_WHEELS_DISTANCE, LR_WHEELS_DISTANCE, PWM_BITS);
//Motor motor1(MOTOR1_PWM, MOTOR1_IN_A, MOTOR1_IN_B); //front
Motor motor1(MOTOR1_PWM, MOTOR1_IN_A, MOTOR1_IN_B); //front
Motor motor2(MOTOR2_PWM, MOTOR2_IN_A, MOTOR2_IN_B); //front
Motor motor3(MOTOR3_PWM, MOTOR3_IN_A, MOTOR3_IN_B); //back
Motor motor4(MOTOR4_PWM, MOTOR4_IN_A, MOTOR4_IN_B); //back
//COUNTS_PER_REV = 0 if no encoder
int Motor::counts_per_rev_ = COUNTS_PER_REV;
PID motor1_pid(-255, 255, K_P, K_I, K_D);
PID motor2_pid(-255, 255, K_P, K_I, K_D);
PID motor3_pid(-255, 255, K_P, K_I, K_D);
PID motor4_pid(-255, 255, K_P, K_I, K_D);

//zzz ros ----------------------------------------------------------------
double g_req_angular_vel_z = 0;
double g_req_linear_vel_x = 0;
double g_req_linear_vel_y = 0;
unsigned char led_blink=0;
unsigned char ros_msg_ctrl=1;
unsigned char stick_wait_ok=0;
unsigned char stick_setup_ok=1;
unsigned long g_prev_loop_first_time = 0;
unsigned long g_prev_stick_ctrl_time = 0;
unsigned long g_prev_stick_time = 0;
unsigned long g_prev_imu_check_time = 0;
unsigned long g_prev_led_blink_time = 0;
unsigned long g_prev_command_time = 0;
unsigned long g_prev_control_time = 0;
unsigned long g_publish_vel_time = 0;
unsigned long g_prev_imu_time = 0;
unsigned long g_prev_debug_time = 0;
void init_g_prev_time(unsigned long t);


bool g_is_first = true;

char g_buffer[50];

//callback function prototypes
void commandCallback(const geometry_msgs::Twist& cmd_msg);
void PIDCallback(const zr_movebase_msgs::PID& pid);
ros::NodeHandle nh;
ros::Subscriber<geometry_msgs::Twist> cmd_sub("cmd_vel", commandCallback);
ros::Subscriber<zr_movebase_msgs::PID> pid_sub("pid", PIDCallback);

ros_arduino_msgs::RawImu raw_imu_msg;
ros::Publisher raw_imu_pub("raw_imu", &raw_imu_msg);
zr_movebase_msgs::Velocities raw_vel_msg;
ros::Publisher raw_vel_pub("raw_vel", &raw_vel_msg);

//zzz ros-------------------------------------------------------------------

#define IO_REG_TYPE      uint32_t
#define PIN_TO_BASEREG(pin)             (portInputRegister(digitalPinToPort(pin)))
#define PIN_TO_BITMASK(pin)             (digitalPinToBitMask(pin))
#define DIRECT_PIN_READ(base, mask)     (((*(base)) & (mask)) ? 1 : 0)

typedef struct {
  volatile IO_REG_TYPE * pin1_register;
  volatile IO_REG_TYPE * pin2_register;
  IO_REG_TYPE            pin1_bitmask;
  IO_REG_TYPE            pin2_bitmask;
  uint8_t pin1;
  uint8_t pin2;
  uint8_t state;
  int32_t position;
} Encoder_internal_state_t;

Encoder_internal_state_t * interruptArgs[4];
Encoder_internal_state_t encoder1;
Encoder_internal_state_t encoder2;
Encoder_internal_state_t encoder3;
Encoder_internal_state_t encoder4;
uint8_t oldstate=0;
float stick_a=0.0;
float stick_v=0.0;
float stick_vy=0.0;
void stick_callback(PS2X_ZZZ::PSS stick_dat){
  //
  if(stick_wait_ok){
  int d=stick_dat.lx-128;
  int d_abs;
  int d_f;
  d_f=d<0?-1:1;
  d_abs=abs(d);
  d=d_abs<2?0:d;
  d_f=d==0?0:d_f;
  stick_a=((float)d-2.0*d_f)/126.0;
  
  d=stick_dat.ry-128;
  d_f=d<0?-1:1;
  d_abs=abs(d);
  d=d_abs<2?0:d;
  d_f=d==0?0:d_f;
  stick_v=-((float)d-2.0*d_f)/126.0;//add"-" zzz 6612 is not same as 8833
//!!!!MECANUM  add y director//
  d=stick_dat.rx-128;
  d_f=d<0?-1:1;
  d_abs=abs(d);
  d=d_abs<2?0:d;
  d_f=d==0?0:d_f;
  stick_vy=((float)d-2.0*d_f)/126.0;
  //Serial.println(stick_dat.lx,DEC);

  ros_msg_ctrl=0;
  //stick yaokong youxian 
  g_req_linear_vel_x=stick_v*0.4;//0.1;//0.4mps
  g_req_linear_vel_y=stick_vy*0.4;//0.1;//0.4mps
  g_req_angular_vel_z=stick_a*3.14/2;//45du/s
  g_prev_stick_ctrl_time=millis();
  g_prev_command_time =millis();
  }
}
void update1(Encoder_internal_state_t *arg) {
      //uint8_t state = arg->state & 3;
      //if (digitalRead(arg->pin1)) state |= 4;
      //if (digitalRead(arg->pin2)) state |= 8;

      uint8_t p1val = DIRECT_PIN_READ(arg->pin1_register, arg->pin1_bitmask);
      uint8_t p2val = DIRECT_PIN_READ(arg->pin2_register, arg->pin2_bitmask);
      uint8_t state = arg->state & 3;
      if (p1val) state |= 4;
      if (p2val) state |= 8;
      
      arg->state = (state >> 2);
   
      switch (state) {
        case 1: case 7: case 8: case 14:
          arg->position++;
          return;
        case 2: case 4: case 11: case 13:
          arg->position--;
          return;
        case 3: case 12:
          arg->position += 2;
          return;
        case 6: case 9:
          arg->position -= 2;
          return;
      }
}
void update(void) {
  //noInterrupts();
  update1(interruptArgs[0]);
  update1(interruptArgs[1]);
  update1(interruptArgs[2]);
  update1(interruptArgs[3]);
  //interrupts();
}
//void callback_srv(const zr_protocol::hw_info::Request & req, zr_protocol::hw_info::Response & res){
//  res.output=RosDeviceInfo::getInfoValue(req.input);  
//}
//ros::ServiceServer<zr_protocol::hw_info::Request, zr_protocol::hw_info::Response> server("zr_hw_info",&callback_srv);

void callback_hw_cmd(const zr_protocol::hw_cmd::Request & req, zr_protocol::hw_cmd::Response & res) {
  if (strcmp(req.cmd, "chr") == 0) {    
    uint8_t dat[8] ;
    char i=1;
    char maxi=req.input_length>9?9:req.input_length;
    while( i<req.input_length){dat[i-1]=req.input[i];i++;}
    i--;
    while( i<8){dat[i]=0;i++;}
    
    //lcd.createChar(req.input[0], dat);
    res.output = ( unsigned char*) "ok";
    res.output_length=2;    
  } else {
    res.output = ( unsigned char*) RosDeviceInfo::getInfoValue(req.cmd);    
    res.output_length=zr::buffStrLen((char*) res.output);
  }
}
ros::ServiceServer<zr_protocol::hw_cmd::Request, zr_protocol::hw_cmd::Response> server_hw_cmd("zr_hw_cmd", &callback_hw_cmd);

void ROS_setup()
{  
  nh.initNode();
  ArduinoHardware* hw = nh.getHardware();
  hw->setBaud(500000);
  hw->init();
  //Serial.begin(500000);
  nh.advertiseService(server_hw_cmd);
  nh.advertise(raw_imu_pub);
  nh.advertise(raw_vel_pub);
  nh.subscribe(pid_sub);
  nh.subscribe(cmd_sub);
}
void init_encoder_pin_data(uint8_t pin1,uint8_t pin2,Encoder_internal_state_t *encoder,uint8_t idx){
  encoder->pin1 = pin1;
  encoder->pin2 = pin2;
  encoder->pin1_register = PIN_TO_BASEREG(pin1);
  encoder->pin1_bitmask = PIN_TO_BITMASK(pin1);
  encoder->pin2_register = PIN_TO_BASEREG(pin2);
  encoder->pin2_bitmask = PIN_TO_BITMASK(pin2);
  //encoder->state=0;
  encoder->position=0;
  pinMode(pin1, INPUT);  
  //digitalWrite(pin1, HIGH);
  //pinMode(pin1, INPUT_PULLUP);
  digitalWrite(pin1, LOW);
  pinMode(pin1, INPUT_PULLDOWN);
  
  pinMode(pin2, INPUT);
  //digitalWrite(pin2, HIGH);
  //pinMode(pin2, INPUT_PULLUP);
  digitalWrite(pin2, LOW);  
  pinMode(pin2, INPUT_PULLDOWN);
  interruptArgs[idx]=encoder;
  //
  delayMicroseconds(2000);
  uint8_t s = 0;
  if (DIRECT_PIN_READ(encoder->pin1_register, encoder->pin1_bitmask)) s |= 1;
  if (DIRECT_PIN_READ(encoder->pin2_register, encoder->pin2_bitmask)) s |= 2;
  encoder->state = s;
}

Kinematics::outputf req_rpm;
int spd1,spd2,spd3,spd4;
float set1,set2,set3,set4;
float cur1,cur2,cur3,cur4;
float pid_p,pid_i,pid_d;
void moveBase()
{  
  //get the required rpm for each motor based on required velocities
  req_rpm = kinematics.getRPM(g_req_linear_vel_x, g_req_linear_vel_y, g_req_angular_vel_z);

  //the required rpm is capped at -/+ MAX_RPM to prevent the PID from having too much error
  //the PWM value sent to the motor driver is the calculated PID based on required RPM vs measured RPM
  
  set1=req_rpm.motor1;//(float) constrain(req_rpm.motor1, -MAX_RPM, MAX_RPM);
  cur1=motor1.rpm;
  spd1=motor1_pid.compute(set1, cur1);
  motor1.spin(spd1);
  
  set3=req_rpm.motor3;//(float) constrain(req_rpm.motor3, -MAX_RPM, MAX_RPM);
  cur3=motor3.rpm;
  spd3=motor3_pid.compute(set3, cur3);
  motor3.spin(spd3);
  
  set2=-req_rpm.motor2;//(float) constrain(-req_rpm.motor2, -MAX_RPM, MAX_RPM);
  cur2=motor2.rpm;
  spd2=motor2_pid.compute(set2, cur2);  
  motor2.spin(spd2);
  
  set4=-req_rpm.motor4;//(float) constrain(-req_rpm.motor4, -MAX_RPM, MAX_RPM);
  cur4=motor4.rpm;
  spd4=motor4_pid.compute(set4, cur4); 
  motor4.spin(spd4);
}
void stopBase()
{
  g_req_linear_vel_x = 0.0;
  g_req_linear_vel_y = 0.0;
  g_req_angular_vel_z = 0.0;
}

void publishVelocities()
{
  //update the current speed of each motor based on encoder's count
//  motor1.updateSpeed(encoder1.position);
//  motor2.updateSpeed(encoder2.position);
//  motor3.updateSpeed(encoder3.position);
//  motor4.updateSpeed(encoder4.position);

  Kinematics::velocities vel;
  vel = kinematics.getVelocities(motor1.rpm, -motor2.rpm, motor3.rpm, -motor4.rpm);

  //fill in the object
  raw_vel_msg.linear_x = vel.linear_x;
  raw_vel_msg.linear_y = vel.linear_y;
  raw_vel_msg.angular_z = vel.angular_z;

//
//Serial.println("encoder  :");
//Serial.print(encoder1.position);Serial.print(" , ");
//Serial.print(encoder2.position);Serial.print(" , ");
//Serial.print(encoder3.position);Serial.print(" , ");
//Serial.println(encoder4.position);
//Serial.println("publishVelocities");
//Serial.print("linear_x  :");Serial.println(vel.linear_x);
//Serial.print("linear_y  :0");
//Serial.print("angular_z :");Serial.println(vel.angular_z);
  //publish raw_vel_msg object to ROS
  raw_vel_pub.publish(&raw_vel_msg);
}

void checkIMU_zzz()
{
  //this function checks if IMU is present
  Serial.println("Accelerometer");
  raw_imu_msg.accelerometer = checkAccelerometer();
raw_imu_msg.accelerometer = checkAccelerometer();
raw_imu_msg.accelerometer = checkAccelerometer();
  Serial.println("gyroscope");  
  raw_imu_msg.gyroscope = checkGyroscope();

  
  
  Serial.println("magnetometer");
  raw_imu_msg.magnetometer = checkMagnetometer();

Serial.println(raw_imu_msg.accelerometer);
Serial.println(raw_imu_msg.gyroscope);
Serial.println(raw_imu_msg.magnetometer);
  if (!raw_imu_msg.accelerometer)
  {
    Serial.println("Accelerometer NOT FOUND!");
  }

  if (!raw_imu_msg.gyroscope)
  {
    Serial.println("Gyroscope NOT FOUND!");
  }

  if (!raw_imu_msg.magnetometer)
  {
    Serial.println("Magnetometer NOT FOUND!");
  }

g_is_first = false;
}
void publishIMU_zzz()
{
  if (raw_imu_msg.accelerometer || raw_imu_msg.gyroscope || raw_imu_msg.magnetometer){
    //this function publishes raw IMU reading
    raw_imu_msg.header.stamp = nh.now();
    raw_imu_msg.header.frame_id = "imu_link";

    //measure accelerometer
    if (raw_imu_msg.accelerometer){
      measureAcceleration();
      raw_imu_msg.raw_linear_acceleration = raw_acceleration;
    }

    //measure gyroscope
    if (raw_imu_msg.gyroscope){
      measureGyroscope();
      raw_imu_msg.raw_angular_velocity = raw_rotation;
    }

    //measure magnetometer
    if (raw_imu_msg.magnetometer){
      measureMagnetometer();
      raw_imu_msg.raw_magnetic_field = raw_magnetic_field;
    }

    //publish raw_imu_msg object to ROS
//    Serial.println("publish raw_imu_msg------");
//    Serial.println("raw_acceleration");
//    Serial.print(raw_acceleration.x);Serial.print(" , ");
//    Serial.print(raw_acceleration.y);Serial.print(" , ");
//    Serial.println(raw_acceleration.z);
//    Serial.println("raw_rotation");
//    Serial.print(raw_rotation.x);Serial.print(" , ");
//    Serial.print(raw_rotation.y);Serial.print(" , ");
//    Serial.println(raw_rotation.z);
//    Serial.println("raw_magnetic_field");
//    Serial.print(raw_magnetic_field.x);Serial.print(" , ");
//    Serial.print(raw_magnetic_field.y);Serial.print(" , ");
//    Serial.println(raw_magnetic_field.z);
    
    raw_imu_pub.publish(&raw_imu_msg);
  }
}
void printDebug()
{
  sprintf (g_buffer, "Encoder FrontLeft: %ld", encoder1.position);
  nh.loginfo(g_buffer);
  sprintf (g_buffer, "Encoder RearLeft: %ld", encoder3.position);
  nh.loginfo(g_buffer);
  sprintf (g_buffer, "Encoder FrontRight: %ld", encoder2.position);
  nh.loginfo(g_buffer);
  sprintf (g_buffer, "Encoder RearRight: %ld", encoder4.position);
  nh.loginfo(g_buffer);
}
void printDebug(float dat1,float dat2,float dat3,float dat4)
{
  sprintf (g_buffer, "dat1: %lf", dat1);
  nh.loginfo(g_buffer);
  sprintf (g_buffer, "dat2: %lf", dat2);
  nh.loginfo(g_buffer);
  sprintf (g_buffer, "dat3: %lf", dat3);
  nh.loginfo(g_buffer);
  sprintf (g_buffer, "dat4: %lf", dat4);
  nh.loginfo(g_buffer);
}
void init_g_prev_time(unsigned long t){
 g_prev_loop_first_time=t;
 g_prev_stick_ctrl_time = t;
 g_prev_stick_time = t;
 g_prev_imu_check_time = t;
 g_prev_led_blink_time = t;
 g_prev_command_time = t;
 g_prev_control_time = t;
 g_publish_vel_time = t;
 g_prev_imu_time = t;
 g_prev_debug_time = t;
}
void setup() {
  motor1.spin(0);
  motor2.spin(0);
  motor3.spin(0);
  motor4.spin(0);
  
  afio_cfg_debug_ports(AFIO_DEBUG_SW_ONLY);
  PS2X_ez.PS2_PSS_callback=stick_callback;

  init_encoder_pin_data(MOTOR1_ENCODER_A,MOTOR1_ENCODER_B,&encoder1,0);//front
  init_encoder_pin_data(MOTOR2_ENCODER_A,MOTOR2_ENCODER_B,&encoder2,1);//front
  init_encoder_pin_data(MOTOR3_ENCODER_A,MOTOR3_ENCODER_B,&encoder3,2);//front
  init_encoder_pin_data(MOTOR4_ENCODER_A,MOTOR4_ENCODER_B,&encoder4,3);//front
  Timer4.pause();
  Timer4.setPrescaleFactor(72); // 1 µs resolution
  Timer4.setMode(TIMER_CH3, TIMER_OUTPUTCOMPARE);    
  Timer4.setCount(0);
  Timer4.setOverflow(100);//脉冲5000hz/s，中断设置10000次/s检测,保证高低电平各取样2个点
  Timer4.setCompare(TIMER_CH1, 100);   // somewhere in the middle
  Timer4.attachInterrupt(TIMER_CH1, update);
  Timer4.refresh();
  Timer4.resume();

  //Serial.begin(9600);
  delay(10);
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(PC13,HIGH);

  pinMode(PB7, INPUT_PULLDOWN);
  Wire.setClock(400000);
  Wire.begin();

  ROS_setup();
  delay(100);
  checkIMU_zzz();//cost long time.
  init_g_prev_time(millis());
}


int c=0;
long pp=-999;
  int pwm1,pwm2,pwm3,pwm4;
void loop() {

  //delay(5);
  //if(c%100==0){    c=0;  }  c++;
  //this block drives the robot based on defined rate
  if (stick_wait_ok==0){
    if(millis()>g_prev_loop_first_time+1000){
      stick_wait_ok=1;//wait 1s ,kepp no cmd
      stick_setup_ok=PS2X_ez.ps_setup;//wait 1s ,try to conect stick device.
    }
  }
  if(stick_setup_ok){//< 1s value==1,> 1s value == the last PS2X_ez.ps_setup
    if ((millis() - g_prev_stick_time) >= (100)){
      stick_a=0.0;
      stick_v=0.0;
      PS2X_ez.PS2_loop();      
      g_prev_stick_time = millis();
    }
  }//else no stick device or device error.
  if ((millis() - g_prev_stick_ctrl_time) >= (250)){
    ros_msg_ctrl=1;
  }
  if ((millis() - g_prev_command_time) >= (200)){
    stopBase();
  }
  //this block drives the robot based on defined rate
  if ((millis() - g_prev_control_time) >= 20){//(1000 / COMMAND_RATE)
    motor1.updateSpeed(encoder1.position);
    motor2.updateSpeed(encoder2.position);
    motor3.updateSpeed(encoder3.position);
    motor4.updateSpeed(encoder4.position);
    moveBase();
//test:    
//    pwm1=motor1_pid.compute(constrain(20, -MAX_RPM, MAX_RPM), motor1.rpm);
//    motor1.spin(pwm1);
//    pwm2=motor2_pid.compute(constrain(-20, -MAX_RPM, MAX_RPM), motor2.rpm);
//    motor2.spin(pwm2);
//    pwm3=motor3_pid.compute(constrain(20, -MAX_RPM, MAX_RPM), motor3.rpm);
//    motor3.spin(pwm3);
//    pwm4=motor4_pid.compute(constrain(-20, -MAX_RPM, MAX_RPM), motor4.rpm);
//    motor4.spin(pwm4);
    
    g_prev_control_time = millis();
  }
  //this block publishes velocity based on defined rate
  if ((millis() - g_publish_vel_time) >= (1000 / VEL_PUBLISH_RATE)){    
    publishVelocities();

//    Serial.println(String("1posi:")+encoder1.position);
//    Serial.println(String("1rpm:")+motor1.rpm);
//    Serial.println(String("1pwm:")+pwm1);
//    Serial.println(String("2posi:")+encoder2.position);
//    Serial.println(String("2rpm:")+motor2.rpm);
//    Serial.println(String("2pwm:")+pwm2);
    g_publish_vel_time = millis();
  }
  //this block publishes the IMU data based on defined rate
  if ((millis() - g_prev_imu_time) >= (1000 / IMU_PUBLISH_RATE)){
    //sanity check if the IMU exits
    if (g_is_first){
      checkIMU_zzz();
    }else{
      //publish the IMU data
      publishIMU_zzz();
    }
    g_prev_imu_time = millis();
  }
  if ((millis() - g_prev_led_blink_time) >= (500 )){
    if(led_blink==0){
      led_blink=1;
    }else{
      led_blink=0;
    }
    digitalWrite(PC13, led_blink);
    g_prev_led_blink_time = millis();
  } 
  if ((millis() - g_prev_imu_check_time) >= (100 )){    
    //checkIMU_zzz();
    g_prev_imu_check_time = millis();
  }
  


  //this block displays the encoder readings. change DEBUG to 0 if you don't want to display
  if(DEBUG){
    if ((millis() - g_prev_debug_time) >= (1000 / DEBUG_RATE)){
      
      g_prev_debug_time = millis();
    }
  }
  //call all the callbacks waiting to be called
  nh.spinOnce();
}
void PIDCallback(const zr_movebase_msgs::PID& pid)
{
  //callback function every time PID constants are received from lino_pid for tuning
  //this callback receives pid object where P,I, and D constants are stored
  pid_p=pid.p;
  pid_i=pid.i;
  pid_d=pid.d;
  //printDebug((float)pid_p,(float)pid_i,(float)pid_d,(float)0.0);//==0??
  motor1_pid.updateConstants(pid.p, pid.i, pid.d);
  motor2_pid.updateConstants(pid.p, pid.i, pid.d);
  motor3_pid.updateConstants(pid.p, pid.i, pid.d);
  motor4_pid.updateConstants(pid.p, pid.i, pid.d);
}

void commandCallback(const geometry_msgs::Twist& cmd_msg)
{
  //callback function every time linear and angular speed is received from 'cmd_vel' topic
  //this callback function receives cmd_msg object where linear and angular speed are stored
  if(ros_msg_ctrl){
  g_req_linear_vel_x = cmd_msg.linear.x;
  g_req_linear_vel_y = cmd_msg.linear.y;
  g_req_angular_vel_z = -cmd_msg.angular.z;//fanle??
  }
  g_prev_command_time = millis();
}

随便来个小车实况图，没有梳妆，显得凌乱了些。

演示：

安装麦轮，底盘遥控演示

视频

安装普通轮，小车配上雷达，树莓派安装ROS 自动导航控制的演示

视频

总结：

用ROS框架的大脑驱动廉价模块搭建成的低成本车，赋予Ta灵魂，也基本做到"辞能达意“了。
机器人得有个名字，就叫Ta”晶意智能“车吧。

不满意小车的表现怎么办？多添加些其他传感器？
让我们深入研究ROS，优化算法，用智慧补足短板吧。