ros 节点实现简易超声雷达串口通讯 模拟出激光雷达消息

ros 节点实现简易超声雷达串口通讯 模拟出激光雷达消息

#include 
//ROS已经内置了的串口包
#include 
#include 
#include 
#include                   //包含boost库函数
//#include 
//#include 
#include 
#include 

using namespace boost::asio;           //定义一个命名空间，用于后面的读写操作
using namespace std;
serial::Serial ser; //声明串口对象
unsigned char buf[1];                      //定义字符串长度
unsigned char data[512];//64(fenbianlv)*2(Hz)*2(byte)*2(rongyu)=512
unsigned int dn=0;
unsigned int i=0,j=0;
unsigned char insertId=0;
 char flag=0;
const unsigned int num_readings = 64;
 unsigned int data_zx[num_readings] ;
 unsigned int data_fx[num_readings] ;
ros::Publisher scan_pub ;



 double laser_frequency = 2;
 double ranges[num_readings];
 double intensities[num_readings];

 int count = 0;
void transData(bool f){
ROS_INFO("pub------------------------");

  //generate some fake data for our laser scan
if(f){
  for(unsigned int i = 0; i < num_readings; ++i){
   ranges[i] = data_zx[i];
   //intensities[i] = 100 + count;
  }
}else{
  for(unsigned int i = 0; i < num_readings; ++i){
   ranges[i] = data_fx[num_readings-i];
   //intensities[i] = 100 + count;
  }
}

  ros::Time scan_time = ros::Time::now();

  //populate the LaserScan message
//data_zx[i]
  sensor_msgs::LaserScan scan;

  scan.header.stamp = scan_time;
  scan.header.frame_id = "footprint";//"base_link";
  //zuoyou xuanzhuan piancha xiuzheng
  scan.angle_min = -3.14*(f?0.48:0.32);
  scan.angle_max = 3.14*(f?0.32:0.48);
  scan.angle_increment = 3.14 *0.8/ num_readings;
  scan.time_increment = (1 / laser_frequency) / (num_readings);
  scan.range_min = 0.0;
  scan.range_max = 4.0;
  scan.ranges.resize(num_readings);
  scan.intensities.resize(num_readings);

  for(unsigned int i = 0; i < num_readings; ++i){
   scan.ranges[i] = (ranges[i]==0?400:ranges[i])/100;
   scan.intensities[i] = ranges[i]/100+100;//intensities[i];
  }

  scan_pub.publish(scan);
}
int main(int argc, char **argv)
{
  ros::init(argc, argv, "node_hcsr04_serial_lidar");
  //声明节点句柄
  ros::NodeHandle nh;
//发布主题
   scan_pub = nh.advertise("scan", 50);


  io_service iosev;
  serial_port sp(iosev, "/dev/ttyUSB1");         //定义传输的串口
  sp.set_option(serial_port::baud_rate(115200));
  sp.set_option(serial_port::flow_control());
  sp.set_option(serial_port::parity());
  sp.set_option(serial_port::stop_bits());
  sp.set_option(serial_port::character_size(8));
  iosev.run();

  //指定循环的频率
  ros::Rate loop_rate(10);
  while(ros::ok())
  {

    while(1){

      try
      {
        read (sp,buffer(buf));
        data[dn]=buf[0];
        dn++;

if(dn>=127){break;}
        //ROS_INFO("%d", buf[0]);//打印接受到的字符串



      }catch (boost::system::system_error e){
          ROS_ERROR_STREAM("read err ");
          break;
      }
    }
    ROS_INFO("read");
//xieyi
    for(i=0;i<(dn-1);){
      if((data[i]&0xF0)==0xD0){
        //
        ROS_INFO("d0flag=%d",flag);
        if(flag==-1)transData(false);
        flag=1;
        insertId=0;
      }else if((data[i]&0xF0)==0xC0){
        ROS_INFO("c0flag=%d",flag);
        if(flag==1)transData(true);
        flag=-1;
        insertId=0;
      }else{
        if((data[i]&0xF0)>0)
          ROS_INFO("T-%d", (data[i]&0xF0)>>4);

      }
      if(flag==1){
        if((i+1)>4,data[i+1]&0x0f,insertId);

          if(insertId>4)*10+((data[i+1])&0x0f);
          i+=1;
          insertId+=1;
          //continue;
        }else{
          break;
        }
      }
      if(flag==-1){
        if((i+1)>4,data[i+1]&0x0f,insertId);


          if(insertId>4)*10+((data[i+1])&0x0f);
          i+=1;
          insertId+=1;
          //continue;
        }else{
          break;
        }
      }

      i+=1;
    }
    //move to 0
    //if(i>0){
    for(j=0;i