激光雷达slam小车集成总结

激光雷达slam小车集成总结
最近完成了一个激光雷达slam小车，主要还是根据网上教程以及部分淘宝卖家提供的技术支持，在这里简单记录一下整个过程。
小车是直接从网上买的两轮差速驱动的小坦克车，控制芯片是stm32（老板非常不错，还可以探讨技术问题）。关于stm32这部分的控制程序可以和老板探讨，这里只简单介绍一下slam过程中会用到的小车的程序，其他底层程序这里就不记录了。（其实主要是用的老板给的程序我也没改）。
激光雷达用的是淘宝买到的rplidar，这个激光雷达的ros包可以直接在官方下载，直接用官网提供的包就可以直接输出点云数据。
小车和电脑之间通讯是要记录的重点，这里的串口调试调试了好长时间。
小车的建图用的gmapping，导航是ros集成的navigation规划路径，这部分还是单纯当了调包侠，毕竟整个过程时间比较紧张。
实际小车的图片：

移动底盘

小车底盘部分主要作用是最终的执行机构，不承担过多的计算控制部分，通过串行通讯的方式收到运动速度信息（固定时间间隔内期望得到的编码器脉冲数值）然后结合实际转速通过PID对小车的电机的pwm信号进行控制。实际控制中使用的是增量型PI控制，PID控制函数如下：这里面直接在初始情况下就对PID的参数进行了确定（其实是淘宝店老板给的程序中的参数）

void PID_control(){
	pwm_left += 3 * (encoder_left_target - Encoder_Left - last_bias_left_pid) + 5 * (encoder_left_target - Encoder_Left);
	pwm_right += 3 * (encoder_right_target - Encoder_Right - last_bias_right_pid) + 5 * (encoder_right_target - Encoder_Right);
	
	// 
	last_bias_left_pid = encoder_left_target - Encoder_Left;
	last_bias_right_pid = encoder_right_target - Encoder_Right;
	//printf("pwm_left: %d, pwm_right: %d \r\n",pwm_left, pwm_right);
	
	Set_Pwm_pid(pwm_left, pwm_right);
}

这个函数与读取左右轮编码器脉冲数值的函数都在定时器产生的50ms中断中执行。

stm32与ROS通讯

在通讯方面用的就是stm32的串行通讯和ROS的serial。电脑和stm32之间通过一个USB转ttl的转接口进行连接。rosserial的安装这里就不记录了，其他地方的教程很多，基本没什么问题。
关于通讯协议，主要也是参考了网上其他大佬的作品，最终选择了下面的通讯协议：
一次完整的通讯数据可以存放在一个由11个u8类型的数字组成的数组中，数组的第0未和第一位是帧头，内容是连续的两个255，第二位至第五位是左轮信息，第六位至第九位是右轮信息，Enc_sig表示的电机转动方向，Enc_val表示的是在给定时间内电机编码器读到的脉冲数值，这个数可能较大，用三个u8类型的数字存。最后一位是校验位，将前面十位数字累加求和得到的结果。
在通讯的时候，小车的信息发送写在了500ms的中断中，记录500ms内的编码器脉冲数值之后发送出去，接收端是在一个接收中断里面。接收中断的程序如下：

void USART2_IRQHandler(){
	if(USART_GetITStatus(USART2, USART_IT_RXNE) != RESET){
	  USART_ClearITPendingBit(USART2, USART_IT_RXNE);
		uart2_receive_tmp = USART_ReceiveData(USART2);
		mmm += 1;
		usart2_receive_buf[0] = usart2_receive_buf[1];
		usart2_receive_buf[1] = usart2_receive_buf[2];
		usart2_receive_buf[2] = usart2_receive_buf[3];
		usart2_receive_buf[3] = usart2_receive_buf[4];
		usart2_receive_buf[4] = usart2_receive_buf[5];
		usart2_receive_buf[5] = usart2_receive_buf[6];
		usart2_receive_buf[6] = usart2_receive_buf[7];
		usart2_receive_buf[7] = usart2_receive_buf[8];
		usart2_receive_buf[8] = usart2_receive_buf[9];
		usart2_receive_buf[9] = usart2_receive_buf[10];
		usart2_receive_buf[10] = uart2_receive_tmp;
		if((usart2_receive_buf[0] == 0xff) && (usart2_receive_buf[1] == 0xff)){
			u8 check_sum_tmp = 0;
			send_flag = 1;
			check_sum_tmp = usart2_receive_buf[0] + usart2_receive_buf[1] + usart2_receive_buf[2] + usart2_receive_buf[3] + usart2_receive_buf[4] + usart2_receive_buf[5] + usart2_receive_buf[6] + usart2_receive_buf[7] + usart2_receive_buf[8] + usart2_receive_buf[9];
			if(check_sum_tmp == usart2_receive_buf[10]){
				encoder_left_target = (usart2_receive_buf[2] > 0 ? 1 : -1) * ((usart2_receive_buf[3] << 16) + (usart2_receive_buf[4] << 8) + usart2_receive_buf[5]);
				encoder_right_target = (usart2_receive_buf[6] > 0 ? 1 : -1) * ((usart2_receive_buf[7] << 16) + (usart2_receive_buf[8] << 8) + usart2_receive_buf[9]);
			  for(int i = 0; i <11; i++){
				 // printf("got msg: %d\r\n", usart2_receive_buf[i]);
				}
				encoder_left_target = encoder_left_target / 10;
				encoder_right_target = encoder_right_target / 10;
			}
		}
	}
}

在电脑端通过USB转ttl可以连接stm32，连接完成之后需要查看并更改可读写属性

ls -l /dev/ttyUSB*
sudo chmod 666 /dev/ttyUSB0  

关于电脑端rosserial的发送和接收数据的程序，调试了很长时间。这部分主要是订阅cmd_vel话题并且将这个消息转化为固定时间内的编码器的脉冲值，然后通过serial发送出去，然后实时接收小车端发送的数据，计算之后得到里程计和tf变换信息并发布出去。这里面读取小车发送过来的数据使用的是read（）函数，它是将读到的数字存起来，但是每次读取到的数字的个数是不确定的，这里的调试花费了比较多的时间，后来看到其他地方有用readline（）函数的，看起来好像很好用，但是后来并没有时间尝试了，也就先这样了。
整个serial_node，大概画了一个流程框图，大概看看意思就好了。

对于具体的程序，篇幅原因就只记录其中的主要部分。
订阅cmd_vel的回调函数：

void Callback(const geometry_msgs::Twist& cmd_vel){
  ROS_INFO("GOT CMD_VEL MSG");
  for(int i = 0; i < 11; i++){
    writebuf[i] = 0x00;
  }
  float angular_temp;
  float linear_temp;
  linear_temp = 600 * (cmd_vel.linear.x);
  angular_temp = 4000 * (cmd_vel.angular.z);

  float linear_max_limit = cmd_vel_linear_max;
  float angular_max_limit = cmd_vel_angular_max;
  if(linear_temp > linear_max_limit){
    linear_temp = linear_max_limit;
  }
  if(linear_temp < (-1 * linear_max_limit)){
    linear_temp = -1 * linear_max_limit;
  }
  if(angular_temp > angular_max_limit){
    angular_temp = angular_max_limit;
  }
  if(angular_temp < (-1 * angular_max_limit)){
    angular_temp = -1 * angular_max_limit;
  }
  
  int delta_encoder_left_temp = (linear_temp + 0.5 * (WheelDistance * angular_temp));// * encoder_sampling_time / speed_ratio;
  int delta_encoder_right_temp = (linear_temp - 0.5 * (WheelDistance * angular_temp));// * encoder_sampling_time / speed_ratio;

  while(send_update_flag != 0);
  writebuf[0]=writebuf[1]=0xff;
  if(delta_encoder_left_temp >= 0){
    writebuf[2] = 0x01;
  }
  else{
    writebuf[2] = 0x00;
  }
  writebuf[3] = abs(delta_encoder_left_temp)>>16;
  writebuf[4] = (abs(delta_encoder_left_temp)>>8)&0xff;
  writebuf[5] = abs(delta_encoder_left_temp)&0xff;
  if(delta_encoder_right_temp >= 0){
    writebuf[6] = 0x01;
  }
  else{
    writebuf[6] = 0x00;
  }
  writebuf[7] = abs(delta_encoder_right_temp)>>16;
  writebuf[8] = (abs(delta_encoder_right_temp)>>8)&0xff;
  writebuf[9] = abs(delta_encoder_right_temp)&0xff;
  int temp = 0;
  for(int i = 0; i <10; i++){
    temp += writebuf[i];
  }
  writebuf[10] = temp & 0xff;
  send_update_flag = 1;
}

serial配置及接收数据程序主要部分如下，其中处理read（）之后的数据部分感觉写的不是很好，有大佬有新的想法欢迎交流。

  std_msgs::UInt8MultiArray readbuf;
  readbuf.data.resize(11);
  for(int i = 0; i < 11; i++){
    readbuf.data[i] = 0;
  }

  ser.setPort("/dev/ttyUSB0");
  ser.setBaudrate(115200);
  serial::Timeout to = serial::Timeout::simpleTimeout(1000); 
  ser.setTimeout(to); 
  ser.open(); 
 
  stopbuf[0]=0xff;
  stopbuf[1]=0xff;
  for(int i = 2; i < 10; i++){
    stopbuf[i] = 0x00;
  }
  stopbuf[10]=0xfe;

  
  float covariance[36] = {0.01, 0, 0, 0, 0, 0, 
                             0, 0.01, 0, 0, 0, 0, 
                           0, 0, 99999, 0, 0, 0, 
                           0, 0, 0, 99999, 0, 0,
                           0, 0, 0, 0, 99999, 0,
                           0, 0, 0, 0, 0, 0.01};
  for(int i = 0; i < 36; i++){
    odom.pose.covariance[i] = covariance[i];
  }
  ros::Rate loop_rate(20);
  if(ser.isOpen()){
    ROS_INFO("serial initialized!");
  }
  
  double delta_encoder_left, delta_encoder_right, delta_x, delta_y;
  ROS_INFO("serial_node initialized");
  int empty_flag = 0;
  double send_time;
  int num = 0, bigNum = 0;    
  int flag=0;//0--inputing,1--findingHead
  int p = 0;
  int continue_flag; //0--last msg is over, 1-- this msg should be added to last msg
  std_msgs::UInt8MultiArray  s_data;
  s_data.data.resize(100);
  while(ros::ok()){
    if(send_update_flag == 1){
      ser.write(writebuf, 11);
      ROS_INFO_STREAM("Writing to serial port");
      send_update_flag = 0;
      send_time = ros::Time::now().toSec();
      ROS_INFO("send_time: %f", send_time);
    }
    if(ros::Time::now().toSec() - send_time > 1.0){
      ser.write(stopbuf, 11);
    }
      

      std_msgs::UInt8MultiArray  serial_data;
        p = ser.available();
        //ROS_INFO("P: %d",p);
        ser.read(serial_data.data, p);
      if(p!=0){
        for(int i = 0; i < p; i++){
          s_data.data[bigNum+i] = serial_data.data[i];
        }
        int n = bigNum;
        while (bigNum<(n+p)&&num<11){
           readbuf.data[num++]=s_data.data[bigNum];
           if(continue_flag == 0){
             if (flag==0){
                if (s_data.data[bigNum]==255){
                   flag=1;
                }
             }
             else{//flag==1
                if (s_data.data[bigNum]==255){
                   if(num!=2){
                     readbuf.data[0]=255;
                     readbuf.data[1]=255;
                     num=2;
                     continue_flag = 1;
	           }
                   flag=0;
                }
             }
           }
           bigNum++;
           //ROS_INFO("bigNum: %d", bigNum);
        }
      }

    
    if(num >= 11){
      for(int i = 0; i < 11; i++){
        ROS_INFO("readbuf[%d]:%d",i, readbuf.data[i]);
      }
      //ROS_INFO("M >= 11");
      if((readbuf.data[0] == 0xff) && (readbuf.data[1] == 0xff)){
        uint8_t check_sum = 0;
        for(int i = 0; i < 10; i++){
          check_sum += readbuf.data[i];
        }
        

        if(readbuf.data[10] == check_sum){
          ROS_INFO("GOT RIGHT MSG");
          delta_encoder_right = (readbuf.data[2] > 0? 1:-1) * ((readbuf.data[3] << 16) + (readbuf.data[4] << 8) + readbuf.data[5]);
          delta_encoder_left = (readbuf.data[6] > 0? 1:-1) * ((readbuf.data[7] << 16) + (readbuf.data[8] << 8) + readbuf.data[9]);
          ROS_INFO("delta_encoder_right: %f,delta_encoder_left: %f", delta_encoder_right, delta_encoder_left);
          float delta_d_left, delta_d_right;
          delta_d_left = delta_encoder_left * speed_ratio;
          delta_d_right = delta_encoder_right * speed_ratio;
          float delta_d = (delta_d_left + delta_d_right) * 0.5;
          float delta_theta = (delta_d_right - delta_d_left) / WheelDistance;
          delta_x = delta_d * cos(oriention + delta_theta * 0.5);
          delta_y = delta_d * sin(oriention + delta_theta * 0.5);
          position_x += delta_x;
          position_y += delta_y; 
          oriention += delta_theta;
          //ROS_INFO("position X: %f,position Y: %f, oriention : %f", position_x, position_y, oriention);
          velocity_linear = delta_d / encoder_sampling_time;
          velocity_angular = delta_theta / encoder_sampling_time;
          receive_flag = 1;
          for(int i = 0; i < 11; i++){
            readbuf.data[i] = 0;
          }
        }
      }
      num = 0;
      bigNum = 0;
      continue_flag = 0;
      for(int i = 0; i < 100; i++){
        s_data.data[i] = 0;
      }
      //ROS_INFO("num = %d", num);
    }
    if(receive_flag == 1){
      odom_quat = tf::createQuaternionMsgFromYaw(oriention);
      odom_trans.header.stamp = ros::Time::now();
      odom_trans.header.frame_id = "odom";
      odom_trans.child_frame_id = "base_link";
      odom_trans.transform.translation.x = position_x;
      odom_trans.transform.translation.y = position_y;
      odom_trans.transform.translation.z = 0.0;  
      odom_trans.transform.rotation = odom_quat;
 
      odom_broadcaster.sendTransform(odom_trans);

      odom.header.stamp = ros::Time::now();
      odom.header.frame_id = "odom";
      odom.child_frame_id = "base_link";
      odom.pose.pose.position.x = position_x;
      odom.pose.pose.position.y = position_y;
      odom.pose.pose.position.z = 0.0;
      odom.pose.pose.orientation = odom_quat;  
      odom.twist.twist.linear.x = velocity_linear;
      odom.twist.twist.angular.z = velocity_angular;
    
      odom_pub.publish(odom);

      wheelspeed_left.data = delta_encoder_left * speed_ratio / encoder_sampling_time;
      wheelspeed_right.data = delta_encoder_right * speed_ratio / encoder_sampling_time;
      wheelspeed_left_pub.publish(wheelspeed_left);
      wheelspeed_right_pub.publish(wheelspeed_right);
      
      //ROS_INFO("once");

    }
    ros::spinOnce();
  }
  
  loop_rate.sleep();
  return 0;

建图

在通讯完成之后，建图过程中需要用键盘的方向键控制小车的移动，这里需要注意的是turtlesim中的turtle_teleop_key节点发布的是/turtle1/cmd_vel话题的消息，需要进行一次重映射，将这个话题映射到cmd_vel。
在建图之前，要根据激光雷达和小车的实际安装关系更改gmapping的launch文件中的tf变换关系，我这里是激光雷达与小车的前方成九十度，于是我在launch文件中改成：

<node pkg="tf" type="static_transform_publisher" name="base_to_laser_broadcaster" args="0 0 0 1.570796 0 0 /base_link /laser 100"/>

然后把gmapping打开，rplidar打开，自己写的serial节点打开，turtle_teleop_key打开，控制小车动起来，再打开rviz，添加/map和TF就可以在rviz中实时看到建图和小车运动了
这里需要注意的是，如果建图效果不好，需要重新回去标定一下ros中写的小车的参数和小车实际的参数，比较好的办法是，设好程序让小车前进一米，然后看在rviz中小车移动的距离，设好程序让小车转360°，看小车在rviz中转动的距离，存在偏差的时候要微调自己当初设置的参数。

导航

导航这边主要就是完全根据教程调包了，其他地方的教程写的也比较清楚，这里就不再重复了，最后放一张导航时候的图好了
差不多也就写完了，搞这样一个小车看起来挺简单，但是自己实际动手做一做才会发现中间的各种坑，不过坑踩多了，也就不觉得是坑了。