change driver_lidar_leishen_chx. not complete.

src/driver/driver_lidar_leishen_chx/leishenchx.cpp

@@ -33,6 +33,28 @@ leishenchx::leishenchx(char * strmemname,double froll,double finclinationang_xax
         cos_list[j] = cos(j * 0.01 * DEG_TO_RAD);
     }
 
+
+    frame_id = std::string("velodyne");
+    lidar_ip_string = std::string("192.168.1.200");
+    add_multicast = false;
+    pcl_type = true;
+    lidar_type = std::string("ch128x1");
+    group_ip_string = std::string("224.1.1.2");
+    msop_udp_port = nDataPort;
+    difop_udp_port = nDevPort;
+    min_range = 0.1;  //0.5
+    max_range = 150.0;
+    angle_disable_min = 0.0;
+    angle_disable_max = 0.0;
+    horizontal_angle_resolution = 0.2;
+    pointcloud_topic = std::string("lslidar_point_cloud");
+    publish_laserscan = false;
+    channel_num = 8;
+    echo_num = 0;
+    dump_file = "";
+    packet_rate = 6737.0;
+    use_time_service = false;
+
     initialize();
 
     mplidarudp = new lidarudp(nDataPort);
@@ -131,8 +153,8 @@ bool leishenchx::initialize()
         ch16x1_cos_theta_2[k] = cos((k % 4) * (-0.17) * DEG_TO_RAD);
     }
 
-    point_cloud_xyzirt_->header.frame_id = frame_id;
-    point_cloud_xyzirt_->height = 1;
+//    point_cloud_xyzirt_->header.frame_id = frame_id;
+//    point_cloud_xyzirt_->height = 1;
 
     point_cloud_xyzi_->header.frame_id = frame_id;
     point_cloud_xyzi_->height = 1;
@@ -772,6 +794,17 @@ int leishenchx::convertCoordinate(struct Firing &lidardata) {
     double sin_H_xita;
     double cos_xita_F;
     double sin_xita_F;
+
+//    static int nprint = 0;
+//    if(nprint<20000)
+//    {
+//        nprint++;
+//        std::cout<<"az: "<<lidardata.azimuth<<std::endl;
+//    }
+
+    static int noldaz = 3000;
+
+
     if (lidar_type == "ch128x1" || lidar_type == "ch128s1" || lidar_type == "cx128s2") {
         _R_ = cos_theta_2[lidardata.vertical_line] * cos_theta_1[lidardata.vertical_line] *
                   cos_list[int(lidardata.azimuth * 0.5)] -
@@ -920,8 +953,32 @@ int leishenchx::convertCoordinate(struct Firing &lidardata) {
         std::cout<<"lidar type error,please reset param in xml file "<<std::endl;
 //        ROS_WARN_ONCE("lidar type error,please reset param in launch file");
     }
+
+
     //add point
     if (pcl_type) {
+        static int g_seq = 0;
+        if((lidardata.azimuth - noldaz)<-9000)
+        {
+            std::cout<< std::chrono::system_clock::now().time_since_epoch().count()/1000000 <<" share point cloud."<<std::endl;
+            ProduceCloud(point_cloud_xyzi_);
+            std::cout<<" produce. "<<std::endl;
+            //share point cloud.
+
+            pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+
+            point_cloud->header.frame_id = "velodyne";
+            point_cloud->height = 1;
+            point_cloud->header.stamp = static_cast<uint64_t>(std::chrono::system_clock::now().time_since_epoch().count()/1000);
+            point_cloud->width = 0;
+            point_cloud->header.seq =g_seq;
+            g_seq++;
+            point_cloud_xyzi_ = point_cloud;
+        }
+
+        noldaz =  lidardata.azimuth;
+
         pcl::PointXYZI point_xyzi;
         point_xyzi.x = x;
         point_xyzi.y = y;

src/driver/driver_lidar_leishen_chx/leishenchx.h

@@ -44,6 +44,7 @@ static const double big_angle_ch16x1[4] = {-1.0,0.0,1.0,2.0};
 
 
 /*    static const double scan_mirror_altitude[4] = {
+ *
             -0.0,
             0.005759586531581287,
             0.011693705988362009,

src/driver/driver_lidar_leishen_chx/main.cpp

@@ -1,8 +1,233 @@
 #include <QCoreApplication>
 
+
+#include "leishenchx.h"
+
+#include <getopt.h>
+#include <yaml-cpp/yaml.h>   //if no this,sudo apt-get install libyaml-cpp-dev
+
+
+
+
+static char gstr_memname[256];
+static char gstr_rollang[256];
+static char gstr_inclinationang_yaxis[256];  //from y axis
+static char gstr_inclinationang_xaxis[256];  //from x axis
+//char gstr_hostip[256];
+static char gstr_port[256];
+static char gstr_devport[256];
+static char gstr_yaml[256];
+static char gstr_modulename[256];
+static char gstr_devmode[256];
+
+/**
+ * @brief print_useage
+ */
+void print_useage()
+{
+    std::cout<<" -n --modulename $modulename : set module name. eq.  -n lidarleft"<<std::endl;
+    std::cout<<" -m --memname $memname : share memory name. eq.  -m lidar_pc"<<std::endl;
+    std::cout<<" -r --rollang $rollang : roll angle. eq.  -r 10.0"<<std::endl;
+    std::cout<<" -x --inclinationang_xaxis $inclinationang_xaxis : inclination angle from x axis. eq.  -x 0.0"<<std::endl;
+    std::cout<<" -y --inclinationang_yaxis $inclinationang_yaxis : inclination angle from y axis. eq.  -y 0.0"<<std::endl;
+    //    std::cout<<" -o --hostip $hostip : host ip. eq.  -o 192.168.1.111"<<std::endl;
+    std::cout<<" -p --port $port : port . eq.  -p 2368"<<std::endl;
+    std::cout<<" -d --devport $port : port . eq.  -d 2369"<<std::endl;
+    std::cout<<" -f --devport $port : port . eq.  -f 0"<<std::endl;
+    std::cout<<" -s --setyaml $yaml : port . eq.  -s rs1.yaml"<<std::endl;
+    std::cout<<" -h --help print help"<<std::endl;
+}
+
+int  GetOptLong(int argc, char *argv[]) {
+    int nRtn = 0;
+    int opt; // getopt_long() 的返回值
+    int digit_optind = 0; // 设置短参数类型及是否需要参数
+    (void)digit_optind;
+
+    // 如果option_index非空，它指向的变量将记录当前找到参数符合long_opts里的
+    // 第几个元素的描述，即是long_opts的下标值
+    int option_index = 0;
+    // 设置短参数类型及是否需要参数
+    const char *optstring = "n:m:r:x:y:p:d:f:s:h";
+
+    // 设置长参数类型及其简写，比如 --reqarg <==>-r
+    /*
+    struct option {
+             const char * name;  // 参数的名称
+             int has_arg; // 是否带参数值，有三种：no_argument， required_argument，optional_argument
+             int * flag; // 为空时，函数直接将 val 的数值从getopt_long的返回值返回出去，
+                     // 当非空时，val的值会被赋到 flag 指向的整型数中，而函数返回值为0
+             int val; // 用于指定函数找到该选项时的返回值，或者当flag非空时指定flag指向的数据的值
+        };
+    其中：
+        no_argument(即0)，表明这个长参数不带参数（即不带数值，如：--name）
+            required_argument(即1)，表明这个长参数必须带参数（即必须带数值，如：--name Bob）
+            optional_argument(即2)，表明这个长参数后面带的参数是可选的，（即--name和--name Bob均可）
+     */
+    static struct option long_options[] = {
+        {"modulename", required_argument, NULL, 'n'},
+        {"memname", required_argument, NULL, 'm'},
+        {"rollang", required_argument, NULL, 'r'},
+        {"inclinationang_xaxis", required_argument, NULL, 'x'},
+        {"inclinationang_yaxis", required_argument, NULL, 'y'},
+        {"port", required_argument, NULL, 'p'},
+        {"devport", required_argument, NULL, 'd'},
+        {"devmode", required_argument, NULL, 'f'},
+        {"setyaml", required_argument, NULL, 's'},
+        {"help",  no_argument,       NULL, 'h'},
+        //       {"optarg", optional_argument, NULL, 'o'},
+        {0, 0, 0, 0}  // 添加 {0, 0, 0, 0} 是为了防止输入空值
+    };
+
+    while ( (opt = getopt_long(argc,
+                              argv,
+                              optstring,
+                              long_options,
+                              &option_index)) != -1) {
+        //        printf("opt = %c\n", opt); // 命令参数，亦即 -a -b -n -r
+        //        printf("optarg = %s\n", optarg); // 参数内容
+        //        printf("optind = %d\n", optind); // 下一个被处理的下标值
+        //        printf("argv[optind - 1] = %s\n",  argv[optind - 1]); // 参数内容
+        //        printf("option_index = %d\n", option_index);  // 当前打印参数的下标值
+        //        printf("\n");
+        switch(opt)
+        {
+        case 'n':
+            strncpy(gstr_modulename,optarg,255);
+            break;
+        case 'm':
+            strncpy(gstr_memname,optarg,255);
+            break;
+        case 'r':
+            strncpy(gstr_rollang,optarg,255);
+            break;
+        case 'x':
+            strncpy(gstr_inclinationang_xaxis,optarg,255);
+            break;
+        case 'y':
+            strncpy(gstr_inclinationang_yaxis,optarg,255);
+            break;
+            //        case 'o':
+            //            strncpy(gstr_hostip,optarg,255);
+            //            break;
+        case 'p':
+            strncpy(gstr_port,optarg,255);
+            break;
+        case 'd':
+            strncpy(gstr_devport,optarg,255);
+            break;
+        case 'f':
+            strncpy(gstr_devmode,optarg,255);
+            break;
+        case 's':
+            strncpy(gstr_yaml,optarg,255);
+            break;
+        case 'h':
+            print_useage();
+            nRtn = 1; //because use -h
+            break;
+        default:
+            break;
+        }
+
+    }
+
+    return nRtn;
+}
+
+/**
+ * @brief decodeyaml
+ * @param stryaml yaml path
+ */
+void decodeyaml(const char * stryaml)
+{
+    YAML::Node config;
+    try
+    {
+        config = YAML::LoadFile(stryaml);
+    }
+    catch(YAML::BadFile e)
+    {
+        qDebug("load yaml error.");
+        return;
+    }
+
+
+    if(config["memname"])
+    {
+        strncpy(gstr_memname,config["memname"].as<std::string>().data(),255);
+    }
+    if(config["rollang"])
+    {
+        strncpy(gstr_rollang,config["rollang"].as<std::string>().data(),255);
+    }
+    if(config["inclinationang_xaxis"])
+    {
+        strncpy(gstr_inclinationang_xaxis,config["inclinationang_xaxis"].as<std::string>().data(),255);
+    }
+    if(config["inclinationang_yaxis"])
+    {
+        strncpy(gstr_inclinationang_yaxis,config["inclinationang_yaxis"].as<std::string>().data(),255);
+    }
+    //    if(config["hostip"])
+    //    {
+    //        strncpy(gstr_hostip,config["hostip"].as<std::string>().data(),255);
+    //    }
+    if(config["port"])
+    {
+        strncpy(gstr_port,config["port"].as<std::string>().data(),255);
+    }
+
+    if(config["devport"])
+    {
+        strncpy(gstr_devport,config["devport"].as<std::string>().data(),255);
+    }
+
+    if(config["devmode"])
+    {
+        strncpy(gstr_devmode,config["port"].as<std::string>().data(),255);
+    }
+
+    //    std::cout<<gstr_memname<<std::endl;
+    //    std::cout<<gstr_rollang<<std::endl;
+    //    std::cout<<gstr_inclinationang_xaxis<<std::endl;
+    //    std::cout<<gstr_inclinationang_yaxis<<std::endl;
+    //    std::cout<<gstr_hostip<<std::endl;
+    //    std::cout<<gstr_port<<std::endl;
+}
+
+
+
 int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
 
+    snprintf(gstr_memname,255,"lidar_pc");
+    snprintf(gstr_rollang,255,"270.0");//这个角度要根据安装情况进行校正！！！！！！！！
+    snprintf(gstr_inclinationang_xaxis,255,"0.0");
+    snprintf(gstr_inclinationang_yaxis,255,"0");
+    //    snprintf(gstr_hostip,255,"192.168.1.102");
+    snprintf(gstr_port,255,"2368");//默认端口号
+    snprintf(gstr_devport,255,"2369");//默认端口号
+    snprintf(gstr_devmode,255,"0");//默认端口号
+    snprintf(gstr_yaml,255," ");
+    snprintf(gstr_modulename,255,"driver_lidar_leishenc16");
+
+    int nRtn = GetOptLong(argc,argv);
+    if(nRtn == 1)  //show help,so exit.
+    {
+        return 0;
+    }
+
+    if(strnlen(gstr_yaml,255)>1)
+    {
+        decodeyaml(gstr_yaml);
+    }
+
+    leishenchx * pleishenchx = new leishenchx(gstr_memname,atof(gstr_rollang),atof(gstr_inclinationang_xaxis),
+                                             atof(gstr_inclinationang_yaxis),atoi(gstr_devmode),static_cast<unsigned short>(atoi(gstr_port)),
+                                             static_cast<unsigned short>(atoi(gstr_devport)));
+    (void)pleishenchx;
+
     return a.exec();
 }