ceres wheel optimization with two ratio and front angle weight.

.gitignore

@@ -37,3 +37,4 @@ src/findwheel/data/*
 *vscode*
 src/.vscode/*
 path.*
+cloud/*

src/findwheel/scripts/bag.launch

@@ -1,5 +1,5 @@
 <launch>
   
-  <node name="bag_play" pkg="rosbag" type="play" args="--pause -r 1 --bags=$(find FindWheel)/bag/9a81.bag --topics /scan2 /scan3 /scan4 /scan5 /scan6 /scan7 /scan8 /scan9" output="screen"/>
+  <node name="bag_play" pkg="rosbag" type="play" args="--pause -r 1 --bags=$(find FindWheel)/bag/wj_scan.bag --topics /scan1 /scan2 /scan3 /scan4 /scan5 /scan6 /scan7 /scan8 /scan9 /scan10 /scan11 /scan12 /scan13 /scan14" output="screen"/>
 
-</launch>
+</launch>

src/findwheel/src/FenceController.cpp

@@ -160,8 +160,15 @@ void FenceController::cloudMergeUpdate(FenceController* fc)
                 fc->p_merged_cloud->operator+=(*cloud);
             }
         }
+        // static int save_count = 0;
         //std::cout<<"get cloud...1"<<std::endl;
         if(fc->p_merged_cloud->size() > 0) {
+            
+            // save_count++;
+            // char cloud_filename[255] = {0};
+            // sprintf(cloud_filename, "/home/youchen/Documents/measure/MainStructure/elecfence_ws/cloud/cloud_%d.txt", save_count);
+            // fc->save_cloud_txt(cloud_filename, fc->p_merged_cloud);
+
             for (int i = 0; i < fc->p_region_workers.size(); ++i) {
                 fc->p_region_workers[i]->update_cloud(fc->p_merged_cloud);
             }

src/findwheel/src/Lidar.cpp

@@ -25,7 +25,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr  laserScan2Cloud(const sensor_msgs::LaserSca
             float x = mat(0, 0);
             float y = mat(1, 0);
 
-            if(x<minx||x>maxx||y<miny||y>maxy)
+            if(x<minx||x>maxx||y<miny||y>maxy||msg.intensities[i]<50||msg.intensities[i]>1500)
             {
                 angle += msg.angle_increment;
                 continue;
@@ -115,11 +115,11 @@ void Lidar::cloud_callback(const sensor_msgs::LaserScan::ConstPtr& msg,Lidar* pL
         char buf[255] = {0};
         if (pLidar->m_lidar_param.topic().size() > 0 && pLidar->m_lidar_param.topic().at(0) == '/')
         {
-            sprintf(buf, "./cloud_%s.txt", pLidar->m_lidar_param.topic().c_str() + 1);
+            sprintf(buf, "./cloud_%s_%d.txt", pLidar->m_lidar_param.topic().c_str() + 1, msg->header.seq);
         }
         else
         {
-            sprintf(buf, "./cloud_%s.txt", pLidar->m_lidar_param.topic().c_str());
+            sprintf(buf, "./cloud_%s_%d.txt", pLidar->m_lidar_param.topic().c_str(), msg->header.seq);
         }
         pLidar->m_mutex.lock();
         SaveTxtCloud(pLidar->m_cloud, buf);
@@ -149,4 +149,4 @@ bool Lidar::GetCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, int timeout_mil
         return false;
     else
         return true;
-}
+}

src/findwheel/src/detect_wheel_ceres.cpp

@@ -74,6 +74,13 @@ public:
         T length = variable[3];
         T width = variable[4];
         T theta_front = variable[5];
+        T theta_front_weight = T(0.006);
+        T wheel_length_img_ratio = variable[6];
+        T wheel_width_length_ratio = variable[7];
+        // [1/3, 5/3]
+        T limited_wheel_length_img_ratio = T(4.0/5.0) / (T(1) + ceres::exp(T(-wheel_length_img_ratio))) + T(4.0/5.0);
+        // [0.2, 0.8]
+        T limited_wheel_length_ratio = T(0.6) / (T(1) + ceres::exp(T(-wheel_width_length_ratio))) + T(0.2);
 
         //整车旋转
         Eigen::Rotation2D<T> rotation(theta);
@@ -107,11 +114,11 @@ public:
             const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_front;
             //旋转
             const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
-            interpolator.Evaluate(point_rotation(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
-                                  point_rotation(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
+            interpolator.Evaluate(point_rotation(1, 0) * limited_wheel_length_img_ratio / limited_wheel_length_ratio* m_scale + rows / 2.0 + 0.5 + T(kPadding),
+                                  point_rotation(0, 0) * limited_wheel_length_img_ratio * m_scale + cols / 2.0 + 0.5 + T(kPadding),
                                   &residual[i]);
+            residual[i] += theta_front*theta_front_weight;
             costs[0] += residual[i];
-
         }
         //右前轮
         int right_front_num = m_right_front_cloud.size();
@@ -122,9 +129,10 @@ public:
             const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_front;
             //旋转
             const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
-            interpolator.Evaluate(point_rotation(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
-                                  point_rotation(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
+            interpolator.Evaluate(point_rotation(1, 0) * limited_wheel_length_img_ratio / limited_wheel_length_ratio* m_scale + rows / 2.0 + 0.5 + T(kPadding),
+                                  point_rotation(0, 0) * limited_wheel_length_img_ratio * m_scale + cols / 2.0 + 0.5 + T(kPadding),
                                   &residual[left_front_num + i]);
+            residual[left_front_num + i] += theta_front*theta_front_weight;
             costs[1] += residual[left_front_num+i];
         }
         //左后轮
@@ -136,8 +144,8 @@ public:
             //减去经过车辆旋转计算的左前中心
             const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_rear;
 
-            interpolator.Evaluate(tanslate_point(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
-                                  tanslate_point(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
+            interpolator.Evaluate(tanslate_point(1, 0) * limited_wheel_length_img_ratio / limited_wheel_length_ratio * m_scale + rows / 2.0 + 0.5 + T(kPadding),
+                                  tanslate_point(0, 0) * limited_wheel_length_img_ratio * m_scale + cols / 2.0 + 0.5 + T(kPadding),
                                   &residual[left_front_num + right_front_num + i]);
             costs[2] += residual[left_front_num + right_front_num + i];
         }
@@ -150,8 +158,8 @@ public:
             //减去经过车辆旋转计算的左前中心
             const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_rear;
 
-            interpolator.Evaluate(tanslate_point(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
-                                  tanslate_point(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
+            interpolator.Evaluate(tanslate_point(1, 0) * limited_wheel_length_img_ratio / limited_wheel_length_ratio * m_scale + rows / 2.0 + 0.5 + T(kPadding),
+                                  tanslate_point(0, 0) * limited_wheel_length_img_ratio * m_scale + cols / 2.0 + 0.5 + T(kPadding),
                                   &residual[left_front_num + right_front_num + left_rear_num + i]);
             costs[3] += residual[left_front_num + right_front_num + left_rear_num + i];
         }
@@ -202,15 +210,16 @@ bool detect_wheel_ceres::update_mat(int rows, int cols)
         cv::Size size(n+2,n+2);
         cv::Rect rect(center-cv::Point(n/2,n/2),size);
         double x = n / double(L);
-        double sigma = 0.06;
+        double sigma = 0.08;
         double miu = 0.0;
         double scale = 0.02;
-        double translation = -0.05;
+        double translation = -0.03;
         double gauss_value = (scale/(sqrt(2*M_PI)*sigma))*exp(-pow(x-miu, 2)/(2*sigma*sigma))+translation;
-        double quadratic_value = x-0.08;
+        double quadratic_value = std::max(x-0.1, 0.03);
         // LOG(INFO) << rect.tl().x<<", "<<rect.tl().y<<", "<<rect.br().x<<", "<<rect.br().y;
         // 对内外分别取色
         cv::rectangle(map,rect,std::max(gauss_value, quadratic_value));
+        // cv::circle(map, center, n/2, std::max(gauss_value, quadratic_value));
 
         // if(n<K*2){
         //     cv::rectangle(map,rect,1.0*float(K*2-n)/float(L));
@@ -226,14 +235,12 @@ bool detect_wheel_ceres::update_mat(int rows, int cols)
 
 detect_wheel_ceres::detect_wheel_ceres()
 {
-    int cols=800;
-    int rows=200;
+    int cols=500;
+    int rows=500; // 200
     update_mat(rows, cols);
 }
 detect_wheel_ceres::~detect_wheel_ceres(){}
 
-
-
 bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> cloud_vec, Detect_result &detect_result)
 {
     //清理点云
@@ -349,21 +356,24 @@ bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> clou
     Loss_result loss_result;
     // 平均误差值，用于获取最小整体平均误差
     double avg_loss = 100;
-    // 定义图像列数，控制图像大小
-    int map_cols = 800;
-    // 优化后图像行数，用于保存优化后结果图像
-    int optimized_map_rows = 200;
+    // // 定义图像列数，控制图像大小
+    // int map_cols = 800;
+    // // 优化后图像行数，用于保存优化后结果图像
+    // int optimized_map_rows = 200;
     // 优化结果
     bool solve_result = false;
     double total_solve_time = 0;
-    for (int j = 2; j < 4; j++)
-    {
-        // double input_vars[] = {detect_result.cx, detect_result.cy, detect_result.theta, detect_result.wheel_base, detect_result.width, detect_result.front_theta};
-        double map_rows = map_cols * 1.0 / j ;
-        update_mat(map_rows, map_cols);
+    bool stop_sign = false;
+    // for (int j = 2; j < 4&&!stop_sign; j++)
+    // {
+    //     // double input_vars[] = {detect_result.cx, detect_result.cy, detect_result.theta, detect_result.wheel_base, detect_result.width, detect_result.front_theta};
+    //     double map_rows = map_cols * 1.0 / j ;
+    //     update_mat(map_rows, map_cols);
+
+    // !!! 将车轮比例添加到优化模型中
         Detect_result t_detect_result = detect_result;
         // 寻找最小loss值对应的初始旋转角
-        for (int i = -5; i < 6; i++)
+        for (int i = -3; i < 4&&!stop_sign; i+=6)
         {
             t_detect_result.theta = (-angle_x + i) * M_PI / 180.0;
             // printf("double x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",input_vars[0],input_vars[1],input_vars[3],input_vars[4],input_vars[2],input_vars[5]);
@@ -376,10 +386,11 @@ bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> clou
                 t_detect_result.front_theta *= (-M_PI) / 180.0;
             }
             auto t1=std::chrono::system_clock::now();
-            bool current_result = Solve(t_detect_result, t_loss_result);
+            bool current_result = Solve(t_detect_result, t_loss_result, true);
             auto t2=std::chrono::system_clock::now();
             auto duration=t2-t1;
-            double second=std::chrono::duration_cast<std::chrono::milliseconds>(duration).count()/1000.0;
+            static double second=0.0;
+            second=std::chrono::duration_cast<std::chrono::milliseconds>(duration).count()/1000.0;
             total_solve_time+=second;
             // std::cout<<" time "<<second<<std::endl;
             // std::cout<<"current_result: "<<current_result<<std::endl;
@@ -393,23 +404,29 @@ bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> clou
                 detect_result = t_detect_result;
                 solve_result = current_result;
                 loss_result = t_loss_result;
-                optimized_map_rows = map_rows;
+                // optimized_map_rows = map_rows;
                 calc_count++;
+                // // 新增，优化时间足够长则认为已找到正确结果
+                // if(second > 0.02)
+                // {
+                //     stop_sign = true;
+                //     break;
+                // }
             }
         }
-    } 
-    std::cout<<"solve time "<<total_solve_time<<std::endl;
+    // } 
+    // std::cout<<"solve time "<<total_solve_time<<std::endl;
     // LOG(INFO) << "final avg cost for four wheels: "<<whole_loss[0]<<" "<<whole_loss[1]<<" "<<whole_loss[2]<<" "<<whole_loss[3]<<" ";
     if(solve_result)
         printf("final avg cost for four wheels: %7.6f %7.6f %7.6f %7.6f---%7.6f\n", loss_result.lf_loss, loss_result.rf_loss, loss_result.lb_loss, loss_result.rb_loss, loss_result.total_avg_loss);
     
-    // // 生成并保存图片
-    // update_mat(optimized_map_rows, map_cols);
-    // Detect_result t_detect_result = detect_result;
-    // t_detect_result.theta *= (-M_PI) / 180.0;
-    // t_detect_result.front_theta *= (-M_PI) / 180.0;
+    // // // 生成并保存图片
+    // // update_mat(optimized_map_rows, map_cols);
+    // Detect_result tt_detect_result = detect_result;
+    // tt_detect_result.theta *= (-M_PI) / 180.0;
+    // tt_detect_result.front_theta *= (-M_PI) / 180.0;
     // Loss_result t_loss_result;
-    // // LOG(INFO) <<"going to show img ";
+    // LOG(INFO) <<"going to show img ";
     // Solve(detect_result, t_loss_result, true);
 
     return solve_result;
@@ -559,7 +576,6 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, double &avg_loss)
 
 }
 
-
 bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_result, bool save_img)
 {
     double SCALE=300.0;
@@ -569,26 +585,29 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_r
     double init_wheel_base=2.7;
     double init_width=1.55;
     double init_theta_front=0*M_PI/180.0;
+    // 车轮长图比, 比例取值1/3-5/3, 当前变量无限制
+    double init_wheel_length_img_ratio = 0;
+    // 车轮宽长比, 默认长为1,比例取值范围0.2-0.8, 当前变量范围无限制
+    double init_wheel_width_length_ratio = 0;
 
-    double variable[] = {cx,cy,init_theta,init_wheel_base,init_width,init_theta_front};
+    double variable[] = {cx, cy, init_theta, init_wheel_base, init_width, init_theta_front, init_wheel_length_img_ratio, init_wheel_width_length_ratio};
     // printf("solve x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n", variable[0], variable[1], variable[3], variable[4], variable[2], variable[5]);
     // 第二部分：构建寻优问题
     ceres::Problem problem;
     CostFunctor *cost_func = new CostFunctor(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,m_map,SCALE);
     //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
     ceres::CostFunction* cost_function =new
-            ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, 6>(
+            ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, 8>(
                     cost_func,
                     m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_cloud.size());
     problem.AddResidualBlock(cost_function, NULL, variable); //向问题中添加误差项，本问题比较简单，添加一个就行。
 
-
     //第三部分： 配置并运行求解器
     ceres::Solver::Options options;
     options.use_nonmonotonic_steps=false;
     options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
     options.max_num_iterations=60;
-    options.num_threads=1;
+    options.num_threads=3;
     options.minimizer_progress_to_stdout = false;//输出到cout
     ceres::Solver::Summary summary;//优化信息
     ceres::Solve(options, &problem, &summary);//求解!!!
@@ -609,14 +628,18 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_r
     if(detect_result.theta<0)
         detect_result.theta+=180.0;
 
+    // [1/3, 5/3]
+    double limited_wheel_length_img_ratio = (4.0/5.0) / (1 + std::exp(-variable[6])) + (4.0/5.0);
+    // [0.2, 0.8]
+    double limited_wheel_length_ratio = 0.6 / (1 + std::exp(-variable[7])) + 0.2;
     //检验
     // printf("loss: %.5f\n", loss);
-    // printf("middle x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n", variable[0], variable[1], variable[3], variable[4], variable[2], variable[5]);
-    if(loss>0.01)
-    {
-        LOG(WARNING) <<"总loss过大 "<<loss;
-        return false;
-    }
+    printf("middle x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f, ratio: %.3f %.3f\n", variable[0], variable[1], variable[3], variable[4], variable[2], variable[5], limited_wheel_length_img_ratio, limited_wheel_length_ratio);
+    // if(loss>0.0006)//0.0141
+    // {
+    //     LOG(WARNING) <<"总loss过大 "<<loss;
+    //     return false;
+    // }
     if (detect_result.width < 1.350 || detect_result.width > 2.000 || detect_result.wheel_base > 3.000 || detect_result.wheel_base < 2.200)
     {
         LOG(WARNING) <<"宽度(1.35, 2.00) 轴距(2.20, 3.00): "<<detect_result.width<<", "<<detect_result.wheel_base;
@@ -658,15 +681,15 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_r
             loss_result.rb_loss = loss;
         }
         // 判断每个轮子平均loss是否满足条件
-        double single_wheel_loss_threshold = 0.09;
-        if(loss_result.lf_loss > single_wheel_loss_threshold || loss_result.rf_loss > single_wheel_loss_threshold  || loss_result.lb_loss > single_wheel_loss_threshold || loss_result.rb_loss > single_wheel_loss_threshold)
-        {
-            LOG(WARNING) <<"四轮loss过大";
-            return false;
-        }
+        double single_wheel_loss_threshold = 0.023;//0.115
+        // if(loss_result.lf_loss > single_wheel_loss_threshold || loss_result.rf_loss > single_wheel_loss_threshold  || loss_result.lb_loss > single_wheel_loss_threshold || loss_result.rb_loss > single_wheel_loss_threshold)
+        // {
+        //     LOG(WARNING) <<"四轮loss过大"<<loss_result.lf_loss<<", "<<loss_result.rf_loss<<", "<<loss_result.lb_loss<<", "<<loss_result.rb_loss;
+        //     return false;
+        // }
 
         // std::cout<<"save img: "<<save_img<<std::endl;
-        debug_img(detect_result, loss_result, save_img);
+        debug_img(detect_result, loss_result, save_img, limited_wheel_length_img_ratio, limited_wheel_length_ratio);
     }
     else
     {
@@ -683,7 +706,7 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_r
 }
 
 // 显示/保存图片供调试用
-void detect_wheel_ceres::debug_img(Detect_result detect_result, Loss_result loss_result, bool save_img, std::string out_img_path)
+void detect_wheel_ceres::debug_img(Detect_result detect_result, Loss_result loss_result, bool save_img, double li_ratio , double wl_ratio, std::string out_img_path)
 {
     double SCALE=300.0;
     cv::Mat lf = m_map.clone();
@@ -691,7 +714,7 @@ void detect_wheel_ceres::debug_img(Detect_result detect_result, Loss_result loss
     cv::Mat lb = m_map.clone();
     cv::Mat rb = m_map.clone();
     //整车旋转
-    Eigen::Rotation2D<double> rotation(detect_result.theta);
+    Eigen::Rotation2D<double> rotation(detect_result.theta*(-M_PI/180.0));
 
     Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
 
@@ -705,7 +728,7 @@ void detect_wheel_ceres::debug_img(Detect_result detect_result, Loss_result loss
     Eigen::Matrix<double, 2, 1> wheel_center_normal_right_rear(-detect_result.wheel_base / 2.0, (-detect_result.width + 0.15) / 2.0);
 
     //前轮旋转
-    Eigen::Rotation2D<double> rotation_front(detect_result.front_theta);
+    Eigen::Rotation2D<double> rotation_front(detect_result.front_theta*(-M_PI/180.0));
     Eigen::Matrix<double, 2, 2> rotation_matrix_front = rotation_front.toRotationMatrix();
 
     // 左前轮
@@ -717,8 +740,8 @@ void detect_wheel_ceres::debug_img(Detect_result detect_result, Loss_result loss
         Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_front;
         //旋转
         Eigen::Matrix<double, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
-        int r = (int)(point_rotation(1, 0) * SCALE + m_map.rows / 2.0 + 0.5);
-        int c = (int)(point_rotation(0, 0) * SCALE + m_map.cols / 2.0 + 0.5);
+        int r = (int)(point_rotation(1, 0) *li_ratio/ wl_ratio * SCALE + m_map.rows / 2.0 + 0.5);
+        int c = (int)(point_rotation(0, 0) *li_ratio * SCALE + m_map.cols / 2.0 + 0.5);
         cv::circle(lf, cv::Point(c, r), 1, cv::Scalar(150));
         // std::cout<<"map r,c "<<m_map.rows<<", "<<m_map.cols<<std::endl;
         // std::cout<<"r,c "<<r<<", "<<c<<std::endl;
@@ -733,8 +756,8 @@ void detect_wheel_ceres::debug_img(Detect_result detect_result, Loss_result loss
         Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_front;
         //旋转
         Eigen::Matrix<double, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
-        int r = (int)(point_rotation(1, 0) * SCALE + m_map.rows / 2.0 + 0.5);
-        int c = (int)(point_rotation(0, 0) * SCALE + m_map.cols / 2.0 + 0.5);
+        int r = (int)(point_rotation(1, 0)*li_ratio/ wl_ratio  * SCALE + m_map.rows / 2.0 + 0.5);
+        int c = (int)(point_rotation(0, 0)*li_ratio  * SCALE + m_map.cols / 2.0 + 0.5);
         cv::circle(rf, cv::Point(c, r), 1, cv::Scalar(150));
     }
     //左后轮
@@ -744,8 +767,8 @@ void detect_wheel_ceres::debug_img(Detect_result detect_result, Loss_result loss
                                           (double(m_left_rear_cloud[i].y) - detect_result.cy));
         //减去经过车辆旋转计算的左前中心
         Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_rear;
-        int r = (int)(tanslate_point(1, 0) * SCALE + m_map.rows / 2.0 + 0.5);
-        int c = (int)(tanslate_point(0, 0) * SCALE + m_map.cols / 2.0 + 0.5);
+        int r = (int)(tanslate_point(1, 0)*li_ratio / wl_ratio * SCALE + m_map.rows / 2.0 + 0.5);
+        int c = (int)(tanslate_point(0, 0)*li_ratio  * SCALE + m_map.cols / 2.0 + 0.5);
         cv::circle(lb, cv::Point(c, r), 1, cv::Scalar(150));
     }
 
@@ -756,8 +779,8 @@ void detect_wheel_ceres::debug_img(Detect_result detect_result, Loss_result loss
                                           (double(m_right_rear_cloud[i].y) - detect_result.cy));
         //减去经过车辆旋转计算的左前中心
         Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_rear;
-        int r = (int)(tanslate_point(1, 0) * SCALE + m_map.rows / 2.0 + 0.5);
-        int c = (int)(tanslate_point(0, 0) * SCALE + m_map.cols / 2.0 + 0.5);
+        int r = (int)(tanslate_point(1, 0)*li_ratio / wl_ratio * SCALE + m_map.rows / 2.0 + 0.5);
+        int c = (int)(tanslate_point(0, 0)*li_ratio  * SCALE + m_map.cols / 2.0 + 0.5);
         cv::circle(rb, cv::Point(c, r), 1, cv::Scalar(150));
     }
 
@@ -798,23 +821,25 @@ void detect_wheel_ceres::debug_img(Detect_result detect_result, Loss_result loss
     rft.copyTo(total_img(cv::Rect(m_map.rows, 0, m_map.rows, m_map.cols)));
     lbt.copyTo(total_img(cv::Rect(0, m_map.cols, m_map.rows, m_map.cols)));
     rbt.copyTo(total_img(cv::Rect(m_map.rows, m_map.cols, m_map.rows, m_map.cols)));
-    // cv::namedWindow("total img", CV_WINDOW_FREERATIO);
-    // cv::imshow("total img", total_img);
-    // cv::waitKey(20);
-    if(save_img)
+    // std::cout << "save? " << save_img << std::endl;
+    if (save_img)
     {
-        cv::Mat cvted_img = cv::Mat::zeros(m_map.cols * 2, m_map.rows * 2, CV_8U);
-        int total_milli = (std::chrono::time_point_cast<std::chrono::milliseconds>(std::chrono::system_clock::now())).time_since_epoch().count();
-        std::string img_filename="";
-        if(out_img_path.empty()){
-            img_filename = std::string("/home/youchen/Documents/measure/MainStructure/elecfence_ws/img/img").append(std::to_string(total_milli)).append(".jpg");
-        }
-        else{
-            img_filename = out_img_path;
-        }
-        LOG(INFO) << "write to " << img_filename.c_str();
-        // cv::cvtColor(total_img*255, cvted_img, CV_8U);
-        cv::convertScaleAbs(total_img * 255, cvted_img);
-        cv::imwrite(img_filename, cvted_img);
+        cv::namedWindow("total img", CV_WINDOW_FREERATIO);
+        cv::imshow("total img", total_img);
+        cv::waitKey(20);
+
+        // cv::Mat cvted_img = cv::Mat::zeros(m_map.cols * 2, m_map.rows * 2, CV_8U);
+        // int total_milli = (std::chrono::time_point_cast<std::chrono::milliseconds>(std::chrono::system_clock::now())).time_since_epoch().count();
+        // std::string img_filename = "";
+        // if(out_img_path.empty()){
+        //     img_filename = std::string("/home/youchen/Documents/measure/MainStructure/elecfence_ws/img/img").append(std::to_string(total_milli)).append(".jpg");
+        // }
+        // else{
+        //     img_filename = out_img_path;
+        // }
+        // LOG(INFO) << "write to " << img_filename.c_str();
+        // // cv::cvtColor(total_img*255, cvted_img, CV_8U);
+        // cv::convertScaleAbs(total_img * 255, cvted_img);
+        // cv::imwrite(img_filename, cvted_img);
     }
 }

src/findwheel/src/detect_wheel_ceres.h

@@ -67,7 +67,7 @@ protected:
     bool update_mat(int rows, int cols);
     bool Solve(Detect_result &detect_result, double &avg_loss);
     bool Solve(Detect_result &detect_result, Loss_result &loss_result, bool save_img=false);
-    void debug_img(Detect_result detect_result, Loss_result loss_result, bool save_img=false, std::string out_img_path="");
+    void debug_img(Detect_result detect_result, Loss_result loss_result, bool save_img=false, double li_ratio = 1, double wl_ratio=1, std::string out_img_path="");
 
     void create_mark(double x,double y,double theta,pcl::PointCloud<pcl::PointXYZ>& cloud);
 public:

src/findwheel/src/region_detect.cpp

@@ -352,7 +352,15 @@ bool Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, doub
 
     detect_wheel_ceres::Detect_result detect_result{x,y,c,wheelbase,width,front_wheel_theta};
     if(!m_detector_ceres.detect(seg_clouds, detect_result))
+    {
         return false;
+    }
+    x = detect_result.cx;
+    y = detect_result.cy;
+    c = detect_result.theta;
+    wheelbase = detect_result.wheel_base;
+    width = detect_result.width;
+    front_wheel_theta = detect_result.front_theta;
 
     return true;
 }

src/findwheel/src/region_detect.h

@@ -9,9 +9,9 @@
 #include <fstream>
 #include <string>
 #include <mutex>
-#include <ros/ros.h>
-#include <sensor_msgs/LaserScan.h>
-#include <tf/transform_listener.h>
+// #include <ros/ros.h>
+// #include <sensor_msgs/LaserScan.h>
+// #include <tf/transform_listener.h>
 #include <vector>
 #include "eigen3/Eigen/Core"
 #include "eigen3/Eigen/Dense"

src/findwheel/src/region_worker.cpp

@@ -165,6 +165,6 @@ void Region_worker::detect_loop(Region_worker *worker)
             p->UpdateData(code, worker->m_detector->m_region_id);
         }*/
         // worker->m_mutex.unlock();
-        // usleep(1000*100);
+        // usleep(1000*500);
     }
 }

src/findwheel/src/region_worker.h

@@ -12,6 +12,10 @@
 #include <iostream>
 #include "tools/define.h"
 
+#include <ros/ros.h>
+#include <sensor_msgs/LaserScan.h>
+#include <tf/transform_listener.h>
+
 class Region_worker{
 public:
     Region_worker(int id, EleFence::Region region,ros::NodeHandle handle);

src/wj_716_lidar/CMakeLists.txt

@@ -124,6 +124,10 @@ add_executable(wj_716_lidar src/wj_716_lidar_01.cpp src/async_client.cpp src/wj_
 add_dependencies(wj_716_lidar ${PROJECT_NAME}_gencfg)
 target_link_libraries(wj_716_lidar ${catkin_LIBRARIES})
 
+#add_executable(wj_718_lidar src/wj_718_lidar_01.cpp src/async_client.cpp src/wj_718_lidar_protocol.cpp )
+#add_dependencies(wj_718_lidar ${PROJECT_NAME}_gencfg)
+#target_link_libraries(wj_718_lidar ${catkin_LIBRARIES})
+
 #add_dependencies(wj_716_lidar ${PROJECT_NAME}_gencfg)
 
 ## Declare a C++ library

src/wj_716_lidar/launch/wj_716_lidar_01.launch

@@ -8,40 +8,10 @@ Check IP-address, if you scanner is not found after roslaunch.
 -->
 
 <launch>
-  <node name="wj_716_lidar_03" pkg="wj_716_lidar" type="wj_716_lidar" respawn="false" output="screen">
-  <param name="hostname"         type="string"  value="192.168.2.203" />
-  <param name="port"             type="string"  value="2110" />
-  <!-- -135° -->
-  <param name="min_ang"          type="double"  value="-2.35619449" />
-  <!-- 135° -->
-  <param name="max_ang"          type="double"  value="2.35619449" />
-  <param name="angle_increment"  type="double"  value="0.00582" />
-  <param name="frame_id"         type="str"     value="map" />
-  <param name="time_increment"   type="double"  value="0.00006167129" />
-  <param name="range_min"        type="double"  value="0.05" />
-  <param name="range_max"        type="double"  value="30.0" />
-  <param name="resize"           type="int"     value="811" />
-  <remap from="/scan" to="/scan3" />
-  </node>
-	
-  <node name="wj_716_lidar_04" pkg="wj_716_lidar" type="wj_716_lidar" respawn="false" output="screen">
-  <param name="hostname"         type="string"  value="192.168.2.204" />
-  <param name="port"             type="string"  value="2110" />
-  <!-- -135° -->
-  <param name="min_ang"          type="double"  value="-2.35619449" />
-  <!-- 135° -->
-  <param name="max_ang"          type="double"  value="2.35619449" />
-  <param name="angle_increment"  type="double"  value="0.00582" />
-  <param name="frame_id"         type="str"     value="map" />
-  <param name="time_increment"   type="double"  value="0.00006167129" />
-  <param name="range_min"        type="double"  value="0.05" />
-  <param name="range_max"        type="double"  value="30.0" />
-  <param name="resize"           type="int"     value="811" />
-  <remap from="/scan" to="/scan4" />
-  </node>
+<node name="rviz" pkg="rviz" type="rviz" required="true" args="-d $(find wj_716_lidar)/launch/demo.rviz"/>
 
-  <node name="wj_716_lidar_05" pkg="wj_716_lidar" type="wj_716_lidar" respawn="false" output="screen">
-  <param name="hostname"         type="string"  value="192.168.2.205" />
+  <node name="wj_716_lidar" pkg="wj_716_lidar" type="wj_716_lidar" respawn="false" output="screen">
+  <param name="hostname"         type="string"  value="192.168.0.208" />
   <param name="port"             type="string"  value="2110" />
   <!-- -135° -->
   <param name="min_ang"          type="double"  value="-2.35619449" />
@@ -53,7 +23,7 @@ Check IP-address, if you scanner is not found after roslaunch.
   <param name="range_min"        type="double"  value="0.05" />
   <param name="range_max"        type="double"  value="30.0" />
   <param name="resize"           type="int"     value="811" />
-  <remap from="/scan" to="/scan5" />
+  <remap from="/scan" to="/scan_1" />
   </node>
 	
 </launch>

src/wj_716_lidar/src/wj_716_lidar_protocol.cpp

@@ -87,6 +87,7 @@ namespace wj_lidar
      m_sdata.m_u32in += reclen;
      while(m_sdata.m_u32out < m_sdata.m_u32in)
      {
+       printf("%02X %02X\n", m_sdata.m_acdata[m_sdata.m_u32out+4], m_sdata.m_acdata[m_sdata.m_u32out + 1+5]);
        if(m_sdata.m_acdata[m_sdata.m_u32out] == 0x02 && m_sdata.m_acdata[m_sdata.m_u32out+1] == 0x02 &&
           m_sdata.m_acdata[m_sdata.m_u32out+2] == 0x02 && m_sdata.m_acdata[m_sdata.m_u32out+3] == 0x02)
        {

src/wj_716_lidar/src/wj_718_lidar_01.cpp

@@ -0,0 +1,145 @@
+#include <ros/ros.h>
+#include "async_client.h"
+#include "wj_718_lidar_protocol.h"
+using namespace wj_lidar;
+
+int InitTcpConnection(const char* addr,int port,Async_Client **client_,fundata_t fundata_)
+{
+
+  io_service iosev;
+
+  ip::tcp::endpoint ep(ip::address_v4::from_string(addr),port);
+  *client_ = new Async_Client(iosev,ep,fundata_);
+
+  iosev.run();
+
+  return 1 ;
+}
+
+int boost_tcp_init_connection(const char* addr,int port,Async_Client **client_,fundata_t fundata_)
+{
+  int timecnt=0 ;
+  *client_ = NULL ;
+
+  boost::thread tmp(&InitTcpConnection,addr,port,client_,fundata_);
+  tmp.detach() ;
+
+  while(timecnt<50){
+    timecnt++ ;
+    usleep(20000); //20 ms
+    if((*client_)->client_return_status()){
+      return 0 ;
+    }
+  }
+  *client_ = NULL ;
+  return -1 ;
+
+}
+
+int boost_tcp_sync_send(Async_Client *client_ ,const char* msg,const int len)
+{
+  if(client_==NULL || client_->client_return_status()==0 ){
+    printf("not connected , please connect first \n");
+    return -1 ;
+  }
+  else{
+    client_->client_async_write((char*)msg,len);
+    return 0 ;
+  }
+
+  return 1;
+}
+
+int boost_tcp_sync_read(Async_Client *client_ )
+{
+  if(client_==NULL || client_->client_return_status()==0 ){
+    printf("not connected , please connect first \n");
+    return -1 ;
+  }
+  else{
+    client_->client_async_read();
+    return 0 ;
+  }
+  return 1;
+}
+
+/* ------------------------------------------------------------------------------------------
+ *  show demo --
+ * ------------------------------------------------------------------------------------------ */
+wj_718_lidar_protocol *protocol;
+Async_Client *client;
+void CallBackRead(const char* addr,int port,const char* data,const int len)
+{
+  protocol->dataProcess(data,len);
+}
+
+void callback(wj_716_lidar::wj_716_lidarConfig &config,uint32_t level)
+{
+  protocol->setConfig(config,level);
+}
+
+void timerCallback(const ros::TimerEvent&)
+{
+//  char heartframe[34]={0xFF,0xAA,0x00,0x1E,0x00,0x00,0x00,0x00,
+//                       0x00,0x00,0x01,0x01,0x00,0x05,0x00,0x00,
+//                       0x00,0x00,0x00,0x00,0x00,0x00,0x05,0x04,
+//                       0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x1A,
+//                       0xEE,0xEE};
+
+//  //cout << "20ms" <<endl;
+//  boost_tcp_sync_send(client,heartframe,34);
+
+  if(protocol->heartstate)
+  {
+    protocol->heartstate=false;
+    cout <<"once heart cycle "<<endl;
+  }
+  else
+  {
+    client->socket_close();
+    client=NULL;
+    ros::NodeHandle nh("~");
+    std::string hostname;
+    nh.getParam("hostname",hostname);
+    std::string port;
+    nh.getParam("port",port);
+
+
+    //InitTcpConnection(hostname.c_str(),atoi(port.c_str()),&client,&CallBackRead);
+   //boost_tcp_init_connection(hostname.c_str(),atoi(port.c_str()),&client,&CallBackRead);
+    boost_tcp_sync_read(client);
+
+    cout <<"try to reconect "<<endl;
+    cout << hostname.c_str()<<endl;
+    cout << port.c_str()<<endl;
+    //ROS_INFO("Hello wj_716_lidar!");
+  }
+
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "wj_718_lidar_01");
+  ros::NodeHandle nh("~");
+//  ros::Timer timer;
+  std::string hostname;
+  nh.getParam("hostname",hostname);
+  cout << hostname <<endl;
+  std::string port;
+  nh.getParam("port",port);
+
+  protocol = new wj_718_lidar_protocol();
+  dynamic_reconfigure::Server<wj_716_lidar::wj_716_lidarConfig> server;
+  dynamic_reconfigure::Server<wj_716_lidar::wj_716_lidarConfig>::CallbackType f;
+  f = boost::bind(&callback,_1,_2);
+  server.setCallback(f);
+  client=NULL;
+
+  boost_tcp_init_connection(hostname.c_str(),atoi(port.c_str()),&client,&CallBackRead);
+//  timer= nh.createTimer(ros::Duration(2), timerCallback);
+  boost_tcp_sync_read(client);
+
+  ROS_INFO("Hello wj_718_lidar!");
+  ros::spin();
+
+}

src/wj_716_lidar/src/wj_718_lidar_protocol.cpp

@@ -0,0 +1,287 @@
+#include "wj_718_lidar_protocol.h"
+#include <iostream>
+
+namespace wj_lidar
+{
+  bool wj_718_lidar_protocol::setConfig(wj_716_lidar::wj_716_lidarConfig &new_config,uint32_t level)
+  {
+    config_ = new_config;
+    scan.header.frame_id = config_.frame_id;
+    scan.angle_min = config_.min_ang;
+    scan.angle_max = config_.max_ang;
+    scan.angle_increment = config_.angle_increment;
+    scan.time_increment = config_.time_increment;//0.00002469;
+    scan.range_min = config_.range_min;
+    scan.range_max = config_.range_max;
+//    scan.ranges.resize(config_.resize);
+    scan.intensities.resize(config_.resize);	  
+
+    scan_TwoEcho.header.frame_id = config_.frame_id;
+    scan_TwoEcho.angle_min = config_.min_ang;
+    scan_TwoEcho.angle_max = config_.max_ang;
+    scan_TwoEcho.angle_increment = config_.angle_increment;
+    scan_TwoEcho.time_increment = config_.time_increment;//0.00002469;
+    scan_TwoEcho.range_min = config_.range_min;
+    scan_TwoEcho.range_max = config_.range_max;
+//    scan.ranges.resize(config_.resize);
+    scan.intensities.resize(config_.resize);
+
+    cout << "frame_id:" <<scan.header.frame_id<<endl;
+    cout << "min_ang:" <<scan.angle_min<<endl;
+    cout << "max_ang:" <<scan.angle_max<<endl;
+    cout << "angle_increment:" <<scan.angle_increment<<endl;
+    cout << "time_increment:" <<scan.time_increment<<endl;
+    cout << "range_min:" <<scan.range_min<<endl;
+    cout << "range_max:" <<scan.range_max<<endl;
+    int resizeNum;
+    cout << "resizeNum:" <<resizeNum<<endl;
+    return true;
+  }
+   wj_718_lidar_protocol::wj_718_lidar_protocol()
+   {
+     memset(&m_sdata,0,sizeof(m_sdata));
+     marker_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 50);
+     twoecho_=nh.advertise<sensor_msgs::LaserScan>("scan_TwoEcho", 5);
+     ros::Time scan_time = ros::Time::now();      //make a virtual data per sec
+     scan.header.stamp = scan_time;
+
+     g_u32PreFrameNo = 0;
+
+     scan.header.frame_id = "wj_716_lidar_frame";
+     scan.angle_min = -2.35619449;
+     scan.angle_max = 2.35619449;
+     scan.angle_increment = 0.00582;
+     scan.time_increment = 0.0667/1081;
+     scan.range_min = 0;
+     scan.range_max = 30;
+     scan.ranges.resize(811);
+     scan.intensities.resize(811);
+
+     scan_TwoEcho.header.frame_id = "wj_716_lidar_frame";
+     scan_TwoEcho.angle_min = -2.35619449;
+     scan_TwoEcho.angle_max = 2.35619449;
+     scan_TwoEcho.angle_increment = 0.00582;
+     scan_TwoEcho.time_increment = 0.0667/1081;
+     scan_TwoEcho.range_min = 0;
+     scan_TwoEcho.range_max = 50;
+     scan_TwoEcho.ranges.resize(811);
+     scan_TwoEcho.intensities.resize(811);
+     cout << "wj_716_lidar_protocl start success" << endl;
+
+   }
+   bool wj_718_lidar_protocol::dataProcess(const char *data, const int reclen)
+   {
+     unsigned char *u_data = reinterpret_cast<unsigned char*>(data);
+     //  cout << "reclen: "<<reclen<<endl;
+     if(reclen > MAX_LENGTH_DATA_PROCESS)
+     {
+		// cout << "1111111"<<endl;
+       return false;
+     }
+
+     if(m_sdata.m_u32in + reclen > MAX_LENGTH_DATA_PROCESS)
+     {
+		// cout << "2222222"<<endl;
+       memset(&m_sdata,0,sizeof(m_sdata));
+       return false;
+     }
+     memcpy(&m_sdata.m_acdata[m_sdata.m_u32in],data,reclen*sizeof(char));
+     m_sdata.m_u32in += reclen;
+     printf("%02X %02X\n", m_sdata.m_acdata[m_sdata.m_u32out+2], m_sdata.m_acdata[m_sdata.m_u32out + 3]);
+     while(m_sdata.m_u32out < m_sdata.m_u32in)
+     {
+       if((m_sdata.m_acdata[m_sdata.m_u32out] & 0xff) >0 && (m_sdata.m_acdata[m_sdata.m_u32out+1] & 0xaa) >0)
+       {
+         unsigned l_u32reallen = (m_sdata.m_acdata[m_sdata.m_u32out + 2] << 8)  |
+                                 (m_sdata.m_acdata[m_sdata.m_u32out + 3] << 0);
+        //  printf("real length: %02x\n", l_u32reallen);
+         l_u32reallen = l_u32reallen + 4;
+
+         if(l_u32reallen <= (m_sdata.m_u32in - m_sdata.m_u32out + 1))
+         {
+          //  cout << "in: " << m_sdata.m_u32in << " out: " << m_sdata.m_u32out << " frame size: " << l_u32reallen << endl;
+           if(OnRecvProcess(&m_sdata.m_acdata[m_sdata.m_u32out],l_u32reallen))
+           {
+             m_sdata.m_u32out += l_u32reallen;
+           }
+           else
+           {
+             cout << "continuous frame"<<endl;
+             int i;
+             for(i == 1; i<l_u32reallen; i++)
+             {
+               if((m_sdata.m_acdata[m_sdata.m_u32out + i] == 0x02) &&
+                  (m_sdata.m_acdata[m_sdata.m_u32out + i + 1] == 0x02) &&
+                  (m_sdata.m_acdata[m_sdata.m_u32out + i + 2] == 0x02) &&
+                  (m_sdata.m_acdata[m_sdata.m_u32out + i + 3] == 0x02))
+               {
+                 m_sdata.m_u32out += i;
+                 break;
+               }
+               if(i == l_u32reallen)
+               {
+                 m_sdata.m_u32out += l_u32reallen;
+               }
+             }
+           }
+         }
+         else if(l_u32reallen >= MAX_LENGTH_DATA_PROCESS)
+         {
+          //  cout << "l_u32reallen >= MAX_LENGTH_DATA_PROCESS"<<endl;
+          //  cout << "reallen: "<<l_u32reallen<<endl;
+           memset(&m_sdata,0,sizeof(m_sdata));
+
+         }
+         else
+         {
+           cout<<"reallen: "<<l_u32reallen<<" indata: "<<m_sdata.m_u32in<<" outdata: "<<m_sdata.m_u32out<<endl;
+           break;
+         }
+		 //cout << "---in: " << m_sdata.m_u32in << " out: " << m_sdata.m_u32out << " frame size: " << l_u32reallen <<endl;  
+       }
+       else
+       {
+         m_sdata.m_u32out++;
+       }
+     } //end while(m_sdata.m_u32out < m_sdata.m_u32in)
+
+	  // cout << "-----in: " << m_sdata.m_u32in << " out: " << m_sdata.m_u32out <<endl;
+     if(m_sdata.m_u32out >= m_sdata.m_u32in)
+     {
+       memset(&m_sdata,0,sizeof(m_sdata));
+     }
+	  // cout << "-------in: " << m_sdata.m_u32in << " out: " << m_sdata.m_u32out <<endl;
+      return true;
+   }
+
+   bool wj_718_lidar_protocol::OnRecvProcess( char *data, int len)
+   {
+     if(len > 0)
+     {
+       if(checkXor(data,len))
+       {
+         protocl(data,len);
+       }
+     }
+     else
+     {
+       return false;
+     }
+     return true;
+   }
+
+   bool wj_718_lidar_protocol::protocl(const char *data, const int len)
+   {
+     if(data[10] == 0x01 && data[11] == 0x02 && data[12] == 0x00 && data[13] == 0x06)   //constant value
+     {
+       static int s_n32ScanIndex;
+       //　当前包号
+       int l_n32PackageNo = data[36]; //shuju bao xu hao  01 or 02
+       // 总包号
+       unsigned total_package_num = data[35];
+       // 圈号
+       unsigned int l_u32FrameNo = (data[31] << 24) + (data[32] << 16) + (data[33] << 8) + data[34]; //quan hao
+       // 扫描数据长度
+       unsigned scan_data_length = (data[37] << 8) | data[38];
+       if (l_n32PackageNo == 1)
+       {
+         s_n32ScanIndex = 0;
+         g_u32PreFrameNo = l_u32FrameNo;
+
+         for (int j = 0; j < scan_data_length; j = j + 2)
+         {
+           scandata[s_n32ScanIndex] = (((unsigned char)data[39 + j]) << 8) + ((unsigned char)data[40 + j]);
+           scandata[s_n32ScanIndex] /= 1000.0;
+           if (scandata[s_n32ScanIndex] >= 25 || scandata[s_n32ScanIndex] == 0)
+           {
+             scandata[s_n32ScanIndex] = NAN;
+           }
+           s_n32ScanIndex++;
+         }
+         cout << "quan hao1: " << g_u32PreFrameNo << "  danzhen quanhao1: " << l_u32FrameNo<< endl;
+       }
+       else if (l_n32PackageNo == 2)
+       {
+         if (g_u32PreFrameNo == l_u32FrameNo)
+         {
+           for (int j = 0; j < scan_data_length; j = j + 2)
+           {
+             scandata[s_n32ScanIndex] = (((unsigned char)data[39 + j]) << 8) + ((unsigned char)data[40 + j]);
+             scandata[s_n32ScanIndex] /= 1000.0;
+             if (scandata[s_n32ScanIndex] >= 25 || scandata[s_n32ScanIndex] == 0)
+             {
+               scandata[s_n32ScanIndex] = NAN;
+             }
+             scan.intensities[s_n32ScanIndex] = 0;
+             s_n32ScanIndex++;
+           }
+         }
+         else
+         {
+           s_n32ScanIndex = 0;
+           cout << "quan hao2: " << g_u32PreFrameNo << "  danzhen quanhao2: " << l_u32FrameNo<< endl;
+           return false;
+         }
+       }
+        cout << "curr pack " << l_n32PackageNo << " total pack " << total_package_num<< endl;
+       if (l_n32PackageNo == total_package_num)
+       {
+         // adjust angle_min to min_ang config param
+         int index_min = (config_.min_ang + 2.35619449) / scan.angle_increment;
+         // adjust angle_max to max_ang config param
+         int index_max = 811 - ((2.35619449 - config_.max_ang) / scan.angle_increment);
+         scan.ranges.resize(index_max - index_min);
+         scan.intensities.resize(index_max - index_min);
+
+         /*for (int j = index_min; j <= index_max; ++j)
+            {
+                scan.ranges[j - index_min] = scandata[j];
+                scan.intensities[j - index_min]=scaninden[j];
+            }*/
+         // changed by yct
+         for (int j = index_min; j <= index_max; ++j)
+         {
+           scan.ranges[index_max - j] = scandata[j];
+           scan.intensities[index_max - j] = 50;
+         }
+         ros::Time scan_time = ros::Time::now();
+         scan.header.stamp = scan_time;
+         marker_pub.publish(scan);
+       }
+       return true;
+     }
+     else
+     {
+       return false;
+     }
+
+   }
+
+   bool wj_718_lidar_protocol::checkXor( char *recvbuf,  int recvlen)
+   {
+     int i = 0;
+     char check = 0;
+     char *p = recvbuf;
+     int len;
+     if(*p == (char)0xFF)
+     {
+       p = p+2;
+       len = recvlen - 6;
+       for(i = 0; i < len; i++)
+       {
+         check ^= *p++;
+       }
+       if(check == *p)
+       {
+         return true;
+       }
+       else
+         return false;
+     }
+     else
+     {
+       return false;
+     }
+   }
+
+}

src/wj_716_lidar/src/wj_718_lidar_protocol.h

@@ -0,0 +1,54 @@
+#ifndef WJ_718_LIDAR_PROTOCOL_H
+#define WJ_718_LIDAR_PROTOCOL_H
+#include <iostream>
+#include "string.h"
+#include <boost/shared_ptr.hpp>
+#include <boost/asio.hpp>
+#include <boost/asio/placeholders.hpp>
+#include <boost/system/error_code.hpp>
+#include <boost/bind/bind.hpp>
+#include <ros/ros.h>
+#include <visualization_msgs/Marker.h>
+#include <sensor_msgs/LaserScan.h>
+#include <dynamic_reconfigure/server.h>
+#include <wj_716_lidar/wj_716_lidarConfig.h>
+using namespace std ;
+namespace wj_lidar
+{
+  #define MAX_LENGTH_DATA_PROCESS 200000
+  typedef struct TagDataCache
+  {
+    char m_acdata[MAX_LENGTH_DATA_PROCESS];
+    unsigned int m_u32in;
+    unsigned int m_u32out;
+  }DataCache;
+  class wj_718_lidar_protocol
+  {
+  public:
+    wj_718_lidar_protocol();
+    bool dataProcess(const char *data,const int reclen);
+    bool protocl(const char *data,const int len);
+    bool OnRecvProcess(char *data, int len);
+    bool checkXor(char *recvbuf, int recvlen);
+    void send_scan(const char *data,const int len);
+    ros::NodeHandle nh;
+    ros::Publisher marker_pub;                       //saomiao shujufabu
+    ros::Publisher twoecho_;                         //er chong huibo shuju
+    sensor_msgs::LaserScan scan;
+    sensor_msgs::LaserScan scan_TwoEcho;
+    bool setConfig(wj_716_lidar::wj_716_lidarConfig &new_config,uint32_t level);
+    //dynamic_reconfigure::Server<wj_safety_lidar_protocl::> dr_srv;
+    bool heartstate;
+  private:
+    char        data_[MAX_LENGTH_DATA_PROCESS];
+    DataCache   m_sdata;
+    wj_716_lidar::wj_716_lidarConfig config_;
+    unsigned int g_u32PreFrameNo;
+    float scandata[811];
+    float scandata_te[811];
+    float scaninden[811];
+
+  };
+
+}
+#endif // WJ_718_LIDAR_PROTOCOL_H

src/wj_718_lidar/CMakeLists.txt

@@ -0,0 +1,207 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(wj_718_lidar)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  dynamic_reconfigure
+
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+ generate_dynamic_reconfigure_options(
+    cfg/wj_718_lidar.cfg
+
+  )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES wj_718_lidar
+#  CATKIN_DEPENDS roscpp
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include ${catkin_INCLUDE_DIRS}
+# include
+#  ${catkin_INCLUDE_DIRS}
+)
+
+add_executable(wj_718_lidar
+   src/wj_718_lidar_01.cpp src/async_client.cpp src/wj_718_lidar_protocol.cpp )
+add_dependencies(wj_718_lidar ${PROJECT_NAME}_gencfg)
+target_link_libraries(wj_718_lidar ${catkin_LIBRARIES})
+
+#add_dependencies(wj_718_lidar ${PROJECT_NAME}_gencfg)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/wj_718_lidar.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/wj_718_lidar_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_wj_718_lidar.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

src/wj_718_lidar/cfg/wj_718_lidar.cfg

@@ -0,0 +1,31 @@
+#! /usr/bin/env python
+
+PACKAGE='wj_718_lidar'
+from dynamic_reconfigure.parameter_generator_catkin import *
+#from math import pi
+
+#from driver_base.msg import SensorLevels
+
+gen = ParameterGenerator()
+#       Name              Type      Reconfiguration level             Description                                      Default    Min       Max
+gen.add("min_ang",        double_t, 0, "The angle of the first range measurement [rad].",                               -3.14,    -3.14,    3.14)
+gen.add("max_ang",        double_t, 0, "The angle of the last range measurement [rad].",                                3.14,     -3.14,    3.14)
+gen.add("angle_increment",double_t, 0, "The angle_increment of the first range measurement [rad].",                     0.00582,  0.00582,  0.0174533)
+gen.add("time_increment", double_t, 0, "The time_increment[s].",                                                        0.00008220304,   0.00008220304,   0.0001476015)
+gen.add("range_min",      int_t,    0, "The range_min [m].",                                                            0,        0,        10)
+gen.add("range_max",      int_t,    0, "The range_max[m].",                                                             10,       0,        10)
+gen.add("resize",         int_t,    0, "The resize[num].",                                                              811,      0,        811)
+gen.add("frequency_scan", int_t,    0, "The mode of frequency.",                                                        1,        0,        1)
+gen.add("frame_id",       str_t,    0, "The TF frame in which laser scans will be returned.",                        "laser")
+#gen.add("intensity",      bool_t,   SensorLevels.RECONFIGURE_RUNNING, "Whether or not the TiM3xx returns intensity values.",                        True)
+# gen.add("cluster",        int_t,    SensorLevels.RECONFIGURE_RUNNING, "The number of adjacent range measurements to cluster into a single reading.", 1,         0,   99)
+#gen.add("skip",           int_t,    SensorLevels.RECONFIGURE_RUNNING, "The number of scans to skip between each measured scan.",                    0,         0,    9)
+# gen.add("port",           str_t,    SensorLevels.RECONFIGURE_CLOSE,   "The serial port where the TiM3xx device can be found.",                       "/dev/ttyACM0")
+# gen.add("calibrate_time", bool_t,   SensorLevels.RECONFIGURE_CLOSE,   "Whether the node should calibrate the TiM3xx's time offset.",                 True)
+
+#gen.add("time_offset",    double_t, SensorLevels.RECONFIGURE_RUNNING, "An offset to add to the time stamp before publication of a scan [s].",       -0.001,     -0.25, 0.25)
+
+exit(gen.generate(PACKAGE, "wj_718_lidar", "wj_718_lidar"))
+
+
+

src/wj_718_lidar/launch/wj_718_lidar_01.launch

@@ -0,0 +1,146 @@
+<?xml version="1.0"?>
+<!-- start and stop angle is given in [rad] -->
+<!--
+default min_angle is -135 degree.
+default max_angle is +135 degree.
+
+Check IP-address, if you scanner is not found after roslaunch.
+-->
+
+<launch>
+<node name="rviz" pkg="rviz" type="rviz" required="true" args="-d $(find wj_718_lidar)/launch/demo.rviz"/>
+
+  <node name="wj_718_lidar_01" pkg="wj_718_lidar" type="wj_718_lidar" respawn="false" output="screen">
+  <param name="hostname"         type="string"  value="192.168.0.216" />
+  <param name="port"             type="string"  value="2110" />
+  <param name="frequency_scan"   type="int"     value="0" />    <!--0:15hz/0.33  1:25hz/1 -->
+  <!-- -135° -->
+  <param name="min_ang"          type="double"  value="-2.35619449" />
+  <!-- 135° -->
+  <param name="max_ang"          type="double"  value="2.35619449" />
+  <param name="angle_increment"  type="double"  value="0.017453" />
+  <param name="frame_id"         type="str"     value="laser" />
+  <param name="time_increment"   type="double"  value="0.0001476" />
+  <param name="range_min"        type="double"  value="0.05" />
+  <param name="range_max"        type="double"  value="10.0" />
+  <param name="resize"           type="int"     value="271" />
+
+  </node>
+</launch>
+
+<!--
+Conversion between degree and rad
+
+DEG	RAD
+-180	-3.141592654
+-175	-3.054326191
+-170	-2.967059728
+-165	-2.879793266
+-160	-2.792526803
+-155	-2.705260341
+-150	-2.617993878
+-145	-2.530727415
+-140	-2.443460953
+-137.5	-2,3998277
+-135	-2.35619449
+-130	-2.268928028
+-125	-2.181661565
+-120	-2.094395102
+-115	-2.00712864
+-110	-1.919862177
+-105	-1.832595715
+-100	-1.745329252
+-95	-1.658062789
+-90	-1.570796327
+-85	-1.483529864
+-80	-1.396263402
+-75	-1.308996939
+-70	-1.221730476
+-65	-1.134464014
+-60	-1.047197551
+-55	-0.959931089
+-50	-0.872664626
+-45	-0.785398163
+-40	-0.698131701
+-35	-0.610865238
+-30	-0.523598776
+-25	-0.436332313
+-20	-0.34906585
+-15	-0.261799388
+-10	-0.174532925
+-5	-0.087266463
+0	0
+5	0.087266463
+10	0.174532925
+15	0.261799388
+20	0.34906585
+25	0.436332313
+30	0.523598776
+35	0.610865238
+40	0.698131701
+45	0.785398163
+50	0.872664626
+55	0.959931089
+60	1.047197551
+65	1.134464014
+70	1.221730476
+75	1.308996939
+80	1.396263402
+85	1.483529864
+90	1.570796327
+95	1.658062789
+100	1.745329252
+105	1.832595715
+110	1.919862177
+115	2.00712864
+120	2.094395102
+125	2.181661565
+130	2.268928028
+135	2.35619449
+137.5	2,3998277
+140	2.443460953
+145	2.530727415
+150	2.617993878
+155	2.705260341
+160	2.792526803
+165	2.879793266
+170	2.967059728
+175	3.054326191
+180	3.141592654
+185	3.228859116
+190	3.316125579
+195	3.403392041
+200	3.490658504
+205	3.577924967
+210	3.665191429
+215	3.752457892
+220	3.839724354
+225	3.926990817
+230	4.01425728
+235	4.101523742
+240	4.188790205
+245	4.276056667
+250	4.36332313
+255	4.450589593
+260	4.537856055
+265	4.625122518
+270	4.71238898
+275	4.799655443
+280	4.886921906
+285	4.974188368
+290	5.061454831
+295	5.148721293
+300	5.235987756
+305	5.323254219
+310	5.410520681
+315	5.497787144
+320	5.585053606
+325	5.672320069
+330	5.759586532
+335	5.846852994
+340	5.934119457
+345	6.021385919
+350	6.108652382
+355	6.195918845
+360	6.283185307
+-->

src/wj_718_lidar/package.xml

@@ -0,0 +1,66 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>wj_718_lidar</name>
+  <version>0.1.0</version>
+  <description>The wj_718_lidar package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="zsy@todo.todo">zsy</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>Apache 2.0</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/wj_718_lidar</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+  <author >zsy</author>
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+
+  <build_depend>dynamic_reconfigure</build_depend>
+
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/wj_718_lidar/src/async_client.cpp

@@ -0,0 +1,148 @@
+#include "async_client.h"
+
+Async_Client::Async_Client(fundata_t fundata_ )
+    :m_fundata(fundata_)
+{
+    m_bConnected = false;
+    m_bReconnecting = false;
+    m_pSocket = boost::shared_ptr<ip::tcp::socket>(new ip::tcp::socket(m_io));
+    cout << "TCP-Connection is initialized!" << endl;
+}
+
+Async_Client::~Async_Client()
+{
+    disconnect();
+    m_fundata = NULL;
+}
+
+bool Async_Client::connect(string ip, int port) 
+{
+    try
+    {
+        if (m_bConnected)
+        {
+            return false;
+        }
+        if(m_pSocket->is_open())
+        {
+            boost::system::error_code errorcode;
+            m_pSocket->shutdown(ip::tcp::socket::shutdown_both, errorcode);
+            m_pSocket->close();
+        }
+        m_sServerIp = ip;
+        m_iServerPort = port;
+        m_ep = ip::tcp::endpoint(ip::address::from_string(ip), port);
+        m_pSocket->connect(m_ep,ec);
+        if(ec)
+        {
+            m_bConnected = false;
+            return false;
+        }
+        else
+        {
+            m_bConnected = true;
+            boost::thread recvThread(boost::bind(&Async_Client::recvData,this));
+            return true;
+        }
+    }
+    catch (boost::exception &e)
+    {
+        return false;
+    }
+}
+
+bool Async_Client::disconnect() 
+{
+    try
+    {
+        cout << "Disconnecting connection!" << endl;
+        m_bConnected = false;
+        if(m_pSocket->is_open())
+        {
+            boost::system::error_code errorcode;
+            m_pSocket->shutdown(ip::tcp::socket::shutdown_both, errorcode);
+            m_pSocket->close();
+        }
+        return true;
+    }
+    catch(boost::exception& e)
+    {
+        return false;
+    }
+}
+
+void Async_Client::recvData()
+{
+    try
+    {
+        while (m_bConnected)
+        {
+            unsigned char buf[10240];
+            size_t len = m_pSocket->read_some(buffer(buf));
+            if (len > 0)
+            {
+                (*m_fundata)(this->m_ep.address().to_string().c_str(),this->m_ep.port(),buf,len);
+            }
+        }
+    }
+    catch (std::exception e)
+    {
+    }
+    m_bConnected = false;
+}
+
+void Async_Client::reconnect()
+{
+    m_bReconnecting = true;
+    while (!m_bConnected)
+    {
+        if (m_pSocket->is_open())
+        {
+            boost::system::error_code errorcode;
+            m_pSocket->shutdown(ip::tcp::socket::shutdown_both, errorcode);
+            m_pSocket->close();
+            cout << "Initializing network!" << endl;
+        }
+        sleep(3);
+        cout << "Start reconnecting laser!" << endl;
+        sleep(2);
+        if (connect(m_sServerIp, m_iServerPort))
+        {
+            cout << "Succesfully connected!" << endl;
+            break;
+        }
+        else
+        {
+            cout << "Failed to reconnect!" << endl;
+        }
+        sleep(2);
+    }
+    m_bReconnecting = false;
+}
+
+bool Async_Client::sendMsg(unsigned char buf[], int length)
+{
+    try
+    {
+        if (m_pSocket->is_open() && m_bConnected)
+        {
+            size_t st = m_pSocket->send(buffer(buf, length));
+            if (st == length)
+            {
+                return true;
+            }
+            else
+            {
+                return false;
+            }
+        }
+        else
+        {
+            return false;
+        }
+    }
+    catch (std::exception e)
+    {
+        return false;
+    }
+}

src/wj_718_lidar/src/async_client.h

@@ -0,0 +1,45 @@
+#ifndef ASYNC_CLIENT_H
+#define ASYNC_CLIENT_H
+#include <iostream>
+#include <stdio.h>
+#include <time.h>
+#include <boost/asio.hpp>
+#include <boost/bind.hpp>
+#include <boost/thread.hpp>
+#include <string>
+#include <unistd.h>
+
+using namespace std ;
+using boost::asio::ip::udp;
+using boost::asio::ip::tcp;
+using namespace boost::asio;
+#define MAX_LENGTH 50000
+typedef void (*fundata_t)(const char* addr,int port,unsigned char* data,const int len);
+class Async_Client
+{
+public:
+    //Async_Client();
+    Async_Client(fundata_t fundata_ );
+    ~Async_Client();
+    bool connect(string ip, int port);
+    bool disconnect();
+    void recvData();
+    void reconnect();
+    bool sendMsg(unsigned char buf[], int length);
+
+    bool        m_bConnected;
+    bool        m_bReconnecting;
+
+private:
+    string m_sServerIp;
+    int m_iServerPort;
+    io_service m_io;
+    ip::tcp::endpoint m_ep;
+    boost::shared_ptr<ip::tcp::socket> m_pSocket;
+    boost::system::error_code ec;
+
+    char        data_[MAX_LENGTH];
+    fundata_t   m_fundata ;
+};
+
+#endif // ASYNC_CLIENT_H

src/wj_718_lidar/src/wj_718_lidar_01.cpp

@@ -0,0 +1,77 @@
+#include <ros/ros.h>
+#include "async_client.h"
+#include "wj_718_lidar_protocol.h"
+using namespace wj_lidar;
+
+/* ------------------------------------------------------------------------------------------
+ *  show demo --
+ * ------------------------------------------------------------------------------------------ */
+wj_718_lidar_protocol *protocol;
+Async_Client *client;
+void CallBackRead(const char* addr,int port,unsigned char* data,const int len)
+{
+    protocol->dataProcess(data,len);
+}
+
+void callback(wj_718_lidar::wj_718_lidarConfig &config,uint32_t level)
+{
+    protocol->setConfig(config,level);
+}
+
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "wj_718_lidar_01");
+    ros::NodeHandle nh("~");
+    std::string hostname;
+    nh.getParam("hostname",hostname);
+    std::string port;
+    nh.getParam("port",port);
+    cout << "laser ip: " << hostname << ", port:" << port <<endl;
+
+    protocol = new wj_718_lidar_protocol();
+    dynamic_reconfigure::Server<wj_718_lidar::wj_718_lidarConfig> server;
+    dynamic_reconfigure::Server<wj_718_lidar::wj_718_lidarConfig>::CallbackType f;
+    f = boost::bind(&callback,_1,_2);
+    server.setCallback(f);
+
+    client = new Async_Client(&CallBackRead);
+    protocol->heartstate = false;
+    while(!client->m_bConnected)
+    {
+        ROS_INFO("Start connecting laser!");
+        if(client->connect(hostname.c_str(),atoi(port.c_str())))
+        {
+            ROS_INFO("Succesfully connected. Hello wj_718_lidar!");
+        }
+        else
+        {
+            ROS_INFO("Failed to connect to laser. Waiting 5s to reconnect!");
+        }
+        ros::Duration(5).sleep();
+    }
+    while(ros::ok())
+    {
+        ros::spinOnce();
+        ros::Duration(2).sleep();
+        if(client->m_bConnected)
+        {
+            if(protocol->heartstate)
+            {
+                protocol->heartstate = false;
+            }
+            else
+            {
+                client->m_bConnected = false;
+            }
+        }
+        else
+        {
+            //reconnect
+            if(!client->m_bReconnecting)
+            {
+                boost::thread t(boost::bind(&Async_Client::reconnect, client));
+            }
+        }
+    }
+}

src/wj_718_lidar/src/wj_718_lidar_protocol.cpp

@@ -0,0 +1,291 @@
+#include "wj_718_lidar_protocol.h"
+#include <iostream>
+
+namespace wj_lidar
+{
+bool wj_718_lidar_protocol::setConfig(wj_718_lidar::wj_718_lidarConfig &new_config,uint32_t level)
+{
+    config_ = new_config;
+    scan.header.frame_id = config_.frame_id;
+    scan.angle_min = config_.min_ang;
+    scan.angle_max = config_.max_ang;
+    scan.angle_increment = config_.angle_increment;
+    scan.time_increment = config_.time_increment;
+    scan.range_min = config_.range_min;
+    scan.range_max = config_.range_max;
+    freq_scan = config_.frequency_scan;
+    resizeNum = config_.resize;
+    if(freq_scan == 0)  //15hz/0.33
+    {
+        scan.angle_increment = config_.angle_increment / 3;
+        scan.time_increment = 1/15.00000000/811;
+        resizeNum = 811;
+    }
+
+    scan.ranges.resize(resizeNum);
+    scan.intensities.resize(resizeNum);
+
+    cout << "frame_id:" <<scan.header.frame_id<<endl;
+    cout << "min_ang:" <<scan.angle_min<<endl;
+    cout << "max_ang:" <<scan.angle_max<<endl;
+    cout << "angle_increment:" <<scan.angle_increment<<endl;
+    cout << "time_increment:" <<scan.time_increment<<endl;
+    cout << "range_min:" <<scan.range_min<<endl;
+    cout << "range_max:" <<scan.range_max<<endl;
+    cout << "resizeNum:" <<resizeNum<<endl;
+    return true;
+}
+
+wj_718_lidar_protocol::wj_718_lidar_protocol()
+{
+    memset(&m_sdata,0,sizeof(m_sdata));
+    marker_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 50);
+    ros::Time scan_time = ros::Time::now();      //make a virtual data per sec
+    scan.header.stamp = scan_time;
+
+    g_u32PreFrameNo = 0;
+
+    scan.header.frame_id = "wj_718_lidar_frame";
+    scan.angle_min = -2.35619449;
+    scan.angle_max = 2.35619449;
+    scan.angle_increment = 0.00582;
+    scan.time_increment = 1/15.00000000/811;
+    scan.range_min = 0;
+    scan.range_max = 10;
+    scan.ranges.resize(811);
+    scan.intensities.resize(811);
+
+    cout << "wj_718_lidar_protocl start success" << endl;
+
+}
+bool wj_718_lidar_protocol::dataProcess(unsigned char *data, const int reclen)
+{
+    if(reclen > MAX_LENGTH_DATA_PROCESS)
+    {
+        return false;
+    }
+
+    if(m_sdata.m_u32in + reclen > MAX_LENGTH_DATA_PROCESS)
+    {
+        memset(&m_sdata,0,sizeof(m_sdata));
+        return false;
+    }
+    memcpy(&m_sdata.m_acdata[m_sdata.m_u32in],data,reclen*sizeof(char));
+    m_sdata.m_u32in += reclen;
+    while(m_sdata.m_u32out < m_sdata.m_u32in)
+    {
+        if(m_sdata.m_acdata[m_sdata.m_u32out] == 0xFF && m_sdata.m_acdata[m_sdata.m_u32out+1] == 0xAA)
+        {
+            unsigned l_u32reallen = (m_sdata.m_acdata[m_sdata.m_u32out + 2] << 8)  |
+                    (m_sdata.m_acdata[m_sdata.m_u32out + 3] << 0);
+            l_u32reallen = l_u32reallen + 4;
+
+            if(l_u32reallen <= (m_sdata.m_u32in - m_sdata.m_u32out + 1))
+            {
+                if(OnRecvProcess(&m_sdata.m_acdata[m_sdata.m_u32out],l_u32reallen))
+                {
+                    m_sdata.m_u32out += l_u32reallen;
+                }
+                else
+                {
+                    cout << "continuous frame"<<endl;
+                    int i;
+                    for(i = 1; i<=l_u32reallen; i++)
+                    {
+                        if((m_sdata.m_acdata[m_sdata.m_u32out + i] == 0xFF) &&
+                                (m_sdata.m_acdata[m_sdata.m_u32out + i + 1] == 0xAA))
+                        {
+                            m_sdata.m_u32out += i;
+                            break;
+                        }
+                        if(i == l_u32reallen)
+                        {
+                            m_sdata.m_u32out += l_u32reallen;
+                        }
+                    }
+                }
+            }
+            else if(l_u32reallen >= MAX_LENGTH_DATA_PROCESS)
+            {
+                cout << "l_u32reallen >= MAX_LENGTH_DATA_PROCESS"<<endl;
+                cout << "reallen: "<<l_u32reallen<<endl;
+                memset(&m_sdata,0,sizeof(m_sdata));
+
+            }
+            else
+            {
+                //cout<<"reallen: "<<l_u32reallen<<" indata: "<<m_sdata.m_u32in<<" outdata: "<<m_sdata.m_u32out<<endl;
+                break;
+            }
+        }
+        else
+        {
+            m_sdata.m_u32out++;
+        }
+    } //end while(m_sdata.m_u32out < m_sdata.m_u32in)
+
+    if(m_sdata.m_u32out >= m_sdata.m_u32in)
+    {
+        memset(&m_sdata,0,sizeof(m_sdata));
+    }
+    return true;
+}
+
+bool wj_718_lidar_protocol::OnRecvProcess(unsigned char *data, int len)
+{
+    if(len > 0)
+    {
+        if(checkXor(data,len))
+        {
+            protocl(data,len);
+        }
+    }
+    else
+    {
+        return false;
+    }
+    return true;
+}
+
+bool wj_718_lidar_protocol::protocl(unsigned char *data, const int len)
+{
+    if((data[22] == 0x02 && data[23] == 0x02)||(data[22] == 0x02 && data[23] == 0x01) )   //command type:0x02 0x01/0X02
+    {
+        static int s_n32ScanIndex;
+        int l_n32TotalPackage= data[35];                                     //total package
+        int l_n32PackageNo= data[36];                                        //package number
+        unsigned int l_u32FrameNo = (data[31]<<24) + (data[32]<<16) + (data[33]<<8) + data[34];        //frame number
+
+        if((freq_scan == 0 && l_n32TotalPackage != 2) ||(freq_scan == 1 && l_n32TotalPackage != 1))
+        {
+            heartstate = true;
+            cout << "The scan frequency does not match the resolution!"<< endl;
+            return false;
+        }
+
+        if(freq_scan == 0)  //15hz/0.33
+        {
+            if(l_n32PackageNo == 1)
+            {
+                s_n32ScanIndex = 0;
+                g_u32PreFrameNo = l_u32FrameNo;
+                heartstate = true;
+                int l_n32PointCount = (data[37]<<8) + data[38];
+                for(int j = 0; j < l_n32PointCount;j++)
+                {
+                    scandata[s_n32ScanIndex] = ((data[39+j*2]) << 8) + (data[40+j*2]);
+                    scandata[s_n32ScanIndex] /= 1000.0;
+                    if(scandata[s_n32ScanIndex]>=10 || scandata[s_n32ScanIndex]==0)
+                    {
+                        scandata[s_n32ScanIndex]= NAN;
+                    }
+                    s_n32ScanIndex++;
+                }
+            }
+            else if(l_n32PackageNo == 2)
+            {
+                if(g_u32PreFrameNo == l_u32FrameNo)
+                {
+                    int l_n32PointCount = (data[37]<<8) + data[38];
+                    for(int j = 0; j < l_n32PointCount;j++)
+                    {
+                        scandata[s_n32ScanIndex] = ((data[39+j*2]) << 8) + (data[40+j*2]);
+                        scandata[s_n32ScanIndex] /= 1000.0;
+                        if(scandata[s_n32ScanIndex]>=10 || scandata[s_n32ScanIndex]==0)
+                        {
+                            scandata[s_n32ScanIndex]= NAN;
+                        }
+                        s_n32ScanIndex++;
+                    }
+
+                    for(int j = 0; j < resizeNum; j++)
+                    {
+                        scan.ranges[resizeNum - 1 -j] = scandata[j];
+                    }
+
+                    ros::Time scan_time = ros::Time::now();
+                    scan.header.stamp = scan_time;
+                    marker_pub.publish(scan);
+                }
+                else
+                {
+                    s_n32ScanIndex = 0;
+                    return false;
+                }
+            }
+            else
+            {
+                return false;
+            }
+        }
+        else  //25hz/1
+        {
+            if(l_n32PackageNo == 1)
+            {
+                s_n32ScanIndex = 0;
+                g_u32PreFrameNo = l_u32FrameNo;
+                heartstate = true;
+                int l_n32PointCount = (data[37]<<8) + data[38];
+
+                for(int j = 0; j < l_n32PointCount;j++)
+                {
+                    scandata[s_n32ScanIndex] = ((data[39+j*2]) << 8) + (data[40+j*2]);
+                    scandata[s_n32ScanIndex] /= 1000.0;
+                    if(scandata[s_n32ScanIndex]>=10 || scandata[s_n32ScanIndex]==0)
+                    {
+                        scandata[s_n32ScanIndex]= NAN;
+                    }
+                    s_n32ScanIndex++;
+                }
+
+                for(int j = 0; j < resizeNum; j++)
+                {
+                    scan.ranges[resizeNum - 1 -j] = scandata[j];
+                }
+
+                ros::Time scan_time = ros::Time::now();
+                scan.header.stamp = scan_time;
+                marker_pub.publish(scan);
+            }
+            else
+            {
+                return false;
+            }
+        }
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+bool wj_718_lidar_protocol::checkXor(unsigned char *recvbuf,  int recvlen)
+{
+    int i = 0;
+    unsigned char check = 0;
+    unsigned char *p = recvbuf;
+    int len;
+    if(*p == 0xFF)
+    {
+        p = p+2;
+        len = recvlen - 6;
+        for(i = 0; i < len; i++)
+        {
+            check ^= *p++;
+        }
+        p++;
+        if(check == *p)
+        {
+            return true;
+        }
+        else
+            return false;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+}

src/wj_718_lidar/src/wj_718_lidar_protocol.h

@@ -0,0 +1,53 @@
+#ifndef WJ_718_LIDAR_PROTOCOL_H
+#define WJ_718_LIDAR_PROTOCOL_H
+#include <iostream>
+#include "string.h"
+#include <boost/shared_ptr.hpp>
+#include <boost/asio.hpp>
+#include <boost/asio/placeholders.hpp>
+#include <boost/system/error_code.hpp>
+#include <boost/bind/bind.hpp>
+#include <ros/ros.h>
+#include <visualization_msgs/Marker.h>
+#include <sensor_msgs/LaserScan.h>
+#include <dynamic_reconfigure/server.h>
+#include <wj_718_lidar/wj_718_lidarConfig.h>
+using namespace std ;
+namespace wj_lidar
+{
+#define MAX_LENGTH_DATA_PROCESS 200000
+typedef struct TagDataCache
+{
+    unsigned char m_acdata[MAX_LENGTH_DATA_PROCESS];
+    unsigned int m_u32in;
+    unsigned int m_u32out;
+}DataCache;
+
+class wj_718_lidar_protocol
+{
+public:
+    wj_718_lidar_protocol();
+    bool dataProcess(unsigned char *data,const int reclen);
+    bool protocl(unsigned char *data,const int len);
+    bool OnRecvProcess(unsigned char *data, int len);
+    bool checkXor(unsigned char *recvbuf, int recvlen);
+    void send_scan(const char *data,const int len);
+    ros::NodeHandle nh;
+    ros::Publisher marker_pub;
+    sensor_msgs::LaserScan scan;
+    bool setConfig(wj_718_lidar::wj_718_lidarConfig &new_config,uint32_t level);
+    bool heartstate;
+private:
+    unsigned char        data_[MAX_LENGTH_DATA_PROCESS];
+    DataCache   m_sdata;
+    wj_718_lidar::wj_718_lidarConfig config_;
+    unsigned int g_u32PreFrameNo;
+    float scandata[811];
+    float scandata_te[811];
+    float scaninden[811];
+    int freq_scan;
+    int resizeNum;
+};
+
+}
+#endif // WJ_718_LIDAR_PROTOCOL_H