20201124 mpc with slowdown weight

model need double check

.gitignore

@@ -34,3 +34,4 @@ build/*
 devel/*
 *vscode*
 .catkin_workspace
+*.bag

src/MPC/launch/MPC_2.launch

@@ -8,8 +8,8 @@
     <node pkg="turtlesim" name="sim" type="turtlesim_node"/>
   </group>
 
-  <node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/blank_map.yaml"/>
-  <node pkg="MPC" type="MPC2_node" name="MPC2_node" output="screen"/>
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find MPC)/maps/blank_map.yaml"/>
+  <!-- <node pkg="MPC" type="MPC2_node" name="MPC2_node" output="screen"/> -->
   <node name="rviz" pkg="rviz" type="rviz" args="-d $(find MPC)/launch/default.rviz" />
 
 </launch>

src/MPC/src/mpc/TowBotMPC.cpp

@@ -34,7 +34,7 @@ double Ly1 = 0;
 double Lx2 = 1.3;
 double Ly2 = 0;
 
-double ref_dist = 2.0;
+double ref_dist = 1.0;
 // double min_dist = 1.5;
 // double max_dist = 3.0;
 
@@ -44,6 +44,7 @@ public:
     // Fitted polynomial coefficients
     Eigen::VectorXd coeffs, coeffs_b;
     Eigen::VectorXd m_data;
+
     FG_eval_2(Eigen::VectorXd coeffs, Eigen::VectorXd coeffs_b, Eigen::VectorXd P)
     {
         this->coeffs = coeffs;
@@ -68,8 +69,13 @@ public:
         double dy = m_data[7];
         double dth = m_data[8]; ///两坐标系角度
 
+        double target_dx = m_data[9];
+        double target_dy = m_data[10];
+        double slowdown_dist = m_data[11];
+
+        const int brake_weight = 200;
         ///前车 cost weight
-        const int y_cost_weight = 5000;
+        const int y_cost_weight = 15000;
         const int th_cost_weight = 1000;
         const int v_cost_weight = 20;
 
@@ -79,7 +85,7 @@ public:
 
         fg[0] = 0;
         ///后车 cost weight
-        const int back_y_cost_weight = 5000;
+        const int back_y_cost_weight = 15000;
         const int back_th_cost_weight = 1000;
         const int back_v_cost_weight = 20;
 
@@ -87,65 +93,6 @@ public:
         const int back_a_cost_weight = 1;
         const int back_ath_cost_weight = 10;
 
-        for (int t = 0; t < K; t++)
-        {
-            // 前车坐标系下，y坐标差
-            AD<double> xt = vars[nx1 + t];
-            AD<double> fx = coeffs[0] + coeffs[1] * xt + coeffs[2] * pow(xt, 2) + coeffs[3] * pow(xt, 3);
-            fg[0] += y_cost_weight * CppAD::pow(vars[ny1 + t] - fx, 2);
-
-            // 前车坐标系下，theta cost & v cost
-            AD<double> fth = CppAD::atan(coeffs[1] + 2 * coeffs[2] * xt + 3 * coeffs[3] * pow(xt, 2));
-            fg[0] += th_cost_weight * CppAD::pow(vars[nth1 + t] - fth, 2);
-            fg[0] += v_cost_weight * CppAD::pow(vars[nv1 + t] - ref_v, 2);
-            fg[0] += vth_cost_weight * CppAD::pow(vars[nvth1 + t], 2);
-
-            // 在前车坐标系下，后车y坐标差
-            AD<double> xt2 = vars[nx2 + t];
-            AD<double> fx2 = coeffs_b[0] + coeffs_b[1] * xt2 + coeffs_b[2] * pow(xt2, 2) + coeffs_b[3] * pow(xt2, 3);
-            fg[0] += back_y_cost_weight * CppAD::pow(vars[ny2 + t] - fx2, 2);
-
-            // 在前车坐标系下，后车theta cost & v cost
-            AD<double> fth2 = CppAD::atan(coeffs_b[1] + 2 * coeffs_b[2] * xt2 + 3 * coeffs_b[3] * pow(xt2, 2));
-            fg[0] += back_th_cost_weight * CppAD::pow(vars[nth2 + t] - fth2, 2);
-            fg[0] += back_v_cost_weight * CppAD::pow(vars[nv2 + t] - ref_v, 2);
-            fg[0] += back_vth_cost_weight * CppAD::pow(vars[nvth2 + t], 2);
-        }
-        //加速度 cost
-        // Costs for steering (delta) and acceleration (a)
-        for (int t = 0; t < K - 1; t++)
-        {
-            fg[0] += a_cost_weight * CppAD::pow(vars[nacc1+ t], 2);
-            fg[0] += ath_cost_weight * CppAD::pow(vars[naccth1 + t], 2);
-
-            fg[0] += back_a_cost_weight * CppAD::pow(vars[nacc2 + t], 2);
-            fg[0] += back_ath_cost_weight * CppAD::pow(vars[naccth2 + t], 2);
-        }
-
-        ///添加两车距离损失
-        const int distance_weight = 800;
-        const int delta_th_weight = 10;
-        char buf[255]={0};
-        AD<double> dtheta = vars[nth1] - vars[nth2] + dth;
-        for (int t = 0; t < K; ++t)
-        {
-            AD<double> x1_in_front = vars[nx1 + t];
-            AD<double> y1_in_front = vars[ny1 + t];
-            ///将坐标旋转到后车坐标系下
-            AD<double> x1_in_back = CppAD::cos(dtheta) * x1_in_front - CppAD::sin(dtheta) * y1_in_front + dx;
-            AD<double> y1_in_back = CppAD::sin(dtheta) * x1_in_front + CppAD::cos(dtheta) * y1_in_front + dy;
-
-            AD<double> distance_x = x1_in_back - vars[nx2 + t];
-            AD<double> distance_y = y1_in_back - vars[ny2 + t];
-
-            fg[0] += distance_weight * CppAD::pow(CppAD::sqrt(CppAD::pow(distance_x, 2) + CppAD::pow(distance_y, 2)) - ref_dist, 2);
-            fg[0] += delta_th_weight * CppAD::pow(CppAD::atan(y1_in_back / x1_in_back), 2);
-            //sprintf(buf+strlen(buf), " %.4f ", sqrt(pow(distance_x,2)+pow(distance_y,2)));
-            // // 两车最短距离约束
-            // fg[1 + relative_pose_contrains + t - 4*(K-1)] = CppAD::sqrt(CppAD::pow(distance_x, 2) + CppAD::pow(distance_y, 2));
-        }
-        //std::cout<<buf<<std::endl;
-
         // x, y, theta 路径约束
         fg[1 + nx1] = vars[nx1];// - vars[nv1] * dt;
         fg[1 + ny1] = vars[ny1];
@@ -159,6 +106,10 @@ public:
         fg[1 + nv2 - 2*(K-1)] = vars[nv2];
         fg[1 + nvth2 - 2*(K-1)] = vars[nvth2];
 
+        AD<double> accumulate_fx = vars[nx1];
+        AD<double> accumulate_fy = vars[ny1];
+        AD<double> accumulate_bx = vars[nx2];
+        AD<double> accumulate_by = vars[ny2];
         // The rest of the constraints
         for (int t = 1; t < K; t++)
         {
@@ -178,6 +129,9 @@ public:
             AD<double> facc0 = vars[nacc1 + t - 1];
             AD<double> faccth0 = vars[naccth1 + t - 1];
 
+            accumulate_fx += fv0 * CppAD::cos(fth0) * dt;
+            accumulate_fy += fv0 * CppAD::sin(fth0) * dt;
+
             fg[1 + nx1 + t] = fx1 - (fx0 + fv0 * CppAD::cos(fth0) * dt);
             fg[1 + ny1 + t] = fy1 - (fy0 + fv0 * CppAD::sin(fth0) * dt);
             fg[1 + nth1 + t] = fth1 - (fth0 + fvth0 * dt);
@@ -200,15 +154,96 @@ public:
             AD<double> bacc0 = vars[nacc2 + t - 1];
             AD<double> baccth0 = vars[naccth2 + t - 1];
 
+            accumulate_bx += bv0 * CppAD::cos(bth0) * dt;
+            accumulate_by += bv0 * CppAD::sin(bth0) * dt;
+
             fg[1 + nx2 + t - 2*(K-1)] = bx1 - (bx0 + bv0 * CppAD::cos(bth0) * dt);
             fg[1 + ny2 + t - 2*(K-1)] = by1 - (by0 + bv0 * CppAD::sin(bth0) * dt);
             fg[1 + nth2 + t - 2*(K-1)] = bth1 - (bth0 + bvth0 * dt);
             fg[1 + nv2 + t - 2*(K-1)] = bv1 - (bv0 + bacc0 * dt);
             fg[1 + nvth2 + t - 2*(K-1)] = bvth1 - (bvth0 + baccth0 * dt);
         }
+
+        // 筛选前进方向前车到目标偏移
+        double current_dist = pow(target_dx, 2) + pow(target_dy, 2);
+        double target_forward_weight = 1.0 / (1.0 + exp(-1000.0 * ref_v));
+        double target_backward_weight = 1.0 / (1.0 + exp(1000.0 * ref_v));
+        AD<double> final_dx = CppAD::pow(accumulate_fx-target_dx, 2) * target_forward_weight + CppAD::pow(accumulate_bx-target_dx, 2) * target_backward_weight;
+        AD<double> final_dy = CppAD::pow(accumulate_fy-target_dy, 2) * target_forward_weight + CppAD::pow(accumulate_by-target_dy, 2) * target_backward_weight;
+        // 预测末端与目标点偏移
+        // AD<double> final_dist = CppAD::sqrt(final_dx + final_dy);
+
+        // 减速模式权重,越接近目标,该权重越大,最终减速到达目标
+        double slowdown_weight = 1.0 / (1.0 + exp(10.0 * sqrt(current_dist) - 5.0));
+        // char disp[255];
+        // memset(disp, 0, 255);
+        // sprintf(disp, "to target: %.3f, pred dist: %.3f, slowdown weight: %.3f\n", current_dist, final_dx, slowdown_weight);
+        // std::cout << disp << std::endl;
+
+        for (int t = 0; t < K; t++)
+        {
+            // 前车坐标系下，y坐标差
+            AD<double> xt = vars[nx1 + t];
+            AD<double> fx = coeffs[0] + coeffs[1] * xt + coeffs[2] * pow(xt, 2) + coeffs[3] * pow(xt, 3);
+            fg[0] += y_cost_weight * CppAD::pow(vars[ny1 + t] - fx, 2);
+
+            // 前车坐标系下，theta cost & v cost
+            AD<double> fth = CppAD::atan(coeffs[1] + 2 * coeffs[2] * xt + 3 * coeffs[3] * pow(xt, 2));
+            fg[0] += th_cost_weight * CppAD::pow(vars[nth1 + t] - fth, 2);
+            // 修改运动速度损失函数, 从ref_v 转到 接近终点时的最终预测点的dist误差与v误差
+            fg[0] += (1-slowdown_weight)*v_cost_weight * CppAD::pow(vars[nv1 + t] - ref_v, 2) + slowdown_weight * brake_weight * (final_dx / K + final_dy / K + CppAD::pow(vars[nv1 + K - 1], 2));
+            fg[0] += vth_cost_weight * CppAD::pow(vars[nvth1 + t], 2);
+
+            // 在后车坐标系下，后车y坐标差
+            AD<double> xt2 = vars[nx2 + t];
+            AD<double> fx2 = coeffs_b[0] + coeffs_b[1] * xt2 + coeffs_b[2] * pow(xt2, 2) + coeffs_b[3] * pow(xt2, 3);
+            fg[0] += back_y_cost_weight * CppAD::pow(vars[ny2 + t] - fx2, 2);
+
+            // 在后车坐标系下，后车theta cost & v cost
+            AD<double> fth2 = CppAD::atan(coeffs_b[1] + 2 * coeffs_b[2] * xt2 + 3 * coeffs_b[3] * pow(xt2, 2));
+            fg[0] += back_th_cost_weight * CppAD::pow(vars[nth2 + t] - fth2, 2);
+            // 修改运动速度损失函数
+            fg[0] += (1-slowdown_weight)*back_v_cost_weight * CppAD::pow(vars[nv2 + t] - ref_v, 2) + slowdown_weight * brake_weight * (final_dx / K + final_dy / K + CppAD::pow(vars[nv2 + K - 1], 2));
+            fg[0] += back_vth_cost_weight * CppAD::pow(vars[nvth2 + t], 2);
+        }
+        //加速度 cost
+        // Costs for steering (delta) and acceleration (a)
+        for (int t = 0; t < K - 1; t++)
+        {
+            fg[0] += a_cost_weight * CppAD::pow(vars[nacc1+ t], 2);
+            fg[0] += ath_cost_weight * CppAD::pow(vars[naccth1 + t], 2);
+
+            fg[0] += back_a_cost_weight * CppAD::pow(vars[nacc2 + t], 2);
+            fg[0] += back_ath_cost_weight * CppAD::pow(vars[naccth2 + t], 2);
+        }
+
+        ///添加两车距离损失
+        const int distance_weight = 800;
+        const int delta_th_weight = 10;
+        char buf[255]={0};
+        AD<double> dtheta = vars[nth1] - vars[nth2] + dth;
+        for (int t = 0; t < K; ++t)
+        {
+            AD<double> x1_in_front = vars[nx1 + t];
+            AD<double> y1_in_front = vars[ny1 + t];
+            ///将坐标旋转到后车坐标系下
+            AD<double> x1_in_back = CppAD::cos(dtheta) * x1_in_front - CppAD::sin(dtheta) * y1_in_front + dx;
+            AD<double> y1_in_back = CppAD::sin(dtheta) * x1_in_front + CppAD::cos(dtheta) * y1_in_front + dy;
+
+            AD<double> distance_x = x1_in_back - vars[nx2 + t];
+            AD<double> distance_y = y1_in_back - vars[ny2 + t];
+
+            fg[0] += distance_weight * CppAD::pow(CppAD::sqrt(CppAD::pow(distance_x, 2) + CppAD::pow(distance_y, 2)) - ref_dist, 2);
+            fg[0] += delta_th_weight * CppAD::pow(CppAD::atan(y1_in_back / x1_in_back), 2);
+            //sprintf(buf+strlen(buf), " %.4f ", sqrt(pow(distance_x,2)+pow(distance_y,2)));
+            // // 两车最短距离约束
+            // fg[1 + relative_pose_contrains + t - 4*(K-1)] = CppAD::sqrt(CppAD::pow(distance_x, 2) + CppAD::pow(distance_y, 2));
+        }
+        //std::cout<<buf<<std::endl;
     }
 };
 
+#include <chrono>
 bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v, Eigen::VectorXd coeffs, Eigen::VectorXd coeffs_b, std::vector<double> &out)
 {
     bool ok = true;
@@ -263,20 +298,26 @@ bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v, Eigen::VectorXd coeff
         vars_upperbound[nv2 + i] = max_v;
 
         //// limit  vth1 2
-        vars_lowerbound[nvth1 + i] = -0.5;
-        vars_upperbound[nvth1 + i] = 0.5;
+        vars_lowerbound[nvth1 + i] = -0.25;
+        vars_upperbound[nvth1 + i] = 0.25;
 
-        vars_lowerbound[nvth2 + i] = -0.5;
-        vars_upperbound[nvth2 + i] = 0.5;
+        vars_lowerbound[nvth2 + i] = -0.25;
+        vars_upperbound[nvth2 + i] = 0.25;
     }
     for (int i = 0; i < K - 1; i++)
     {
         //// limit acc1 acc2
-        vars_lowerbound[nacc1 + i] = -5.0;
-        vars_upperbound[nacc2 + i] = 5.0;
+        vars_lowerbound[nacc1 + i] = -1.0;
+        vars_upperbound[nacc1 + i] = 1.0;
+
+        vars_lowerbound[nacc2 + i] = -1.0;
+        vars_upperbound[nacc2 + i] = 1.0;
 
         //// limit accth1 accth2
         vars_lowerbound[naccth1 + i] = -0.05;//3度
+        vars_upperbound[naccth1 + i] = 0.05;
+
+        vars_lowerbound[naccth2 + i] = -0.05;//3度
         vars_upperbound[naccth2 + i] = 0.05;
     }
     
@@ -290,7 +331,7 @@ bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v, Eigen::VectorXd coeff
         constraints_upperbound[i] = 0;
     }
 
-    for (int i = 1; i < K; i++)
+    for (int i = 0; i < 1; i++)
     {
         ////acc v1 v2
         constraints_lowerbound[nv1 + i] = -3.0;
@@ -307,17 +348,17 @@ bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v, Eigen::VectorXd coeff
         // constraints_lowerbound[relative_pose_contrains + i - 4*(K-1)] = min_dist;
         // constraints_upperbound[relative_pose_contrains + i - 4*(K-1)] = max_dist;
     }
-    // acc v1 v2
-    constraints_lowerbound[nv1]=v1-0.1;
-    constraints_upperbound[nv1]=v1+0.1;
-    constraints_lowerbound[nv2-2*(K-1)]=v2-0.1;
-    constraints_upperbound[nv2-2*(K-1)]=v2+0.1;
-
-    //acc th1 th2
-    constraints_lowerbound[nvth1]=vth1-0.017;
-    constraints_upperbound[nvth1]=vth1+0.017;
-    constraints_lowerbound[nvth2-2*(K-1)]=vth2-0.1;
-    constraints_upperbound[nvth2-2*(K-1)]=vth2+0.1;
+    // // acc v1 v2
+    // constraints_lowerbound[nv1]=v1-0.1;
+    // constraints_upperbound[nv1]=v1+0.1;
+    // constraints_lowerbound[nv2-2*(K-1)]=v2-0.1;
+    // constraints_upperbound[nv2-2*(K-1)]=v2+0.1;
+
+    // //acc th1 th2
+    // constraints_lowerbound[nvth1]=vth1-0.017;
+    // constraints_upperbound[nvth1]=vth1+0.017;
+    // constraints_lowerbound[nvth2-2*(K-1)]=vth2-0.1;
+    // constraints_upperbound[nvth2-2*(K-1)]=vth2+0.1;
 
     // object that computes objective and constraintsv
     FG_eval_2 fg_eval(coeffs, coeffs_b, state);
@@ -344,17 +385,17 @@ bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v, Eigen::VectorXd coeff
     out.clear();
     m_trajectory_f.clear();
     m_trajectory_b.clear();
-    float speed1=solution.x[nv1];
-    if(fabs(speed1)<0.1)
-    {
-        speed1=speed1>0?0.1:-0.1;
-    }
-
-    float speed2=solution.x[nv2];
-    if(fabs(speed2)<0.1)
-    {
-        speed2=speed2>0?0.1:-0.1;
-    }
+    // float speed1=solution.x[nv1];
+    // if(fabs(speed1)<0.1)
+    // {
+    //     speed1=speed1>0?0.1:-0.1;
+    // }
+
+    // float speed2=solution.x[nv2];
+    // if(fabs(speed2)<0.1)
+    // {
+    //     speed2=speed2>0?0.1:-0.1;
+    // }
     out.push_back(solution.x[nv1]);
     out.push_back(solution.x[nvth1]);
     out.push_back(solution.x[nv2]);
@@ -367,16 +408,30 @@ bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v, Eigen::VectorXd coeff
         m_trajectory_f.push_back(pose1);
         m_trajectory_b.push_back(pose2);
     }
-
-    // printf("Cost : %.3f ref:%.3f  cv : %.3f  cvth : %.3f  ||  v2:%.3f  th2:%.3f\n",solution.obj_value,
-    //     ref_v,v2,vth2,solution.x[nv2],solution.x[nvth2]);
-
+    static double velo = 0;
+    static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now();
+    if (velo != 0)
+    {
+        double interval = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - last_time).count() * 0.001;
+        double acc = (solution.x[nv1] - velo) / dt;
+        // printf("is ok? %d, cost: %.3f\n", ok, cost);
+        printf("Cost: %.3f\tv1:%.3f\tvth1:%.3f\t||\tnv1:%.3f\tnvth1:%.3f\tnacc1:%.3f\tinterval:%.3f\n", solution.obj_value, v1, vth1, solution.x[nv1], solution.x[nvth1], acc, interval);
+        
+        // printf("Cost : %.3f\tref:%.3f\tcv : %.3f\tcvth : %.3f\t||\tv2:%.3f\tth2:%.3f\n",solution.obj_value,
+        //     ref_v,v2,vth2,solution.x[nv2],solution.x[nvth2]);
+    }
+    velo = solution.x[nv1];
+    last_time = std::chrono::steady_clock::now();
     return ok;
 }
 
 TowBotMPC::TowBotMPC()
 {
+    // ROS_WARN("构造");
+    // std::cout << "构造" << std::endl;
 }
 TowBotMPC::~TowBotMPC()
 {
+    // ROS_WARN("析构");
+    // std::cout << "析构" << std::endl;
 }

src/MPC/src/node2.cpp

@@ -182,6 +182,7 @@ bool MPC()
 
     // 设置线速度、角速度，以及终点判定
     double dis_th=0,offsetx = 0, offsety = 0,liner_vx=0,angular_vz=0;
+    // ?????
     if (g_direction == 1) {
         dis_th=fabs(yaw - theta);
         offsetx = g_goal.getOrigin().getX() - g_pose_f.getOrigin().getX();
@@ -214,11 +215,29 @@ bool MPC()
 
     tf::Vector3 length(dx, dy, 0);
     double direct_x = dx * cos(-theta) - dy * sin(-theta);
-    double ref_v = length.length() / (g_max_vx * 1.5);
-    ref_v = std::max(ref_v, 0.02);
-    ref_v = std::min(ref_v, g_max_vx);
+    double ref_v = g_max_vx;
+    // double ref_v = length.length() / (g_max_vx * 1.5);
+    // ref_v = std::max(ref_v, 0.02);
+    // ref_v = std::min(ref_v, g_max_vx);
     if (direct_x < 0) ref_v = -ref_v;
 
+    // added by yct. 计算当前到目标的偏移(在靠近目标的车的坐标系下)
+    double target_dx, target_dy;
+    if (direct_x > 0)
+    {
+        offsetx = g_goal.getOrigin().getX() - g_pose_f.getOrigin().getX();
+        offsety = g_goal.getOrigin().getY() - g_pose_f.getOrigin().getY();
+        target_dx = offsetx * cos(-theta) - offsety * sin(-theta);
+        target_dy = offsetx * sin(-theta) + offsety * cos(-theta);
+        // std::cout << " forward target dx dy: " << target_dx << ", " << target_dy << std::endl;
+    }else{
+        offsetx = g_goal.getOrigin().getX() - g_pose_b.getOrigin().getX();
+        offsety = g_goal.getOrigin().getY() - g_pose_b.getOrigin().getY();
+        target_dx = offsetx * cos(-theta_b) - offsety * sin(-theta_b);
+        target_dy = offsetx * sin(-theta_b) + offsety * cos(-theta_b);
+        // std::cout << " backward target dx dy: " << target_dx << ", " << target_dy << std::endl;
+    }
+
     ///// 选点
     std::vector<tf::Pose> poses;
     std::vector<tf::Pose> poses_b;
@@ -295,7 +314,7 @@ bool MPC()
     }
     Eigen::VectorXd coeffs_b = mpc_tool.polyfit(ptsx_car_b, ptsy_car_b, 3);
 
-    Eigen::VectorXd state(9);
+    Eigen::VectorXd state(12);
     double vx1 = g_cmd_vel_f.linear.x;
     double vx2 = g_cmd_vel_b.linear.x;
     double vth1 = g_cmd_vel_f.angular.z;
@@ -315,7 +334,9 @@ bool MPC()
         g_pose_f.getOrigin().getY(),g_pose_b.getOrigin().getX(),g_pose_b.getOrigin().getY(),
         g_cmd_vel_f.linear.x,g_cmd_vel_f.angular.z,g_cmd_vel_b.linear.x,g_cmd_vel_b.angular.z);*/
 
-    state << vx1, vx2, vth1, vth2, ref_v, dt, dx1, dy1, dth1;
+    double slowdown_distance = 2.0;
+    state << vx1, vx2, vth1, vth2, ref_v, dt, dx1, dy1, dth1, target_dx, target_dy, slowdown_distance;
+    // std::cout << " target dx dy: " << target_dx << ", " << target_dy << std::endl;
     TowBotMPC mpc;
     std::vector<double> out;
     if (mpc.Solve(state, g_max_vx, coeffs, coeffs_b, out)) {
@@ -448,7 +469,7 @@ int main(int argc ,char* argv[])
     // std::cout<<"  input max vx : ";
     // std::cin>>g_max_vx;
     // std::cout<<std::endl;
-    g_max_vx = 1.0;
+    g_max_vx = 1.5;
 
     ros::init(argc, argv, "MPC_node");
 

src/agv_bundle/configuration_files/200521map_origin.yaml

@@ -1,4 +1,4 @@
-image: 200521map_origin.pgm
+image: /home/youchen/extra_space/agv_200731/src/agv_bundle/configuration_files/200521map_origin.pgm
 resolution: 0.010000
 origin: [-19.701677, -9.709916, 0.0]
 negate: 0

src/agv_bundle/configuration_files/200702map_origin.yaml

@@ -1,4 +1,4 @@
-image: /home/zx/zzw/agv_ws/src/agv_bundle/configuration_files/200702map_origin.pgm
+image: /home/youchen/extra_space/agv_200731/src/agv_bundle/configuration_files/200702map_origin.pgm
 resolution: 0.010000
 origin: [-13.350000, -10.982070, 0.0]
 negate: 0

src/agv_bundle/configuration_files/200919map_origin.yaml

@@ -0,0 +1,6 @@
+image: /home/youchen/extra_space/agv_200731/src/agv_bundle/configuration_files/200919map_origin.pgm
+resolution: 0.010000
+origin: [-25.065845, -10.061523, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

src/agv_bundle/configuration_files/201113map_origin.yaml

@@ -0,0 +1,6 @@
+image: /home/youchen/extra_space/agv_200731/src/agv_bundle/configuration_files/201113map_origin.pgm
+resolution: 0.010000
+origin: [-2.800810, -2.384956, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

src/agv_bundle/launch/locate.sh

@@ -4,15 +4,15 @@ time=$(date "+%Y-%m-%d-%H-%M-%S")
 # gnome-terminal -x bash -c "roscore"
 
 
-gnome-terminal -x bash -c "cd ~/zzw/agv_ws; \
+gnome-terminal -x bash -c "cd ~//extra_space/agv_200731; \
 source devel/setup.bash; \
 rosrun agv_bundle agv_bundle_locate"
-# > ~/zzw/agv_ws/src/agv_bundle/logs/${time}-locate.log"
+# > ~//extra_space/agv_200731/src/agv_bundle/logs/${time}-locate.log"
 
-gnome-terminal -x bash -c "cd ~/zzw/agv_ws; \
+gnome-terminal -x bash -c "cd ~//extra_space/agv_200731; \
 source devel/setup.bash; \
 roslaunch agv_bundle map_scan.launch"
-# > ~/zzw/agv_ws/src/agv_bundle/logs/map_server.log"
+# > ~//extra_space/agv_200731/src/agv_bundle/logs/map_server.log"
 
 # gnome-terminal -x bash -c "cd ~/Documents/git_ws; \
 # source devel/setup.bash; \

src/agv_bundle/launch/map_scan.launch

@@ -1,6 +1,8 @@
 <launch>
-  <node name="map_server" pkg="map_server" type="map_server" args="$(find agv_bundle)/configuration_files/200702map_origin.yaml"/>
+  <node name="map_server" pkg="map_server" type="map_server" args="/home/youchen/extra_space/agv_200731/src/agv_bundle/configuration_files/201113map_origin.yaml"/>
   <node name="rviz" pkg="rviz" type="rviz" required="true"
       args="-d $(find agv_bundle)/configuration_files/demo.rviz" />
-  <!--node name="agv_bundle" pkg="agv_bundle" type="agv_bundle_locate"/-->
+  <!-- <node name="agv_bundle" pkg="agv_bundle" type="agv_bundle_locate"/> -->
+    <!-- <node pkg="tf" type="static_transform_publisher" name="laser_to_imu"
+    args="0.0 0.0 0.0 0.0 0.0 0.0 1.0 /laser /imu 100" />-->
 </launch>

src/agv_bundle/launch/rosbag.launch

@@ -1,5 +1,5 @@
 <launch>
-  <arg name="file" default="$(find agv_bundle)/launch/gedian_fast1.bag"/>
+  <arg name="file" default="$(find agv_bundle)/launch/2020-09-19-12-41-59.bag"/>
   <node pkg="rosbag" type="play" name="playback" ns="bag_replay" args="
 	--clock $(arg file) -r 1 --topics /scan "
 	output="screen">

src/agv_bundle/launch/rviz.launch

@@ -0,0 +1,5 @@
+<launch>
+	<node name="map_server" pkg="map_server" type="map_server" args="/home/youchen/extra_space/agv_200731/src/agv_bundle/configuration_files/200919map_origin.yaml"/>
+	<node name="rviz" pkg="rviz" type="rviz" required="true"
+      args="-d $(find agv_bundle)/configuration_files/demo.rviz" />
+</launch>

src/agv_bundle/src/ceres_scan_match_node.cpp

@@ -12,7 +12,7 @@ int main(int argc, char** argv)
   if(argc >= 4){
     origin_trans << stod(argv[1]), stod(argv[2]), stod(argv[3]);
   }
-  Ceres_scan_match sm("/home/zx/zzw/agv_ws/src/agv_bundle/configuration_files/200702map_origin.yaml", origin_trans);
+  Ceres_scan_match sm("/home/youchen/extra_space/agv_200731/src/agv_bundle/configuration_files/201113map_origin.yaml", origin_trans);
   // sm.Solve();
 
   ros::Rate loop_rate(100);

src/agv_bundle/src/locate/ceres_scan_match.cpp

@@ -2,7 +2,7 @@
  * @Description: match scan with grid map
  * @Author: yct
  * @Date: 2020-04-03 13:10:37
- * @LastEditTime: 2020-06-21 23:56:17
+ * @LastEditTime: 2020-10-26 15:02:52
  * @LastEditors: yct
  */
 
@@ -51,9 +51,15 @@ Ceres_scan_match::Ceres_scan_match(std::string filename, Eigen::Vector3d trans)
 {
     // m_odom_pub = m_nh.advertise<nav_msgs::Odometry>("odom", 1, true);
     m_scan_subscriber = m_nh.subscribe("scan", 1, &Ceres_scan_match::scan_callback, this);
+    m_imu_subscriber = m_nh.subscribe("/imu/data", 1, &Ceres_scan_match::imu_callback, this);
     m_initial_pose_subscriber = m_nh.subscribe("/initialpose", 1, &Ceres_scan_match::initial_pose_callback, this);
     m_cloud_publisher = m_nh.advertise<sensor_msgs::PointCloud2>("match_cloud", 10);
+    m_imu_pose_publisher = m_nh.advertise<nav_msgs::Odometry>("inter_pose", 1);
     m_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+    m_locate_timepoint = ros::Time::now();
+    m_imu_vec.resize(MOVING_WINDOW_SIZE);
+    m_start_index = 0;
+    m_end_index = 0;
     //read_pointcloud("/home/youchen/Documents/git_ws/src/map_scan_match/my1.txt", m_cloud);
     m_trans = trans;
     // m_trans << 0.0, 0.0, 0.0;
@@ -203,13 +209,17 @@ bool Ceres_scan_match::Solve()
     // // 临时添加，测试carto地图用
     // trans << 0.0, 0.0, 0.0;
     // cv::Mat t_map = cv::imread("/home/youchen/Documents/git_ws/src/map_scan_match/my1.jpg", cv::IMREAD_GRAYSCALE);
-    // t_map.convertTo(t_map, CV_32FC1, 1.0 / 255.0);
+    // t_map.convertTo(t_map, CV_32FC1, 1.0 / 255.0);m_imu_pose_publisher
     // read_pointcloud("/home/youchen/Documents/git_ws/src/map_scan_match/my1.txt", m_cloud);
 
     Solve(trans, m_cloud, m_global_map);
     m_initial_pose.setOrigin(tf::Vector3(trans[0], trans[1], 0));
     m_initial_pose.setRotation(tf::Quaternion(tf::Vector3(0,0,1), trans[2]));
 
+    // 保存定位结果
+    m_locate_pose.setOrigin(tf::Vector3(trans[0], trans[1], 0));
+    m_locate_pose.setRotation(tf::Quaternion(tf::Vector3(0,0,1), trans[2]));
+
     std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
     int duration = std::chrono::duration_cast<std::chrono::microseconds>(t1 - t0).count();
     std::cout << "solve time us: " << duration << std::endl;
@@ -286,7 +296,26 @@ void Ceres_scan_match::scan_callback(const sensor_msgs::LaserScan::ConstPtr &sca
         return;
     if (m_initialized)
     {
-        std::cout << "-------------- scan callback --------------" << std::endl;
+        // 有最新imu数据时,判断z轴偏差值,超过10度丢掉该帧数据
+        double t_time_interval_sec = (ros::Time::now() - m_imu_vec[m_end_index].header.stamp).toSec();
+        if(t_time_interval_sec < 0.2)
+        {
+            tf::Quaternion orient;
+            orient.setX(m_imu_vec[m_end_index].orientation.x);
+            orient.setY(m_imu_vec[m_end_index].orientation.y);
+            orient.setZ(m_imu_vec[m_end_index].orientation.z);
+            orient.setW(m_imu_vec[m_end_index].orientation.w);
+            orient.normalize();
+            double cos_half_angle = cos(asin(orient.getW()));
+            Eigen::Vector3d axis = {orient.getX() / cos_half_angle, orient.getY() / cos_half_angle, orient.getZ() / cos_half_angle};
+            if (axis.transpose() * Eigen::Vector3d(0,0,1) < cos(3.0 * M_PI / 180.0))
+            {
+                std::cout << "please place device horizontally!" << std::endl;
+                return;
+            }
+        }
+
+        // std::cout << "-------------- scan callback --------------" << std::endl;
         m_mutex.lock();
         m_cloud->clear();
         double l, angle;
@@ -304,7 +333,101 @@ void Ceres_scan_match::scan_callback(const sensor_msgs::LaserScan::ConstPtr &sca
         }
         std::cout << "cloud size: " << m_cloud->size() << std::endl;
         m_mutex.unlock();
-        Solve();
+        
+        if(Solve()){
+            if((ros::Time::now() - m_locate_timepoint).toSec()*1000>=TIME_INTERVAL_LIMIT_MILLI){
+                std::cout << "locate timeout" << std::endl;
+            }
+            m_locate_timepoint = ros::Time::now();
+        }
+    }
+}
+
+void Ceres_scan_match::imu_callback(const sensor_msgs::Imu::ConstPtr &img_msg)
+{
+    static double offset_x = 0;
+    static double offset_y = 0;
+    // std::cout << "imu callback" << std::endl;
+    if (m_imu_vec.size() != MOVING_WINDOW_SIZE)
+    {
+        std::cout << "vec size error." << std::endl;
+    }
+    nav_msgs::Odometry t_pose_to_pub;
+    t_pose_to_pub.header = img_msg->header;
+    t_pose_to_pub.header.frame_id = "/map";
+    m_imu_vec[m_end_index] = *img_msg;
+    m_end_index = (m_end_index + 1) % MOVING_WINDOW_SIZE;
+    // 下标重叠,移动起始下标,相当于丢掉多余历史数据
+    if(m_end_index == m_start_index)
+    {
+        m_start_index = (m_start_index + 1) % MOVING_WINDOW_SIZE;
+    }
+    // 0.5s内存在定位结果则插值,否则认为已出现定位异常,不进行插值
+    if((ros::Time::now()-m_locate_timepoint).toSec() < 0.5){
+        std::lock_guard<std::mutex> lck(m_mutex);
+        update_imu_interpolated_pose(m_initial_pose);
+
+        t_pose_to_pub.pose.pose.position.x = m_initial_pose.getOrigin().x();
+        t_pose_to_pub.pose.pose.position.y = m_initial_pose.getOrigin().y();
+        t_pose_to_pub.pose.pose.position.z = m_initial_pose.getOrigin().z();
+        t_pose_to_pub.pose.pose.orientation.x = m_initial_pose.getRotation().x();
+        t_pose_to_pub.pose.pose.orientation.y = m_initial_pose.getRotation().y();
+        t_pose_to_pub.pose.pose.orientation.z = m_initial_pose.getRotation().z();
+        t_pose_to_pub.pose.pose.orientation.w = m_initial_pose.getRotation().w();
+        
+        m_imu_pose_publisher.publish(t_pose_to_pub);
+    }
+}
+
+void Ceres_scan_match::update_imu_interpolated_pose(tf::Pose &pose)
+{
+    ros::Time current = ros::Time::now();
+    pose.setOrigin(m_locate_pose.getOrigin());
+    pose.setRotation(m_locate_pose.getRotation());
+    // 队列中数据滤波
+    // 队列数据积分
+    uint t_start = m_start_index, t_end = m_end_index;
+    if(t_start == t_end)
+    {
+        std::cout << "imu vector is empty." << std::endl;
+        return;
+    }
+    if(t_start > t_end)
+    {
+        t_end += MOVING_WINDOW_SIZE;
+    }
+    for (size_t i = t_start; i < t_end; i++)
+    {
+        // 早于最近定位结果的imu数据不参与计算
+        uint t_curr = i % MOVING_WINDOW_SIZE;
+        uint t_prev = i<1?(i+MOVING_WINDOW_SIZE-1):(i-1)% MOVING_WINDOW_SIZE;
+        if ((m_imu_vec[t_curr].header.stamp - m_locate_timepoint).toSec() < 0)
+        {
+            continue;
+        }
+        // 计算时间差
+        double t_time_interval_sec = 0.0;
+        if (i == t_start)
+        {
+            t_time_interval_sec = (m_imu_vec[t_curr].header.stamp - m_locate_timepoint).toSec();
+        }
+        else
+        {
+            t_time_interval_sec = (m_imu_vec[t_curr].header.stamp - m_imu_vec[t_prev].header.stamp).toSec();
+        }
+        // 时间差超过0.2s数据不参与计算
+        if(t_time_interval_sec > 0.2)
+        {
+            continue;
+        }
+        tf::Quaternion t_quat;
+        geometry_msgs::Vector3 t_rpy = m_imu_vec[t_curr].angular_velocity;
+        t_quat.setRPY(t_rpy.x * t_time_interval_sec, t_rpy.y * t_time_interval_sec, t_rpy.z * t_time_interval_sec);
+        // 当前存入加速度a与角速度ang
+        pose.getOrigin().setX(pose.getOrigin().x() + 0.5 * m_imu_vec[t_curr].linear_acceleration.x * std::pow(t_time_interval_sec,2));
+        pose.getOrigin().setY(pose.getOrigin().y() + 0.5 * m_imu_vec[t_curr].linear_acceleration.y * std::pow(t_time_interval_sec,2));
+        pose.getOrigin().setZ(pose.getOrigin().z() + 0.5 * m_imu_vec[t_curr].linear_acceleration.z * std::pow(t_time_interval_sec,2));
+        pose.setRotation(pose.getRotation() * t_quat);
     }
 }
 

src/agv_bundle/src/locate/ceres_scan_match.h

@@ -2,7 +2,7 @@
  * @Description: match scan with grid map, 监听scan与initpose话题，转成点云并发布
  * @Author: yct
  * @Date: 2020-04-03 11:50:01
- * @LastEditTime: 2020-06-21 23:45:18
+ * @LastEditTime: 2020-10-26 11:26:20
  * @LastEditors: yct
  */
 #ifndef CERES_SCAN_MATCH_HH
@@ -16,6 +16,8 @@
 #include <mutex>
 #include <cmath>
 #include <chrono>
+#include <queue>
+
 #include <ros/ros.h>
 #include <sensor_msgs/LaserScan.h>
 #include <sensor_msgs/PointCloud2.h>
@@ -35,6 +37,7 @@
 #include "ceres/cubic_interpolation.h"
 
 #include <nav_msgs/Odometry.h>
+#include <sensor_msgs/Imu.h>
 
 class prob_array
 {
@@ -75,18 +78,40 @@ private:
     bool Solve();
     
     void scan_callback(const sensor_msgs::LaserScan::ConstPtr &scan_msg);
+    // void imu_callback(const nav_msgs::Odometry::ConstPtr &odom_msg);
+    void imu_callback(const sensor_msgs::Imu::ConstPtr &img_msg);
+    void update_imu_interpolated_pose(tf::Pose &pose);
     void initial_pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &initial_pose_msg);
 
 private:
     // ros句柄
     ros::NodeHandle m_nh;
     ros::Subscriber m_scan_subscriber;
+    // 监听imu发布的里程计数据,生成中间位姿,提高稳定性
+    ros::Subscriber m_imu_subscriber;
+    // 发布插值后位姿
+    ros::Publisher m_imu_pose_publisher;
+    // 滑动窗口大小
+    const int MOVING_WINDOW_SIZE = 10;
+    // 时间范围限制
+    const int TIME_INTERVAL_LIMIT_MILLI = 1000;
+    // 历史imu数据
+    std::vector<sensor_msgs::Imu> m_imu_vec;
+    // imu数据数组起止下标
+    uint m_start_index, m_end_index;
+
     ros::Subscriber m_initial_pose_subscriber;
     ros::Publisher m_cloud_publisher;
     ros::Publisher m_odom_pub; // 发布里程计测试用
     tf::TransformBroadcaster tf_broadcaster_;
     std::atomic_bool m_initial_pose_updated;
+    // 初始位姿
     tf::Pose m_initial_pose;
+    // 获得定位结果
+    tf::Pose m_locate_pose;
+    // 获得定位结果时的时间戳
+    ros::Time m_locate_timepoint;
+
     std::mutex m_mutex;
 
     // cv全局地图

src/imu_enc/CMakeLists.txt

@@ -0,0 +1,211 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(imu_enc)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+set(CMAKE_BUILD_TYPE "RELEASE")
+
+## 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
+  std_msgs
+  tf
+)
+find_package(Boost REQUIRED)
+
+## 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
+# )
+
+################################################
+## 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/DynReconf1.cfg
+#   cfg/DynReconf2.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 imu_enc
+  CATKIN_DEPENDS roscpp std_msgs tf
+  DEPENDS Boost
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+  ./include
+  ${Boost_INCLUDE_DIRS}
+)
+
+link_directories(./lib)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/imu_enc.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/imu_enc_node.cpp)
+
+add_executable(${PROJECT_NAME}_node src/imu_node.cpp src/JY901.cpp)
+target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ${Boost_LIBRARIES} libserial.so)
+
+add_executable(serial_example src/serial_example.cc src/JY901.cpp)
+target_link_libraries(serial_example ${catkin_LIBRARIES} libserial.so)
+
+## 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_imu_enc.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/imu_enc/include/JY901.h

@@ -0,0 +1,155 @@
+#ifndef JY901_h
+#define JY901_h
+#include "string.h"
+#include <iostream>
+#include <math.h>
+
+#define SAVE 			0x00
+#define CALSW 		0x01
+#define RSW 			0x02
+#define RRATE			0x03
+#define BAUD 			0x04
+#define AXOFFSET	0x05
+#define AYOFFSET	0x06
+#define AZOFFSET	0x07
+#define GXOFFSET	0x08
+#define GYOFFSET	0x09
+#define GZOFFSET	0x0a
+#define HXOFFSET	0x0b
+#define HYOFFSET	0x0c
+#define HZOFFSET	0x0d
+#define D0MODE		0x0e
+#define D1MODE		0x0f
+#define D2MODE		0x10
+#define D3MODE		0x11
+#define D0PWMH		0x12
+#define D1PWMH		0x13
+#define D2PWMH		0x14
+#define D3PWMH		0x15
+#define D0PWMT		0x16
+#define D1PWMT		0x17
+#define D2PWMT		0x18
+#define D3PWMT		0x19
+#define IICADDR		0x1a
+#define LEDOFF 		0x1b
+#define GPSBAUD		0x1c
+
+#define YYMM				0x30
+#define DDHH				0x31
+#define MMSS				0x32
+#define MS					0x33
+#define AX					0x34
+#define AY					0x35
+#define AZ					0x36
+#define GX					0x37
+#define GY					0x38
+#define GZ					0x39
+#define HX					0x3a
+#define HY					0x3b
+#define HZ					0x3c			
+#define Roll				0x3d
+#define Pitch				0x3e
+#define Yaw					0x3f
+#define TEMP				0x40
+#define D0Status		0x41
+#define D1Status		0x42
+#define D2Status		0x43
+#define D3Status		0x44
+#define PressureL		0x45
+#define PressureH		0x46
+#define HeightL			0x47
+#define HeightH			0x48
+#define LonL				0x49
+#define LonH				0x4a
+#define LatL				0x4b
+#define LatH				0x4c
+#define GPSHeight   0x4d
+#define GPSYAW      0x4e
+#define GPSVL				0x4f
+#define GPSVH				0x50
+      
+#define DIO_MODE_AIN 0
+#define DIO_MODE_DIN 1
+#define DIO_MODE_DOH 2
+#define DIO_MODE_DOL 3
+#define DIO_MODE_DOPWM 4
+#define DIO_MODE_GPS 5		
+
+struct STime
+{
+	unsigned char ucYear;
+	unsigned char ucMonth;
+	unsigned char ucDay;
+	unsigned char ucHour;
+	unsigned char ucMinute;
+	unsigned char ucSecond;
+	unsigned short usMiliSecond;
+};
+struct SAcc
+{
+	short a[3];
+	short T;
+};
+struct SGyro
+{
+	short w[3];
+	short T;
+};
+struct SAngle
+{
+	short Angle[3];
+	short T;
+};
+struct SMag
+{
+	short h[3];
+	short T;
+};
+
+struct SDStatus
+{
+	short sDStatus[4];
+};
+
+struct SPress
+{
+	long lPressure;
+	long lAltitude;
+};
+
+struct SLonLat
+{
+	long lLon;
+	long lLat;
+};
+
+struct SGPSV
+{
+	short sGPSHeight;
+	short sGPSYaw;
+	long lGPSVelocity;
+};
+class CJY901 
+{
+public: 
+	struct STime		stcTime;
+	struct SAcc 		stcAcc, lastAcc;
+	struct SGyro 		stcGyro, lastGyro;
+	struct SAngle 		stcAngle, lastAngle;
+	struct SMag 		stcMag;
+	struct SDStatus 	stcDStatus;
+	struct SPress 		stcPress;
+	struct SLonLat 		stcLonLat;
+	struct SGPSV 		stcGPSV;
+	
+
+    CJY901 (); 
+    bool CopeSerialData(char ucData[], int& start, int& end,unsigned short usLength);
+
+private:
+	bool checkData();
+private:
+	double accInitLimit, gyroInitLimit, angleInitLimit;
+	double accDiffLimit, gyroDiffLimit, angleDiffLimit;
+};
+#endif

src/imu_enc/include/imu_node.h

@@ -0,0 +1,112 @@
+#ifndef __IMU_NODE_HH
+#define __IMU_NODE_HH
+
+#include "JY901.h"
+#include <boost/thread.hpp>
+#include "serial/serial.h"
+
+#include <chrono>
+#include <iostream>
+#include <mutex>
+#include <ros/ros.h>
+#include <nav_msgs/Odometry.h>
+#include <tf/tf.h>
+#include <tf/transform_datatypes.h>
+#include <tf/transform_broadcaster.h>
+
+#define		iBufferSize 50
+#define     UARTBufferLength 1000
+
+#include "JY901.h"
+CJY901 JY901;
+
+class ImuNode
+{
+    public:
+        ImuNode(std::string port, unsigned long baud);
+        ~ImuNode();
+        void publish_data();
+
+    protected:
+        static void thread_get_data(ImuNode *imu_node)
+        {
+            int countDown=20;
+            while ((imu_node->b_connected_ || countDown-- > 0) && !imu_node->b_exit_)
+            {
+                // std::cout<<imu_node->my_serial->isOpen()<<std::endl;
+                if (imu_node->my_serial->isOpen())
+                {
+                    imu_node->b_connected_ = true;
+                    // std::vector<uint8_t> temp_buffer;
+                    int count = imu_node->my_serial->read(imu_node->p_temp_buf, iBufferSize);
+                    // std::cout<<"count: "<<count<<std::endl;
+                    std::unique_lock<std::mutex> lck(imu_node->mtx);
+                    // end 添加后在范围内
+                    if ((imu_node->end + count) <= UARTBufferLength)
+                    {
+                        // std::cout<<"bbb"<<std::endl;
+                        // std::cout<<"end: "<<imu_node->end<<std::endl;
+                        memcpy((imu_node->p_buffer + imu_node->end), imu_node->p_temp_buf, count);
+                        // std::cout<<"start: "<<imu_node->start<<std::endl;
+                        if (imu_node->start > imu_node->end && imu_node->start <= imu_node->end + count)
+                        {
+                            imu_node->start = imu_node->end + count + 1;
+                            imu_node->start %= UARTBufferLength;
+                        }
+                        imu_node->end += count;
+                        imu_node->end %= UARTBufferLength;
+                    }
+                    // 超出范围
+                    else
+                    {
+                        // std::cout<<"ccc"<<std::endl;
+                        int offset = UARTBufferLength - imu_node->end;
+                        // std::cout<<"offset: "<<offset<<std::endl;
+                        memcpy((imu_node->p_buffer + imu_node->end), imu_node->p_temp_buf, offset);
+                        memcpy(imu_node->p_buffer, (imu_node->p_temp_buf + offset), count - offset);
+                        if (imu_node->start > imu_node->end || (imu_node->start < imu_node->end && imu_node->start < (imu_node->end + count) % UARTBufferLength))
+                        {
+                            imu_node->start = (imu_node->end + count) % UARTBufferLength + 1;
+                        }
+                        imu_node->end += count;
+                        imu_node->end %= UARTBufferLength;
+                    }
+                }
+                else
+                {
+                    imu_node->b_connected_ = false;
+                }
+
+                // std::cout << "count down: " << countDown << std::endl;
+                usleep(5 * 1000);
+            }
+
+            std::cout << "数据获取线程退出" << std::endl;
+        };
+
+    public:
+        bool b_exit_;
+        // 0默认，1正向，2反向
+        int m_angle_status_[3]={0,0,0};
+    protected:
+
+        bool b_connected_;
+        
+        std::mutex mtx;
+        //std::unique_lock<std::mutex> buffer_lock;
+        serial::Serial *my_serial;
+        char *p_buffer;
+        uint8_t *p_temp_buf;        
+        int start,end;
+        boost::thread*  read_thread_  ;
+
+        ros::NodeHandle nh_;
+        ros::Publisher odom_pub_;
+        tf::Pose last_pose_, curr_pose_;
+        geometry_msgs::Twist last_vel_, curr_vel_;
+        ros::Time last_time_, curr_time_, ori_time_;
+        tf::TransformBroadcaster tf_broadcaster_;
+
+};
+
+#endif

src/imu_enc/include/serial/impl/unix.h

@@ -0,0 +1,221 @@
+/*!
+ * \file serial/impl/unix.h
+ * \author  William Woodall <wjwwood@gmail.com>
+ * \author  John Harrison <ash@greaterthaninfinity.com>
+ * \version 0.1
+ *
+ * \section LICENSE
+ *
+ * The MIT License
+ *
+ * Copyright (c) 2012 William Woodall, John Harrison
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * \section DESCRIPTION
+ *
+ * This provides a unix based pimpl for the Serial class. This implementation is
+ * based off termios.h and uses select for multiplexing the IO ports.
+ *
+ */
+
+#if !defined(_WIN32)
+
+#ifndef SERIAL_IMPL_UNIX_H
+#define SERIAL_IMPL_UNIX_H
+
+#include "serial/serial.h"
+
+#include <pthread.h>
+
+namespace serial {
+
+using std::size_t;
+using std::string;
+using std::invalid_argument;
+
+using serial::SerialException;
+using serial::IOException;
+
+class MillisecondTimer {
+public:
+  MillisecondTimer(const uint32_t millis);         
+  int64_t remaining();
+
+private:
+  static timespec timespec_now();
+  timespec expiry;
+};
+
+class serial::Serial::SerialImpl {
+public:
+  SerialImpl (const string &port,
+              unsigned long baudrate,
+              bytesize_t bytesize,
+              parity_t parity,
+              stopbits_t stopbits,
+              flowcontrol_t flowcontrol);
+
+  virtual ~SerialImpl ();
+
+  void
+  open ();
+
+  void
+  close ();
+
+  bool
+  isOpen () const;
+
+  size_t
+  available ();
+
+  bool
+  waitReadable (uint32_t timeout);
+
+  void
+  waitByteTimes (size_t count);
+
+  size_t
+  read (uint8_t *buf, size_t size = 1);
+
+  size_t
+  write (const uint8_t *data, size_t length);
+
+  void
+  flush ();
+
+  void
+  flushInput ();
+
+  void
+  flushOutput ();
+
+  void
+  sendBreak (int duration);
+
+  void
+  setBreak (bool level);
+
+  void
+  setRTS (bool level);
+
+  void
+  setDTR (bool level);
+
+  bool
+  waitForChange ();
+
+  bool
+  getCTS ();
+
+  bool
+  getDSR ();
+
+  bool
+  getRI ();
+
+  bool
+  getCD ();
+
+  void
+  setPort (const string &port);
+
+  string
+  getPort () const;
+
+  void
+  setTimeout (Timeout &timeout);
+
+  Timeout
+  getTimeout () const;
+
+  void
+  setBaudrate (unsigned long baudrate);
+
+  unsigned long
+  getBaudrate () const;
+
+  void
+  setBytesize (bytesize_t bytesize);
+
+  bytesize_t
+  getBytesize () const;
+
+  void
+  setParity (parity_t parity);
+
+  parity_t
+  getParity () const;
+
+  void
+  setStopbits (stopbits_t stopbits);
+
+  stopbits_t
+  getStopbits () const;
+
+  void
+  setFlowcontrol (flowcontrol_t flowcontrol);
+
+  flowcontrol_t
+  getFlowcontrol () const;
+
+  void
+  readLock ();
+
+  void
+  readUnlock ();
+
+  void
+  writeLock ();
+
+  void
+  writeUnlock ();
+
+protected:
+  void reconfigurePort ();
+
+private:
+  string port_;               // Path to the file descriptor
+  int fd_;                    // The current file descriptor
+
+  bool is_open_;
+  bool xonxoff_;
+  bool rtscts_;
+
+  Timeout timeout_;           // Timeout for read operations
+  unsigned long baudrate_;    // Baudrate
+  uint32_t byte_time_ns_;     // Nanoseconds to transmit/receive a single byte
+
+  parity_t parity_;           // Parity
+  bytesize_t bytesize_;       // Size of the bytes
+  stopbits_t stopbits_;       // Stop Bits
+  flowcontrol_t flowcontrol_; // Flow Control
+
+  // Mutex used to lock the read functions
+  pthread_mutex_t read_mutex;
+  // Mutex used to lock the write functions
+  pthread_mutex_t write_mutex;
+};
+
+}
+
+#endif // SERIAL_IMPL_UNIX_H
+
+#endif // !defined(_WIN32)

src/imu_enc/include/serial/impl/win.h

@@ -0,0 +1,207 @@
+/*!
+ * \file serial/impl/windows.h
+ * \author  William Woodall <wjwwood@gmail.com>
+ * \author  John Harrison <ash@greaterthaninfinity.com>
+ * \version 0.1
+ *
+ * \section LICENSE
+ *
+ * The MIT License
+ *
+ * Copyright (c) 2012 William Woodall, John Harrison
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * \section DESCRIPTION
+ *
+ * This provides a windows implementation of the Serial class interface.
+ *
+ */
+
+#if defined(_WIN32)
+
+#ifndef SERIAL_IMPL_WINDOWS_H
+#define SERIAL_IMPL_WINDOWS_H
+
+#include "serial/serial.h"
+
+#include "windows.h"
+
+namespace serial {
+
+using std::string;
+using std::wstring;
+using std::invalid_argument;
+
+using serial::SerialException;
+using serial::IOException;
+
+class serial::Serial::SerialImpl {
+public:
+  SerialImpl (const string &port,
+              unsigned long baudrate,
+              bytesize_t bytesize,
+              parity_t parity,
+              stopbits_t stopbits,
+              flowcontrol_t flowcontrol);
+
+  virtual ~SerialImpl ();
+
+  void
+  open ();
+
+  void
+  close ();
+
+  bool
+  isOpen () const;
+
+  size_t
+  available ();
+  
+  bool
+  waitReadable (uint32_t timeout);
+
+  void
+  waitByteTimes (size_t count);
+
+  size_t
+  read (uint8_t *buf, size_t size = 1);
+
+  size_t
+  write (const uint8_t *data, size_t length);
+
+  void
+  flush ();
+
+  void
+  flushInput ();
+
+  void
+  flushOutput ();
+
+  void
+  sendBreak (int duration);
+
+  void
+  setBreak (bool level);
+
+  void
+  setRTS (bool level);
+
+  void
+  setDTR (bool level);
+
+  bool
+  waitForChange ();
+
+  bool
+  getCTS ();
+
+  bool
+  getDSR ();
+
+  bool
+  getRI ();
+
+  bool
+  getCD ();
+
+  void
+  setPort (const string &port);
+
+  string
+  getPort () const;
+
+  void
+  setTimeout (Timeout &timeout);
+
+  Timeout
+  getTimeout () const;
+
+  void
+  setBaudrate (unsigned long baudrate);
+
+  unsigned long
+  getBaudrate () const;
+
+  void
+  setBytesize (bytesize_t bytesize);
+
+  bytesize_t
+  getBytesize () const;
+
+  void
+  setParity (parity_t parity);
+
+  parity_t
+  getParity () const;
+
+  void
+  setStopbits (stopbits_t stopbits);
+
+  stopbits_t
+  getStopbits () const;
+
+  void
+  setFlowcontrol (flowcontrol_t flowcontrol);
+
+  flowcontrol_t
+  getFlowcontrol () const;
+
+  void
+  readLock ();
+
+  void
+  readUnlock ();
+
+  void
+  writeLock ();
+
+  void
+  writeUnlock ();
+
+protected:
+  void reconfigurePort ();
+
+private:
+  wstring port_;               // Path to the file descriptor
+  HANDLE fd_;
+
+  bool is_open_;
+
+  Timeout timeout_;           // Timeout for read operations
+  unsigned long baudrate_;    // Baudrate
+
+  parity_t parity_;           // Parity
+  bytesize_t bytesize_;       // Size of the bytes
+  stopbits_t stopbits_;       // Stop Bits
+  flowcontrol_t flowcontrol_; // Flow Control
+
+  // Mutex used to lock the read functions
+  HANDLE read_mutex;
+  // Mutex used to lock the write functions
+  HANDLE write_mutex;
+};
+
+}
+
+#endif // SERIAL_IMPL_WINDOWS_H
+
+#endif // if defined(_WIN32)

src/imu_enc/include/serial/serial.h

@@ -0,0 +1,775 @@
+/*!
+ * \file serial/serial.h
+ * \author  William Woodall <wjwwood@gmail.com>
+ * \author  John Harrison   <ash.gti@gmail.com>
+ * \version 0.1
+ *
+ * \section LICENSE
+ *
+ * The MIT License
+ *
+ * Copyright (c) 2012 William Woodall
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * \section DESCRIPTION
+ *
+ * This provides a cross platform interface for interacting with Serial Ports.
+ */
+
+#ifndef SERIAL_H
+#define SERIAL_H
+
+#include <limits>
+#include <vector>
+#include <string>
+#include <cstring>
+#include <sstream>
+#include <exception>
+#include <stdexcept>
+#include <serial/v8stdint.h>
+
+#define THROW(exceptionClass, message) throw exceptionClass(__FILE__, \
+__LINE__, (message) )
+
+namespace serial {
+
+/*!
+ * Enumeration defines the possible bytesizes for the serial port.
+ */
+typedef enum {
+  fivebits = 5,
+  sixbits = 6,
+  sevenbits = 7,
+  eightbits = 8
+} bytesize_t;
+
+/*!
+ * Enumeration defines the possible parity types for the serial port.
+ */
+typedef enum {
+  parity_none = 0,
+  parity_odd = 1,
+  parity_even = 2,
+  parity_mark = 3,
+  parity_space = 4
+} parity_t;
+
+/*!
+ * Enumeration defines the possible stopbit types for the serial port.
+ */
+typedef enum {
+  stopbits_one = 1,
+  stopbits_two = 2,
+  stopbits_one_point_five
+} stopbits_t;
+
+/*!
+ * Enumeration defines the possible flowcontrol types for the serial port.
+ */
+typedef enum {
+  flowcontrol_none = 0,
+  flowcontrol_software,
+  flowcontrol_hardware
+} flowcontrol_t;
+
+/*!
+ * Structure for setting the timeout of the serial port, times are
+ * in milliseconds.
+ *
+ * In order to disable the interbyte timeout, set it to Timeout::max().
+ */
+struct Timeout {
+#ifdef max
+# undef max
+#endif
+  static uint32_t max() {return std::numeric_limits<uint32_t>::max();}
+  /*!
+   * Convenience function to generate Timeout structs using a
+   * single absolute timeout.
+   *
+   * \param timeout A long that defines the time in milliseconds until a
+   * timeout occurs after a call to read or write is made.
+   *
+   * \return Timeout struct that represents this simple timeout provided.
+   */
+  static Timeout simpleTimeout(uint32_t timeout) {
+    return Timeout(max(), timeout, 0, timeout, 0);
+  }
+
+  /*! Number of milliseconds between bytes received to timeout on. */
+  uint32_t inter_byte_timeout;
+  /*! A constant number of milliseconds to wait after calling read. */
+  uint32_t read_timeout_constant;
+  /*! A multiplier against the number of requested bytes to wait after
+   *  calling read.
+   */
+  uint32_t read_timeout_multiplier;
+  /*! A constant number of milliseconds to wait after calling write. */
+  uint32_t write_timeout_constant;
+  /*! A multiplier against the number of requested bytes to wait after
+   *  calling write.
+   */
+  uint32_t write_timeout_multiplier;
+
+  explicit Timeout (uint32_t inter_byte_timeout_=0,
+                    uint32_t read_timeout_constant_=0,
+                    uint32_t read_timeout_multiplier_=0,
+                    uint32_t write_timeout_constant_=0,
+                    uint32_t write_timeout_multiplier_=0)
+  : inter_byte_timeout(inter_byte_timeout_),
+    read_timeout_constant(read_timeout_constant_),
+    read_timeout_multiplier(read_timeout_multiplier_),
+    write_timeout_constant(write_timeout_constant_),
+    write_timeout_multiplier(write_timeout_multiplier_)
+  {}
+};
+
+/*!
+ * Class that provides a portable serial port interface.
+ */
+class Serial {
+public:
+  /*!
+   * Creates a Serial object and opens the port if a port is specified,
+   * otherwise it remains closed until serial::Serial::open is called.
+   *
+   * \param port A std::string containing the address of the serial port,
+   *        which would be something like 'COM1' on Windows and '/dev/ttyS0'
+   *        on Linux.
+   *
+   * \param baudrate An unsigned 32-bit integer that represents the baudrate
+   *
+   * \param timeout A serial::Timeout struct that defines the timeout
+   * conditions for the serial port. \see serial::Timeout
+   *
+   * \param bytesize Size of each byte in the serial transmission of data,
+   * default is eightbits, possible values are: fivebits, sixbits, sevenbits,
+   * eightbits
+   *
+   * \param parity Method of parity, default is parity_none, possible values
+   * are: parity_none, parity_odd, parity_even
+   *
+   * \param stopbits Number of stop bits used, default is stopbits_one,
+   * possible values are: stopbits_one, stopbits_one_point_five, stopbits_two
+   *
+   * \param flowcontrol Type of flowcontrol used, default is
+   * flowcontrol_none, possible values are: flowcontrol_none,
+   * flowcontrol_software, flowcontrol_hardware
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::IOException
+   * \throw std::invalid_argument
+   */
+  Serial (const std::string &port = "",
+          uint32_t baudrate = 9600,
+          Timeout timeout = Timeout(),
+          bytesize_t bytesize = eightbits,
+          parity_t parity = parity_none,
+          stopbits_t stopbits = stopbits_one,
+          flowcontrol_t flowcontrol = flowcontrol_none);
+
+  /*! Destructor */
+  virtual ~Serial ();
+
+  /*!
+   * Opens the serial port as long as the port is set and the port isn't
+   * already open.
+   *
+   * If the port is provided to the constructor then an explicit call to open
+   * is not needed.
+   *
+   * \see Serial::Serial
+   *
+   * \throw std::invalid_argument
+   * \throw serial::SerialException
+   * \throw serial::IOException
+   */
+  void
+  open ();
+
+  /*! Gets the open status of the serial port.
+   *
+   * \return Returns true if the port is open, false otherwise.
+   */
+  bool
+  isOpen () const;
+
+  /*! Closes the serial port. */
+  void
+  close ();
+
+  /*! Return the number of characters in the buffer. */
+  size_t
+  available ();
+
+  /*! Block until there is serial data to read or read_timeout_constant
+   * number of milliseconds have elapsed. The return value is true when
+   * the function exits with the port in a readable state, false otherwise
+   * (due to timeout or select interruption). */
+  bool
+  waitReadable ();
+
+  /*! Block for a period of time corresponding to the transmission time of
+   * count characters at present serial settings. This may be used in con-
+   * junction with waitReadable to read larger blocks of data from the
+   * port. */
+  void
+  waitByteTimes (size_t count);
+
+  /*! Read a given amount of bytes from the serial port into a given buffer.
+   *
+   * The read function will return in one of three cases:
+   *  * The number of requested bytes was read.
+   *    * In this case the number of bytes requested will match the size_t
+   *      returned by read.
+   *  * A timeout occurred, in this case the number of bytes read will not
+   *    match the amount requested, but no exception will be thrown.  One of
+   *    two possible timeouts occurred:
+   *    * The inter byte timeout expired, this means that number of
+   *      milliseconds elapsed between receiving bytes from the serial port
+   *      exceeded the inter byte timeout.
+   *    * The total timeout expired, which is calculated by multiplying the
+   *      read timeout multiplier by the number of requested bytes and then
+   *      added to the read timeout constant.  If that total number of
+   *      milliseconds elapses after the initial call to read a timeout will
+   *      occur.
+   *  * An exception occurred, in this case an actual exception will be thrown.
+   *
+   * \param buffer An uint8_t array of at least the requested size.
+   * \param size A size_t defining how many bytes to be read.
+   *
+   * \return A size_t representing the number of bytes read as a result of the
+   *         call to read.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   */
+  size_t
+  read (uint8_t *buffer, size_t size);
+
+  /*! Read a given amount of bytes from the serial port into a give buffer.
+   *
+   * \param buffer A reference to a std::vector of uint8_t.
+   * \param size A size_t defining how many bytes to be read.
+   *
+   * \return A size_t representing the number of bytes read as a result of the
+   *         call to read.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   */
+  size_t
+  read (std::vector<uint8_t> &buffer, size_t size = 1);
+
+  /*! Read a given amount of bytes from the serial port into a give buffer.
+   *
+   * \param buffer A reference to a std::string.
+   * \param size A size_t defining how many bytes to be read.
+   *
+   * \return A size_t representing the number of bytes read as a result of the
+   *         call to read.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   */
+  size_t
+  read (std::string &buffer, size_t size = 1);
+
+  /*! Read a given amount of bytes from the serial port and return a string
+   *  containing the data.
+   *
+   * \param size A size_t defining how many bytes to be read.
+   *
+   * \return A std::string containing the data read from the port.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   */
+  std::string
+  read (size_t size = 1);
+
+  /*! Reads in a line or until a given delimiter has been processed.
+   *
+   * Reads from the serial port until a single line has been read.
+   *
+   * \param buffer A std::string reference used to store the data.
+   * \param size A maximum length of a line, defaults to 65536 (2^16)
+   * \param eol A string to match against for the EOL.
+   *
+   * \return A size_t representing the number of bytes read.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   */
+  size_t
+  readline (std::string &buffer, size_t size = 65536, std::string eol = "\n");
+
+  /*! Reads in a line or until a given delimiter has been processed.
+   *
+   * Reads from the serial port until a single line has been read.
+   *
+   * \param size A maximum length of a line, defaults to 65536 (2^16)
+   * \param eol A string to match against for the EOL.
+   *
+   * \return A std::string containing the line.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   */
+  std::string
+  readline (size_t size = 65536, std::string eol = "\n");
+
+  /*! Reads in multiple lines until the serial port times out.
+   *
+   * This requires a timeout > 0 before it can be run. It will read until a
+   * timeout occurs and return a list of strings.
+   *
+   * \param size A maximum length of combined lines, defaults to 65536 (2^16)
+   *
+   * \param eol A string to match against for the EOL.
+   *
+   * \return A vector<string> containing the lines.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   */
+  std::vector<std::string>
+  readlines (size_t size = 65536, std::string eol = "\n");
+
+  /*! Write a string to the serial port.
+   *
+   * \param data A const reference containing the data to be written
+   * to the serial port.
+   *
+   * \param size A size_t that indicates how many bytes should be written from
+   * the given data buffer.
+   *
+   * \return A size_t representing the number of bytes actually written to
+   * the serial port.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   * \throw serial::IOException
+   */
+  size_t
+  write (const uint8_t *data, size_t size);
+
+  /*! Write a string to the serial port.
+   *
+   * \param data A const reference containing the data to be written
+   * to the serial port.
+   *
+   * \return A size_t representing the number of bytes actually written to
+   * the serial port.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   * \throw serial::IOException
+   */
+  size_t
+  write (const std::vector<uint8_t> &data);
+
+  /*! Write a string to the serial port.
+   *
+   * \param data A const reference containing the data to be written
+   * to the serial port.
+   *
+   * \return A size_t representing the number of bytes actually written to
+   * the serial port.
+   *
+   * \throw serial::PortNotOpenedException
+   * \throw serial::SerialException
+   * \throw serial::IOException
+   */
+  size_t
+  write (const std::string &data);
+
+  /*! Sets the serial port identifier.
+   *
+   * \param port A const std::string reference containing the address of the
+   * serial port, which would be something like 'COM1' on Windows and
+   * '/dev/ttyS0' on Linux.
+   *
+   * \throw std::invalid_argument
+   */
+  void
+  setPort (const std::string &port);
+
+  /*! Gets the serial port identifier.
+   *
+   * \see Serial::setPort
+   *
+   * \throw std::invalid_argument
+   */
+  std::string
+  getPort () const;
+
+  /*! Sets the timeout for reads and writes using the Timeout struct.
+   *
+   * There are two timeout conditions described here:
+   *  * The inter byte timeout:
+   *    * The inter_byte_timeout component of serial::Timeout defines the
+   *      maximum amount of time, in milliseconds, between receiving bytes on
+   *      the serial port that can pass before a timeout occurs.  Setting this
+   *      to zero will prevent inter byte timeouts from occurring.
+   *  * Total time timeout:
+   *    * The constant and multiplier component of this timeout condition,
+   *      for both read and write, are defined in serial::Timeout.  This
+   *      timeout occurs if the total time since the read or write call was
+   *      made exceeds the specified time in milliseconds.
+   *    * The limit is defined by multiplying the multiplier component by the
+   *      number of requested bytes and adding that product to the constant
+   *      component.  In this way if you want a read call, for example, to
+   *      timeout after exactly one second regardless of the number of bytes
+   *      you asked for then set the read_timeout_constant component of
+   *      serial::Timeout to 1000 and the read_timeout_multiplier to zero.
+   *      This timeout condition can be used in conjunction with the inter
+   *      byte timeout condition with out any problems, timeout will simply
+   *      occur when one of the two timeout conditions is met.  This allows
+   *      users to have maximum control over the trade-off between
+   *      responsiveness and efficiency.
+   *
+   * Read and write functions will return in one of three cases.  When the
+   * reading or writing is complete, when a timeout occurs, or when an
+   * exception occurs.
+   *
+   * A timeout of 0 enables non-blocking mode.
+   *
+   * \param timeout A serial::Timeout struct containing the inter byte
+   * timeout, and the read and write timeout constants and multipliers.
+   *
+   * \see serial::Timeout
+   */
+  void
+  setTimeout (Timeout &timeout);
+
+  /*! Sets the timeout for reads and writes. */
+  void
+  setTimeout (uint32_t inter_byte_timeout, uint32_t read_timeout_constant,
+              uint32_t read_timeout_multiplier, uint32_t write_timeout_constant,
+              uint32_t write_timeout_multiplier)
+  {
+    Timeout timeout(inter_byte_timeout, read_timeout_constant,
+                    read_timeout_multiplier, write_timeout_constant,
+                    write_timeout_multiplier);
+    return setTimeout(timeout);
+  }
+
+  /*! Gets the timeout for reads in seconds.
+   *
+   * \return A Timeout struct containing the inter_byte_timeout, and read
+   * and write timeout constants and multipliers.
+   *
+   * \see Serial::setTimeout
+   */
+  Timeout
+  getTimeout () const;
+
+  /*! Sets the baudrate for the serial port.
+   *
+   * Possible baudrates depends on the system but some safe baudrates include:
+   * 110, 300, 600, 1200, 2400, 4800, 9600, 14400, 19200, 28800, 38400, 56000,
+   * 57600, 115200
+   * Some other baudrates that are supported by some comports:
+   * 128000, 153600, 230400, 256000, 460800, 500000, 921600
+   *
+   * \param baudrate An integer that sets the baud rate for the serial port.
+   *
+   * \throw std::invalid_argument
+   */
+  void
+  setBaudrate (uint32_t baudrate);
+
+  /*! Gets the baudrate for the serial port.
+   *
+   * \return An integer that sets the baud rate for the serial port.
+   *
+   * \see Serial::setBaudrate
+   *
+   * \throw std::invalid_argument
+   */
+  uint32_t
+  getBaudrate () const;
+
+  /*! Sets the bytesize for the serial port.
+   *
+   * \param bytesize Size of each byte in the serial transmission of data,
+   * default is eightbits, possible values are: fivebits, sixbits, sevenbits,
+   * eightbits
+   *
+   * \throw std::invalid_argument
+   */
+  void
+  setBytesize (bytesize_t bytesize);
+
+  /*! Gets the bytesize for the serial port.
+   *
+   * \see Serial::setBytesize
+   *
+   * \throw std::invalid_argument
+   */
+  bytesize_t
+  getBytesize () const;
+
+  /*! Sets the parity for the serial port.
+   *
+   * \param parity Method of parity, default is parity_none, possible values
+   * are: parity_none, parity_odd, parity_even
+   *
+   * \throw std::invalid_argument
+   */
+  void
+  setParity (parity_t parity);
+
+  /*! Gets the parity for the serial port.
+   *
+   * \see Serial::setParity
+   *
+   * \throw std::invalid_argument
+   */
+  parity_t
+  getParity () const;
+
+  /*! Sets the stopbits for the serial port.
+   *
+   * \param stopbits Number of stop bits used, default is stopbits_one,
+   * possible values are: stopbits_one, stopbits_one_point_five, stopbits_two
+   *
+   * \throw std::invalid_argument
+   */
+  void
+  setStopbits (stopbits_t stopbits);
+
+  /*! Gets the stopbits for the serial port.
+   *
+   * \see Serial::setStopbits
+   *
+   * \throw std::invalid_argument
+   */
+  stopbits_t
+  getStopbits () const;
+
+  /*! Sets the flow control for the serial port.
+   *
+   * \param flowcontrol Type of flowcontrol used, default is flowcontrol_none,
+   * possible values are: flowcontrol_none, flowcontrol_software,
+   * flowcontrol_hardware
+   *
+   * \throw std::invalid_argument
+   */
+  void
+  setFlowcontrol (flowcontrol_t flowcontrol);
+
+  /*! Gets the flow control for the serial port.
+   *
+   * \see Serial::setFlowcontrol
+   *
+   * \throw std::invalid_argument
+   */
+  flowcontrol_t
+  getFlowcontrol () const;
+
+  /*! Flush the input and output buffers */
+  void
+  flush ();
+
+  /*! Flush only the input buffer */
+  void
+  flushInput ();
+
+  /*! Flush only the output buffer */
+  void
+  flushOutput ();
+
+  /*! Sends the RS-232 break signal.  See tcsendbreak(3). */
+  void
+  sendBreak (int duration);
+
+  /*! Set the break condition to a given level.  Defaults to true. */
+  void
+  setBreak (bool level = true);
+
+  /*! Set the RTS handshaking line to the given level.  Defaults to true. */
+  void
+  setRTS (bool level = true);
+
+  /*! Set the DTR handshaking line to the given level.  Defaults to true. */
+  void
+  setDTR (bool level = true);
+
+  /*!
+   * Blocks until CTS, DSR, RI, CD changes or something interrupts it.
+   *
+   * Can throw an exception if an error occurs while waiting.
+   * You can check the status of CTS, DSR, RI, and CD once this returns.
+   * Uses TIOCMIWAIT via ioctl if available (mostly only on Linux) with a
+   * resolution of less than +-1ms and as good as +-0.2ms.  Otherwise a
+   * polling method is used which can give +-2ms.
+   *
+   * \return Returns true if one of the lines changed, false if something else
+   * occurred.
+   *
+   * \throw SerialException
+   */
+  bool
+  waitForChange ();
+
+  /*! Returns the current status of the CTS line. */
+  bool
+  getCTS ();
+
+  /*! Returns the current status of the DSR line. */
+  bool
+  getDSR ();
+
+  /*! Returns the current status of the RI line. */
+  bool
+  getRI ();
+
+  /*! Returns the current status of the CD line. */
+  bool
+  getCD ();
+
+private:
+  // Disable copy constructors
+  Serial(const Serial&);
+  Serial& operator=(const Serial&);
+
+  // Pimpl idiom, d_pointer
+  class SerialImpl;
+  SerialImpl *pimpl_;
+
+  // Scoped Lock Classes
+  class ScopedReadLock;
+  class ScopedWriteLock;
+
+  // Read common function
+  size_t
+  read_ (uint8_t *buffer, size_t size);
+  // Write common function
+  size_t
+  write_ (const uint8_t *data, size_t length);
+
+};
+
+class SerialException : public std::exception
+{
+  // Disable copy constructors
+  SerialException& operator=(const SerialException&);
+  std::string e_what_;
+public:
+  SerialException (const char *description) {
+      std::stringstream ss;
+      ss << "SerialException " << description << " failed.";
+      e_what_ = ss.str();
+  }
+  SerialException (const SerialException& other) : e_what_(other.e_what_) {}
+  virtual ~SerialException() throw() {}
+  virtual const char* what () const throw () {
+    return e_what_.c_str();
+  }
+};
+
+class IOException : public std::exception
+{
+  // Disable copy constructors
+  IOException& operator=(const IOException&);
+  std::string file_;
+  int line_;
+  std::string e_what_;
+  int errno_;
+public:
+  explicit IOException (std::string file, int line, int errnum)
+    : file_(file), line_(line), errno_(errnum) {
+      std::stringstream ss;
+#if defined(_WIN32) && !defined(__MINGW32__)
+      char error_str [1024];
+      strerror_s(error_str, 1024, errnum);
+#else
+      char * error_str = strerror(errnum);
+#endif
+      ss << "IO Exception (" << errno_ << "): " << error_str;
+      ss << ", file " << file_ << ", line " << line_ << ".";
+      e_what_ = ss.str();
+  }
+  explicit IOException (std::string file, int line, const char * description)
+    : file_(file), line_(line), errno_(0) {
+      std::stringstream ss;
+      ss << "IO Exception: " << description;
+      ss << ", file " << file_ << ", line " << line_ << ".";
+      e_what_ = ss.str();
+  }
+  virtual ~IOException() throw() {}
+  IOException (const IOException& other) : line_(other.line_), e_what_(other.e_what_), errno_(other.errno_) {}
+
+  int getErrorNumber () const { return errno_; }
+
+  virtual const char* what () const throw () {
+    return e_what_.c_str();
+  }
+};
+
+class PortNotOpenedException : public std::exception
+{
+  // Disable copy constructors
+  const PortNotOpenedException& operator=(PortNotOpenedException);
+  std::string e_what_;
+public:
+  PortNotOpenedException (const char * description)  {
+      std::stringstream ss;
+      ss << "PortNotOpenedException " << description << " failed.";
+      e_what_ = ss.str();
+  }
+  PortNotOpenedException (const PortNotOpenedException& other) : e_what_(other.e_what_) {}
+  virtual ~PortNotOpenedException() throw() {}
+  virtual const char* what () const throw () {
+    return e_what_.c_str();
+  }
+};
+
+/*!
+ * Structure that describes a serial device.
+ */
+struct PortInfo {
+
+  /*! Address of the serial port (this can be passed to the constructor of Serial). */
+  std::string port;
+
+  /*! Human readable description of serial device if available. */
+  std::string description;
+
+  /*! Hardware ID (e.g. VID:PID of USB serial devices) or "n/a" if not available. */
+  std::string hardware_id;
+
+};
+
+/* Lists the serial ports available on the system
+ *
+ * Returns a vector of available serial ports, each represented
+ * by a serial::PortInfo data structure:
+ *
+ * \return vector of serial::PortInfo.
+ */
+std::vector<PortInfo>
+list_ports();
+
+} // namespace serial
+
+#endif

src/imu_enc/include/serial/v8stdint.h

@@ -0,0 +1,57 @@
+// This header is from the v8 google project:
+// http://code.google.com/p/v8/source/browse/trunk/include/v8stdint.h
+
+// Copyright 2012 the V8 project authors. All rights reserved.
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above
+//       copyright notice, this list of conditions and the following
+//       disclaimer in the documentation and/or other materials provided
+//       with the distribution.
+//     * Neither the name of Google Inc. nor the names of its
+//       contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+// Load definitions of standard types.
+
+#ifndef V8STDINT_H_
+#define V8STDINT_H_
+
+#include <stddef.h>
+#include <stdio.h>
+
+#if defined(_WIN32) && !defined(__MINGW32__)
+
+typedef signed char int8_t;
+typedef unsigned char uint8_t;
+typedef short int16_t;  // NOLINT
+typedef unsigned short uint16_t;  // NOLINT
+typedef int int32_t;
+typedef unsigned int uint32_t;
+typedef __int64 int64_t;
+typedef unsigned __int64 uint64_t;
+// intptr_t and friends are defined in crtdefs.h through stdio.h.
+
+#else
+
+#include <stdint.h>
+
+#endif
+
+#endif  // V8STDINT_H_

src/imu_enc/launch/imu.launch

@@ -0,0 +1,19 @@
+
+<launch>
+
+<!-- 
+  <group ns="turtlesim1">
+    <node pkg="turtlesim" name="sim" type="turtlesim_node"/>
+  </group>
+ 
+  <group ns="turtlesim2">
+    <node pkg="turtlesim" name="sim" type="turtlesim_node"/>
+  </group> -->
+
+<!-- <node pkg="imu_enc" name="imu" type="imu_enc_node"/> -->
+<node pkg="rviz" name="rviz" type="rviz" args="-d $(find imu_enc)/launch/demo.rviz"/>
+ <node pkg="tf" type="static_transform_publisher" name="map_to_odom"
+    args="0.0 0.0 0.0 0.0 0.0 0.0 1.0 /map /odom 100" />
+
+</launch>
+

src/imu_enc/package.xml

@@ -0,0 +1,70 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>imu_enc</name>
+  <version>0.0.0</version>
+  <description>The imu_enc package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="youchen@todo.todo">youchen</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>TODO</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/imu_enc</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> -->
+
+
+  <!-- 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_depend>std_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>nav_msgs</build_depend>
+
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>tf</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/imu_enc/src/JY901.cpp

@@ -0,0 +1,119 @@
+#include "JY901.h"
+
+CJY901 ::CJY901 ()
+{
+	// minimum value filter, ignore small values near 0
+	accInitLimit = 0.0001; // +- 0.005 m/s^2
+	gyroInitLimit = 0.0001; // +- 0.002 rad/s
+	angleInitLimit = 0.0001;
+	// max difference in delta t
+	accDiffLimit = 5.0; // 5.0 m/s^2 
+	gyroDiffLimit = 2.7; // 0.3 rad/s ~~ 20 deg/s
+	angleDiffLimit = 0.7; // 0.035 deg ~~ 2 deg
+}
+
+bool CJY901::CopeSerialData(char ucData[], int& start, int& end, unsigned short usLength)
+{
+	static unsigned char chrTemp[1000];
+	static unsigned char ucRxCnt = 0;	
+	static unsigned short usRxLength = 0;
+
+	// backup last value
+	for (size_t i = 0; i < 3; i++)
+	{
+		lastAcc.a[i] = stcAcc.a[i];
+		lastGyro.w[i] = stcGyro.w[i];
+		lastAngle.Angle[i] = stcAngle.Angle[i];
+	}
+
+	if(end>start && (start+usLength)>end){
+		usLength=end-start+1;
+		end += 1;
+		end %= 1000;
+		start = end;
+	}else if(end<start){
+		if((start+usLength)<=1000){
+			//remain unchanged
+		}else if((start+usLength)%1000>end){
+			usLength = end + 1000 - start;
+			end +=1;
+			end %=1000;
+			start = end;
+		}
+	}else if(start==end){
+		return false;
+	}
+	// std::cout<<"start, end, len: "<<start<<" "<<end<<" "<<usLength<<std::endl;
+    memcpy(chrTemp,ucData+start,usLength);
+	usRxLength += usLength;
+    while (usRxLength >= 11)
+    {
+        if (chrTemp[0] != 0x55)
+        {
+			usRxLength--;
+			memcpy(&chrTemp[0],&chrTemp[1],usRxLength);                        
+            continue;
+        }
+		switch(chrTemp[1])
+		{
+			case 0x50:	memcpy(&stcTime,&chrTemp[2],8);break;
+			case 0x51:	memcpy(&stcAcc,&chrTemp[2],8);break;
+			case 0x52:	memcpy(&stcGyro,&chrTemp[2],8);break;
+			case 0x53:	memcpy(&stcAngle,&chrTemp[2],8);break;
+			case 0x54:	memcpy(&stcMag,&chrTemp[2],8);break;
+			case 0x55:	memcpy(&stcDStatus,&chrTemp[2],8);break;
+			case 0x56:	memcpy(&stcPress,&chrTemp[2],8);break;
+			case 0x57:	memcpy(&stcLonLat,&chrTemp[2],8);break;
+			case 0x58:	memcpy(&stcGPSV,&chrTemp[2],8);break;
+		}
+		usRxLength -= 11;
+		memcpy(&chrTemp[0],&chrTemp[11],usRxLength);                     
+    }
+	return checkData();
+}
+
+bool CJY901::checkData(){
+	for (size_t i = 0; i < 3; i++)
+	{
+		// 值过小忽略
+		if (fabs((float)stcAcc.a[i] / 32768 * 16 * 9.8) < accInitLimit && (i == 0 || i == 1))
+		{
+			// std::cout << "acc x or y too small" << std::endl;
+			return false;
+		}
+		if (fabs((float)stcAcc.a[i] / 32768 * 16 - 1) * 9.8 < accInitLimit && i == 2)
+		{
+			// std::cout << "acc z too small" << std::endl;
+			return false;
+		}
+		if (fabs((float)stcGyro.w[i] / 32768 * 2000 * M_PI / 180.0) < gyroInitLimit)
+		{
+			// std::cout << "omega too small" << std::endl;
+			return false;
+		}
+		if (fabs((float)stcAngle.Angle[i] / 32768 * M_PI) < angleInitLimit)
+		{
+			// std::cout << "angle too small" << std::endl;
+			return false;
+		}
+
+		// 值差异过大，舍弃
+		if (fabs((float)(stcAcc.a[i] - lastAcc.a[i]) / 32768 * 16 * 9.8) > accDiffLimit)
+		{
+			// std::cout << "diff acc too big" << std::endl;
+			return false;
+		}
+		if (fabs((float)(stcGyro.w[i] - lastGyro.w[i]) / 32768 * 2000 * M_PI / 180.0) > gyroDiffLimit)
+		{
+			// std::cout << "diff omega too big" << std::endl;
+			return false;
+		}
+		if (fabs((float)(stcAngle.Angle[i] - lastAngle.Angle[i]) / 32768 * M_PI) > angleDiffLimit)
+		{
+			// std::cout << "diff angle too big" << std::endl;
+			return false;
+		}
+	}
+	return true;
+}
+// CJY901 JY901 = CJY901();

src/imu_enc/src/imu_node.cpp

@@ -0,0 +1,200 @@
+#include "imu_node.h"
+
+
+ImuNode::ImuNode(std::string port, unsigned long baud){
+    p_buffer = (char *)malloc((UARTBufferLength+2)*sizeof(char));
+    p_temp_buf = (uint8_t *)malloc((iBufferSize+2)*sizeof(uint8_t));
+    // p_temp_buffer = (char *)malloc(iBufferSize*sizeof(char));
+    start = 0;
+    end = 0;
+    b_exit_ = false;
+    read_thread_ = NULL;
+    last_time_ = ros::Time::now();
+    ori_time_ = ros::Time::now();
+    last_pose_ = tf::Pose();
+    last_vel_ = geometry_msgs::Twist();
+    // port, baudrate, timeout in milliseconds
+    try
+    {
+        my_serial = new serial::Serial(port, baud, serial::Timeout::simpleTimeout(1000));
+        if (my_serial->isOpen())
+        {
+            read_thread_ = new boost::thread(boost::bind(ImuNode::thread_get_data, this));
+        }
+    }
+    catch (serial::IOException &e)
+    {
+        std::cout << "serial exception: " << e.what() << std::endl;
+        b_exit_ = true;
+    }
+    catch (std::exception &e)
+    {
+        std::cerr << "Unhandled Exception: " << e.what() << std::endl;
+        b_exit_ = true;
+    }
+    // std::cout<<my_serial.isOpen()<<std::endl;
+    odom_pub_ = nh_.advertise<nav_msgs::Odometry>("odom",2);
+}
+
+ImuNode::~ImuNode(){
+    if (!ros::ok)
+    {
+        b_exit_ = true;
+    }
+    if (read_thread_!=NULL)
+    {
+        std::cout<<read_thread_<<" thread waiting to stop."<<std::endl;
+        read_thread_->join();
+    }
+    free(p_buffer);
+    free(p_temp_buf);
+}
+
+void ImuNode::publish_data(){
+    if (!b_exit_ && b_connected_)
+    {
+        // usleep(10 * 1000);
+        curr_time_ = ros::Time::now();
+        double dt = (curr_time_ - last_time_).toSec();
+        // std::vector<uint8_t> buff;
+        // my_serial->read(buff, 100);
+        // int start_index=0,end_index=100;
+        std::unique_lock<std::mutex> lck(mtx);
+        if (JY901.CopeSerialData(p_buffer, start, end, 5))
+        // if (JY901.CopeSerialData((char *)buff.data(), start_index, end_index, 100))
+        {
+            // 赋值当前位置与速度
+            tf::Vector3 last_pos = last_pose_.getOrigin();
+            // 利用加速度计算速度
+            // curr_vel_.linear.x = last_vel_.linear.x + dt * (float)JY901.stcAcc.a[0] / 32768 * 16 * 9.8;
+            // curr_vel_.linear.y = last_vel_.linear.y + dt * (float)JY901.stcAcc.a[1] / 32768 * 16 * 9.8;
+            // curr_vel_.linear.z = last_vel_.linear.z + dt * ((float)JY901.stcAcc.a[2] / 32768 * 16 - 1) * 9.8;
+            // 保存原始加速度,用于测试
+            curr_vel_.linear.x = (float)JY901.stcAcc.a[0] / 32768 * 16 * 9.8;
+            curr_vel_.linear.y = (float)JY901.stcAcc.a[1] / 32768 * 16 * 9.8;
+            curr_vel_.linear.z = ((float)JY901.stcAcc.a[2] / 32768 * 16 - 1) * 9.8;
+
+            // 利用陀螺仪生成角速度
+            // curr_vel_.angular.x = last_vel_.angular.x + (float)JY901.stcGyro.w[0] / 32768 * 2000 * M_PI / 180.0;
+            // curr_vel_.angular.y = last_vel_.angular.y + (float)JY901.stcGyro.w[1] / 32768 * 2000 * M_PI / 180.0;
+            // curr_vel_.angular.z = last_vel_.angular.z + (float)JY901.stcGyro.w[2] / 32768 * 2000 * M_PI / 180.0;
+			curr_vel_.angular.x = (float)JY901.stcGyro.w[0] / 32768 * 2000 * M_PI / 180.0;
+			curr_vel_.angular.y = (float)JY901.stcGyro.w[1] / 32768 * 2000 * M_PI / 180.0;
+			curr_vel_.angular.z = (float)JY901.stcGyro.w[2] / 32768 * 2000 * M_PI / 180.0;
+            // 利用磁场综合获取角度信息，扩展角度范围从[-pi, pi]到[-2pi, 2pi]
+            double angle[3] = {0.0};
+            for (size_t i = 0; i < 3; i++)
+            {
+                angle[i] = (float)JY901.stcAngle.Angle[i] / 32768 * M_PI;
+                // if (JY901.stcAngle.Angle[i] * JY901.lastAngle.Angle[i] < 0)
+                // {
+                //     std::cout<<i<<" 反号"<<std::endl;
+                //     if (JY901.stcAngle.Angle[i] / 32768 * M_PI < -M_PI_2)
+                //     {
+                //         std::cout<<"---"<<std::endl;
+                //         m_angle_status_[i] = 1;
+                //     }
+                //     else if (JY901.stcAngle.Angle[i] / 32768 * M_PI > M_PI_2)
+                //     {
+                //         std::cout<<"+++"<<std::endl;
+                //         m_angle_status_[i] = 2;
+                //     }
+                // }
+                // if (m_angle_status_[i] == 1)
+                // {
+                //     if (JY901.stcAngle.Angle[i] > 0)
+                //     {
+                //         m_angle_status_[i] = 0;
+                //     }
+                //     else
+                //     {
+                //         angle[i] += M_PI * 2;
+                //     }
+                // }
+                // if (m_angle_status_[i] == 2)
+                // {
+                //     if (JY901.stcAngle.Angle[i] < 0)
+                //     {
+                //         m_angle_status_[i] == 0;
+                //     }
+                //     else
+                //     {
+                //         angle[i] -= M_PI * 2;
+                //     }
+                // }
+            }
+
+            tf::Quaternion quat;quat.setRPY(angle[0],angle[1],angle[2]);
+            curr_pose_.setRotation(quat);
+            
+            // curr_pose_.setRotation(tf::Quaternion((float)JY901.stcAngle.Angle[0] / 32768 * M_PI,
+            //                                     (float)JY901.stcAngle.Angle[1] / 32768 * M_PI, 
+            //                                     (float)JY901.stcAngle.Angle[2] / 32768 * M_PI));
+            // // 积分获得位置信息，存在累计误差
+            // curr_pose_.setOrigin(tf::Vector3(last_pos.getX() + (last_vel_.linear.x+curr_vel_.linear.x)/2.0 * dt,
+            //                                 last_pos.getY() + (last_vel_.linear.y+curr_vel_.linear.y)/2.0 * dt,
+            //                                 last_pos.getZ() + (last_vel_.linear.z+curr_vel_.linear.z)/2.0 * dt));
+            curr_pose_.setOrigin(tf::Vector3(1, 0, 0));
+
+            last_vel_ = curr_vel_;
+            last_pose_ = curr_pose_;
+
+            //std::cout << "----------- Time: " << (ros::Time::now().toNSec()-ori_time_.toNSec())/1000000 << "-----------" << std::endl;
+            printf("Acc: %.4f %.4f %.4f\n", (float)JY901.stcAcc.a[0] / 32768 * 16 * 9.8, (float)JY901.stcAcc.a[1] / 32768 * 16 * 9.8, (float)JY901.stcAcc.a[2] / 32768 * 16 * 9.8);
+            printf("Gyro: %.4f %.4f %.4f\n", (float)JY901.stcGyro.w[0] / 32768 * 2000 * M_PI / 180.0, (float)JY901.stcGyro.w[1] / 32768 * 2000 * M_PI / 180.0, (float)JY901.stcGyro.w[2] / 32768 * 2000 * M_PI / 180.0);
+            printf("linear: %.4f %.4f %.4f\n", curr_vel_.linear.x, curr_vel_.linear.y, curr_vel_.linear.z);
+
+            //printf("Angle: %.4f %.4f %.4f\n", (float)JY901.stcAngle.Angle[0] / 32768 * M_PI, (float)JY901.stcAngle.Angle[1] / 32768 * M_PI, (float)JY901.stcAngle.Angle[2] / 32768 * M_PI);
+		    //printf("Ang speed: %.4f %.4f %.4f\n", curr_vel_.angular.x, curr_vel_.angular.y, curr_vel_.angular.z);
+			// std::cout<<curr_pose_.getBasis().getRow(0)[0] <<" "<< curr_pose_.getBasis().getRow(0)[1] <<" "<< curr_pose_.getBasis().getRow(0)[2] <<std::endl;
+            // std::cout<<curr_pose_.getBasis().getRow(1)[0] <<" "<< curr_pose_.getBasis().getRow(1)[1] <<" "<< curr_pose_.getBasis().getRow(1)[2] <<std::endl;
+            // std::cout<<curr_pose_.getBasis().getRow(2)[0] <<" "<< curr_pose_.getBasis().getRow(2)[1] <<" "<< curr_pose_.getBasis().getRow(2)[2] <<std::endl;
+            // printf("quaternion: %.4f %.4f %.4f %.4f\n", quat.x(),quat.y(),quat.z(),quat.w());
+        }
+        else
+        {
+            curr_vel_.linear = geometry_msgs::Vector3();
+            curr_vel_.angular = geometry_msgs::Vector3();
+            curr_pose_.setRotation(last_pose_.getRotation());
+            curr_pose_.setOrigin(last_pose_.getOrigin());
+            printf("failed\n");
+        }
+        // 发布里程计信息与tf
+        nav_msgs::Odometry odom;
+        odom.header.frame_id = "odom";
+        odom.child_frame_id = "base_link";
+        odom.header.stamp = curr_time_;
+        tf::poseTFToMsg(curr_pose_, odom.pose.pose);
+        odom.twist.twist.linear = curr_vel_.linear;
+        odom.twist.twist.angular = curr_vel_.angular;
+        odom_pub_.publish(odom);
+
+        tf::StampedTransform tf_msg(curr_pose_, ros::Time::now(), "odom", "base_link");
+        tf_broadcaster_.sendTransform(tf_msg);
+
+        // std::cout<<"-----------"<< ros::Time::now().toSec() <<"-----------"<<std::endl;
+        // printf("Acc:\t%.3f\t %.3f\t %.3f\t", (float)JY901.stcAcc.a[0] / 32768 * 16 * 9.8, (float)JY901.stcAcc.a[1] / 32768 * 16 * 9.8, (float)JY901.stcAcc.a[2] / 32768 * 16 * 9.8);
+        // printf("Gyro:\t%.3f\t %.3f\t %.3f\t", (float)JY901.stcGyro.w[0] / 32768 * 2000 * M_PI / 180.0, (float)JY901.stcGyro.w[1] / 32768 * 2000 * M_PI / 180.0, (float)JY901.stcGyro.w[2] / 32768 * 2000 * M_PI / 180.0);
+        // printf("Angle:\t%.3f\t %.3f\t %.3f\r\n", (float)JY901.stcAngle.Angle[0] / 32768 * M_PI, (float)JY901.stcAngle.Angle[1] / 32768 * M_PI, (float)JY901.stcAngle.Angle[2] / 32768 * M_PI);
+        // printf("Mag:%d %d %d\r\n", JY901.stcMag.h[0], JY901.stcMag.h[1], JY901.stcMag.h[2]);
+        // printf("DStatus:%d %d %d %d\r\n", JY901.stcDStatus.sDStatus[0], JY901.stcDStatus.sDStatus[1], JY901.stcDStatus.sDStatus[2], JY901.stcDStatus.sDStatus[3]);
+        last_time_ = curr_time_;
+    }
+}
+
+int main(int argc, char** argv){
+    ros::init(argc, argv, "imu_node");
+    ImuNode imu("/dev/ttyUSB0", 38400);
+    ros::Rate loop_rate(200);
+    while (ros::ok())
+    {
+        if(imu.b_exit_)
+            return 1;
+        imu.publish_data();
+        ros::spinOnce();
+        loop_rate.sleep();
+    }
+    std::cout<<"leave"<<std::endl;
+    imu.b_exit_ = true;
+    return 1;
+}

src/imu_enc/src/serial_example.cc

@@ -0,0 +1,182 @@
+/***
+ * This example expects the serial port has a loopback on it.
+ *
+ * Alternatively, you could use an Arduino:
+ *
+ * <pre>
+ *  void setup() {
+ *    Serial.begin(<insert your baudrate here>);
+ *  }
+ *
+ *  void loop() {
+ *    if (Serial.available()) {
+ *      Serial.write(Serial.read());
+ *    }
+ *  }
+ * </pre>
+ */
+
+#include <string>
+#include <iostream>
+#include <cstdio>
+
+// OS Specific sleep
+#ifdef _WIN32
+#include <windows.h>
+#else
+#include <unistd.h>
+#endif
+
+#include "serial/serial.h"
+#include <chrono>
+
+using std::string;
+using std::exception;
+using std::cout;
+using std::cerr;
+using std::endl;
+using std::vector;
+
+#include "JY901.h"
+CJY901 JY901;
+
+void my_sleep(unsigned long milliseconds) {
+#ifdef _WIN32
+      Sleep(milliseconds); // 100 ms
+#else
+      usleep(milliseconds*1000); // 100 ms
+#endif
+}
+
+void enumerate_ports()
+{
+	vector<serial::PortInfo> devices_found = serial::list_ports();
+
+	vector<serial::PortInfo>::iterator iter = devices_found.begin();
+
+	while( iter != devices_found.end() )
+	{
+		serial::PortInfo device = *iter++;
+
+		printf( "(%s, %s, %s)\n", device.port.c_str(), device.description.c_str(),
+     device.hardware_id.c_str() );
+	}
+}
+
+void print_usage()
+{
+	cerr << "Usage: test_serial {-e|<serial port address>} ";
+    cerr << "<baudrate> [test string]" << endl;
+}
+
+int run(int argc, char **argv)
+{
+  if(argc < 2) {
+	  print_usage();
+    return 0;
+  }
+
+  // Argument 1 is the serial port or enumerate flag
+  string port(argv[1]);
+
+  if( port == "-e" ) {
+	  enumerate_ports();
+	  return 0;
+  }
+  else if( argc < 3 ) {
+	  print_usage();
+	  return 1;
+  }
+
+  // Argument 2 is the baudrate
+  unsigned long baud = 0;
+#if defined(WIN32) && !defined(__MINGW32__)
+  sscanf_s(argv[2], "%lu", &baud);
+#else
+  sscanf(argv[2], "%lu", &baud);
+#endif
+
+  // port, baudrate, timeout in milliseconds
+  serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(1000));
+
+  cout << "Is the serial port open?";
+  if(my_serial.isOpen())
+    cout << " Yes." << endl;
+  else
+    cout << " No." << endl;
+
+  // Get the Test string
+  int count = 0;
+  string test_string;
+  if (argc == 4) {
+    test_string = argv[3];
+  } else {
+    test_string = "Testing.";
+  }
+
+  // Test the timeout, there should be 1 second between prints
+  cout << "Timeout == 1000ms, asking for 1 more byte than written." << endl;
+  std::chrono::time_point<std::chrono::system_clock> start = std::chrono::system_clock::now();
+  while (count < 200) {
+    //size_t bytes_wrote = my_serial.write(test_string);
+
+    //string result = my_serial.read(test_string.length()+14);
+    std::vector<uint8_t> buff;
+    my_serial.read(buff, 11);
+    cout << "Iteration: " << count << ", Bytes read: ";
+    cout << buff.size() << ", String read: " ;
+    for (size_t i = 0; i < buff.size(); i++)
+    {
+      cout<< /*std::ios::dec <<*/ (int)(char)buff[i] <<"\t";
+    }
+    cout<< endl;
+    count += 1;
+  }
+  std::chrono::time_point<std::chrono::system_clock> end = std::chrono::system_clock::now();
+  std::cout<<"time: "<<std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()<<std::endl;
+
+  count=1000;
+  int start_index=0,end_index=100;
+  while(count-->0){
+    std::vector<uint8_t> buff;
+    my_serial.read(buff, 100);
+    if (JY901.CopeSerialData((char *)buff.data(), start_index, end_index, 100))
+    {
+
+      printf("Acc:\t%.3f\t %.3f\t %.3f\t", (float)JY901.stcAcc.a[0] / 32768 * 16 * 9.8, (float)JY901.stcAcc.a[1] / 32768 * 16 * 9.8, (float)JY901.stcAcc.a[2] / 32768 * 16 * 9.8);
+      printf("Gyro:\t%.3f\t %.3f\t %.3f\t", (float)JY901.stcGyro.w[0] / 32768 * 2000 * M_PI / 180.0, (float)JY901.stcGyro.w[1] / 32768 * 2000 * M_PI / 180.0, (float)JY901.stcGyro.w[2] / 32768 * 2000 * M_PI / 180.0);
+      printf("Angle:\t%.3f\t %.3f\t %.3f\r\n", (float)JY901.stcAngle.Angle[0] / 32768 * M_PI, (float)JY901.stcAngle.Angle[1] / 32768 * M_PI, (float)JY901.stcAngle.Angle[2] / 32768 * M_PI);
+      std::cout << "----------------------" << std::endl;
+    }
+    // printf("Time:20%d-%d-%d %d:%d:%.3f\r\n",(short)JY901.stcTime.ucYear,(short)JY901.stcTime.ucMonth,
+    //     (short)JY901.stcTime.ucDay,(short)JY901.stcTime.ucHour,(short)JY901.stcTime.ucMinute,(float)JY901.stcTime.ucSecond+(float)JY901.stcTime.usMiliSecond/1000);
+
+    // printf("Acc:%.3f %.3f %.3f\r\n",(float)JY901.stcAcc.a[0]/32768*16,(float)JY901.stcAcc.a[1]/32768*16,(float)JY901.stcAcc.a[2]/32768*16);
+
+    // printf("Gyro:%.3f %.3f %.3f\r\n",(float)JY901.stcGyro.w[0]/32768*2000,(float)JY901.stcGyro.w[1]/32768*2000,(float)JY901.stcGyro.w[2]/32768*2000);
+
+    // printf("Angle:%.3f %.3f %.3f\r\n",(float)JY901.stcAngle.Angle[0]/32768*180,(float)JY901.stcAngle.Angle[1]/32768*180,(float)JY901.stcAngle.Angle[2]/32768*180);
+
+    // printf("Mag:%d %d %d\r\n",JY901.stcMag.h[0],JY901.stcMag.h[1],JY901.stcMag.h[2]);
+
+    // printf("Pressure:%lx Height%.2f\r\n",JY901.stcPress.lPressure,(float)JY901.stcPress.lAltitude/100);
+
+    // printf("DStatus:%d %d %d %d\r\n",JY901.stcDStatus.sDStatus[0],JY901.stcDStatus.sDStatus[1],JY901.stcDStatus.sDStatus[2],JY901.stcDStatus.sDStatus[3]);
+
+    // printf("Longitude:%ldDeg%.5fm Lattitude:%ldDeg%.5fm\r\n",JY901.stcLonLat.lLon/10000000,(double)(JY901.stcLonLat.lLon % 10000000)/1e5,JY901.stcLonLat.lLat/10000000,(double)(JY901.stcLonLat.lLat % 10000000)/1e5);
+
+    // printf("GPSHeight:%.1fm GPSYaw:%.1fDeg GPSV:%.3fkm/h\r\n\r\n",(float)JY901.stcGPSV.sGPSHeight/10,(float)JY901.stcGPSV.sGPSYaw/10,(float)JY901.stcGPSV.lGPSVelocity/1000);
+
+    usleep(5*1000);
+  }
+
+  return 0;
+}
+
+int main(int argc, char **argv) {
+  try {
+    return run(argc, argv);
+  } catch (exception &e) {
+    cerr << "Unhandled Exception: " << e.what() << endl;
+  }
+}

src/pkgname/CMakeLists.txt

@@ -0,0 +1,68 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(pkgname)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++14)
+
+## 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
+  std_msgs
+)
+
+find_package(PCL REQUIRED)
+FIND_PACKAGE(Protobuf REQUIRED)
+
+set(vtk_flags)
+foreach(it ${dir_defs})
+ if(it MATCHES "vtk*")
+  list(APPEND vtk_flags ${it})
+ endif()
+endforeach()
+
+foreach(d ${vtk_flags})
+ remove_definitions(-D${d})
+endforeach()
+
+catkin_package(
+	
+#  INCLUDE_DIRS include
+#  LIBRARIES pkgname
+#  CATKIN_DEPENDS roscpp std_msgs
+#  DEPENDS system_lib
+)
+
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/src/communication COMMUNICATION_SRC )
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/src/message MESSAGE_SRC )
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/src/tool TOOL_SRC )
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/src/error_code ERROR_SRC )
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+  ${PROTOBUF_INCLUDE_DIRS}
+  ./src/communication
+  ./src/message
+  ./src/tool
+  ./src/error_code
+)
+
+add_executable(${PROJECT_NAME}_node ./src/main.cpp
+        ${TOOL_SRC}
+        ${COMMUNICATION_SRC}
+        ${MESSAGE_SRC}
+        ${ERROR_SRC}
+        )
+
+ target_link_libraries(${PROJECT_NAME}_node
+         /usr/local/lib/libglog.a
+         /usr/local/lib/libgflags.a
+   ${catkin_LIBRARIES}
+   ${PCL_LIBRARIES}
+   ${PROTOBUF_LIBRARIES}
+   nnxx
+   nanomsg
+ )
+

src/pkgname/launch/agv.launch

@@ -0,0 +1,5 @@
+<launch>
+	<node name="map_server" pkg="map_server" type="map_server" args="$(find pkgname)/launch/map_1cm.yaml"/>
+  <node pkg="pkgname" type="pkgname_node" name="pkgname_node" output="screen"/>
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find pkgname)/launch/default.rviz" />
+</launch>

src/pkgname/launch/map_1cm.yaml

@@ -0,0 +1,6 @@
+image: /home/youchen/extra_space/agv_200731/src/pkgname/launch/map_1cm.pgm
+resolution: 0.010000
+origin: [-15.986741, -12.390000, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

src/pkgname/package.xml

@@ -0,0 +1,65 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>pkgname</name>
+  <version>0.0.0</version>
+  <description>The pkgname package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="zx@todo.todo">zx</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>TODO</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/pkgname</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> -->
+
+
+  <!-- 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_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/pkgname/src/communication/communication.pb.cc

@@ -0,0 +1,729 @@
+// Generated by the protocol buffer compiler.  DO NOT EDIT!
+// source: communication.proto
+
+#include "communication.pb.h"
+
+#include <algorithm>
+
+#include <google/protobuf/stubs/common.h>
+#include <google/protobuf/stubs/port.h>
+#include <google/protobuf/stubs/once.h>
+#include <google/protobuf/io/coded_stream.h>
+#include <google/protobuf/wire_format_lite_inl.h>
+#include <google/protobuf/descriptor.h>
+#include <google/protobuf/generated_message_reflection.h>
+#include <google/protobuf/reflection_ops.h>
+#include <google/protobuf/wire_format.h>
+// This is a temporary google only hack
+#ifdef GOOGLE_PROTOBUF_ENFORCE_UNIQUENESS
+#include "third_party/protobuf/version.h"
+#endif
+// @@protoc_insertion_point(includes)
+namespace Communication_proto {
+class Communication_parameterDefaultTypeInternal {
+ public:
+  ::google::protobuf::internal::ExplicitlyConstructed<Communication_parameter>
+      _instance;
+} _Communication_parameter_default_instance_;
+class Communication_parameter_allDefaultTypeInternal {
+ public:
+  ::google::protobuf::internal::ExplicitlyConstructed<Communication_parameter_all>
+      _instance;
+} _Communication_parameter_all_default_instance_;
+}  // namespace Communication_proto
+namespace protobuf_communication_2eproto {
+void InitDefaultsCommunication_parameterImpl() {
+  GOOGLE_PROTOBUF_VERIFY_VERSION;
+
+#ifdef GOOGLE_PROTOBUF_ENFORCE_UNIQUENESS
+  ::google::protobuf::internal::InitProtobufDefaultsForceUnique();
+#else
+  ::google::protobuf::internal::InitProtobufDefaults();
+#endif  // GOOGLE_PROTOBUF_ENFORCE_UNIQUENESS
+  {
+    void* ptr = &::Communication_proto::_Communication_parameter_default_instance_;
+    new (ptr) ::Communication_proto::Communication_parameter();
+    ::google::protobuf::internal::OnShutdownDestroyMessage(ptr);
+  }
+  ::Communication_proto::Communication_parameter::InitAsDefaultInstance();
+}
+
+void InitDefaultsCommunication_parameter() {
+  static GOOGLE_PROTOBUF_DECLARE_ONCE(once);
+  ::google::protobuf::GoogleOnceInit(&once, &InitDefaultsCommunication_parameterImpl);
+}
+
+void InitDefaultsCommunication_parameter_allImpl() {
+  GOOGLE_PROTOBUF_VERIFY_VERSION;
+
+#ifdef GOOGLE_PROTOBUF_ENFORCE_UNIQUENESS
+  ::google::protobuf::internal::InitProtobufDefaultsForceUnique();
+#else
+  ::google::protobuf::internal::InitProtobufDefaults();
+#endif  // GOOGLE_PROTOBUF_ENFORCE_UNIQUENESS
+  protobuf_communication_2eproto::InitDefaultsCommunication_parameter();
+  {
+    void* ptr = &::Communication_proto::_Communication_parameter_all_default_instance_;
+    new (ptr) ::Communication_proto::Communication_parameter_all();
+    ::google::protobuf::internal::OnShutdownDestroyMessage(ptr);
+  }
+  ::Communication_proto::Communication_parameter_all::InitAsDefaultInstance();
+}
+
+void InitDefaultsCommunication_parameter_all() {
+  static GOOGLE_PROTOBUF_DECLARE_ONCE(once);
+  ::google::protobuf::GoogleOnceInit(&once, &InitDefaultsCommunication_parameter_allImpl);
+}
+
+::google::protobuf::Metadata file_level_metadata[2];
+
+const ::google::protobuf::uint32 TableStruct::offsets[] GOOGLE_PROTOBUF_ATTRIBUTE_SECTION_VARIABLE(protodesc_cold) = {
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::Communication_proto::Communication_parameter, _has_bits_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::Communication_proto::Communication_parameter, _internal_metadata_),
+  ~0u,  // no _extensions_
+  ~0u,  // no _oneof_case_
+  ~0u,  // no _weak_field_map_
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::Communication_proto::Communication_parameter, bind_string_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::Communication_proto::Communication_parameter, connect_string_vector_),
+  0,
+  ~0u,
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::Communication_proto::Communication_parameter_all, _has_bits_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::Communication_proto::Communication_parameter_all, _internal_metadata_),
+  ~0u,  // no _extensions_
+  ~0u,  // no _oneof_case_
+  ~0u,  // no _weak_field_map_
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::Communication_proto::Communication_parameter_all, communication_parameters_),
+  0,
+};
+static const ::google::protobuf::internal::MigrationSchema schemas[] GOOGLE_PROTOBUF_ATTRIBUTE_SECTION_VARIABLE(protodesc_cold) = {
+  { 0, 7, sizeof(::Communication_proto::Communication_parameter)},
+  { 9, 15, sizeof(::Communication_proto::Communication_parameter_all)},
+};
+
+static ::google::protobuf::Message const * const file_default_instances[] = {
+  reinterpret_cast<const ::google::protobuf::Message*>(&::Communication_proto::_Communication_parameter_default_instance_),
+  reinterpret_cast<const ::google::protobuf::Message*>(&::Communication_proto::_Communication_parameter_all_default_instance_),
+};
+
+void protobuf_AssignDescriptors() {
+  AddDescriptors();
+  ::google::protobuf::MessageFactory* factory = NULL;
+  AssignDescriptors(
+      "communication.proto", schemas, file_default_instances, TableStruct::offsets, factory,
+      file_level_metadata, NULL, NULL);
+}
+
+void protobuf_AssignDescriptorsOnce() {
+  static GOOGLE_PROTOBUF_DECLARE_ONCE(once);
+  ::google::protobuf::GoogleOnceInit(&once, &protobuf_AssignDescriptors);
+}
+
+void protobuf_RegisterTypes(const ::std::string&) GOOGLE_PROTOBUF_ATTRIBUTE_COLD;
+void protobuf_RegisterTypes(const ::std::string&) {
+  protobuf_AssignDescriptorsOnce();
+  ::google::protobuf::internal::RegisterAllTypes(file_level_metadata, 2);
+}
+
+void AddDescriptorsImpl() {
+  InitDefaults();
+  static const char descriptor[] GOOGLE_PROTOBUF_ATTRIBUTE_SECTION_VARIABLE(protodesc_cold) = {
+      "\n\023communication.proto\022\023Communication_pro"
+      "to\"M\n\027Communication_parameter\022\023\n\013bind_st"
+      "ring\030\001 \001(\t\022\035\n\025connect_string_vector\030\002 \003("
+      "\t\"m\n\033Communication_parameter_all\022N\n\030comm"
+      "unication_parameters\030\001 \001(\0132,.Communicati"
+      "on_proto.Communication_parameter"
+  };
+  ::google::protobuf::DescriptorPool::InternalAddGeneratedFile(
+      descriptor, 232);
+  ::google::protobuf::MessageFactory::InternalRegisterGeneratedFile(
+    "communication.proto", &protobuf_RegisterTypes);
+}
+
+void AddDescriptors() {
+  static GOOGLE_PROTOBUF_DECLARE_ONCE(once);
+  ::google::protobuf::GoogleOnceInit(&once, &AddDescriptorsImpl);
+}
+// Force AddDescriptors() to be called at dynamic initialization time.
+struct StaticDescriptorInitializer {
+  StaticDescriptorInitializer() {
+    AddDescriptors();
+  }
+} static_descriptor_initializer;
+}  // namespace protobuf_communication_2eproto
+namespace Communication_proto {
+
+// ===================================================================
+
+void Communication_parameter::InitAsDefaultInstance() {
+}
+#if !defined(_MSC_VER) || _MSC_VER >= 1900
+const int Communication_parameter::kBindStringFieldNumber;
+const int Communication_parameter::kConnectStringVectorFieldNumber;
+#endif  // !defined(_MSC_VER) || _MSC_VER >= 1900
+
+Communication_parameter::Communication_parameter()
+  : ::google::protobuf::Message(), _internal_metadata_(NULL) {
+  if (GOOGLE_PREDICT_TRUE(this != internal_default_instance())) {
+    ::protobuf_communication_2eproto::InitDefaultsCommunication_parameter();
+  }
+  SharedCtor();
+  // @@protoc_insertion_point(constructor:Communication_proto.Communication_parameter)
+}
+Communication_parameter::Communication_parameter(const Communication_parameter& from)
+  : ::google::protobuf::Message(),
+      _internal_metadata_(NULL),
+      _has_bits_(from._has_bits_),
+      _cached_size_(0),
+      connect_string_vector_(from.connect_string_vector_) {
+  _internal_metadata_.MergeFrom(from._internal_metadata_);
+  bind_string_.UnsafeSetDefault(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
+  if (from.has_bind_string()) {
+    bind_string_.AssignWithDefault(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), from.bind_string_);
+  }
+  // @@protoc_insertion_point(copy_constructor:Communication_proto.Communication_parameter)
+}
+
+void Communication_parameter::SharedCtor() {
+  _cached_size_ = 0;
+  bind_string_.UnsafeSetDefault(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
+}
+
+Communication_parameter::~Communication_parameter() {
+  // @@protoc_insertion_point(destructor:Communication_proto.Communication_parameter)
+  SharedDtor();
+}
+
+void Communication_parameter::SharedDtor() {
+  bind_string_.DestroyNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
+}
+
+void Communication_parameter::SetCachedSize(int size) const {
+  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
+  _cached_size_ = size;
+  GOOGLE_SAFE_CONCURRENT_WRITES_END();
+}
+const ::google::protobuf::Descriptor* Communication_parameter::descriptor() {
+  ::protobuf_communication_2eproto::protobuf_AssignDescriptorsOnce();
+  return ::protobuf_communication_2eproto::file_level_metadata[kIndexInFileMessages].descriptor;
+}
+
+const Communication_parameter& Communication_parameter::default_instance() {
+  ::protobuf_communication_2eproto::InitDefaultsCommunication_parameter();
+  return *internal_default_instance();
+}
+
+Communication_parameter* Communication_parameter::New(::google::protobuf::Arena* arena) const {
+  Communication_parameter* n = new Communication_parameter;
+  if (arena != NULL) {
+    arena->Own(n);
+  }
+  return n;
+}
+
+void Communication_parameter::Clear() {
+// @@protoc_insertion_point(message_clear_start:Communication_proto.Communication_parameter)
+  ::google::protobuf::uint32 cached_has_bits = 0;
+  // Prevent compiler warnings about cached_has_bits being unused
+  (void) cached_has_bits;
+
+  connect_string_vector_.Clear();
+  cached_has_bits = _has_bits_[0];
+  if (cached_has_bits & 0x00000001u) {
+    GOOGLE_DCHECK(!bind_string_.IsDefault(&::google::protobuf::internal::GetEmptyStringAlreadyInited()));
+    (*bind_string_.UnsafeRawStringPointer())->clear();
+  }
+  _has_bits_.Clear();
+  _internal_metadata_.Clear();
+}
+
+bool Communication_parameter::MergePartialFromCodedStream(
+    ::google::protobuf::io::CodedInputStream* input) {
+#define DO_(EXPRESSION) if (!GOOGLE_PREDICT_TRUE(EXPRESSION)) goto failure
+  ::google::protobuf::uint32 tag;
+  // @@protoc_insertion_point(parse_start:Communication_proto.Communication_parameter)
+  for (;;) {
+    ::std::pair< ::google::protobuf::uint32, bool> p = input->ReadTagWithCutoffNoLastTag(127u);
+    tag = p.first;
+    if (!p.second) goto handle_unusual;
+    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
+      // optional string bind_string = 1;
+      case 1: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(10u /* 10 & 0xFF */)) {
+          DO_(::google::protobuf::internal::WireFormatLite::ReadString(
+                input, this->mutable_bind_string()));
+          ::google::protobuf::internal::WireFormat::VerifyUTF8StringNamedField(
+            this->bind_string().data(), static_cast<int>(this->bind_string().length()),
+            ::google::protobuf::internal::WireFormat::PARSE,
+            "Communication_proto.Communication_parameter.bind_string");
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
+      // repeated string connect_string_vector = 2;
+      case 2: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(18u /* 18 & 0xFF */)) {
+          DO_(::google::protobuf::internal::WireFormatLite::ReadString(
+                input, this->add_connect_string_vector()));
+          ::google::protobuf::internal::WireFormat::VerifyUTF8StringNamedField(
+            this->connect_string_vector(this->connect_string_vector_size() - 1).data(),
+            static_cast<int>(this->connect_string_vector(this->connect_string_vector_size() - 1).length()),
+            ::google::protobuf::internal::WireFormat::PARSE,
+            "Communication_proto.Communication_parameter.connect_string_vector");
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
+      default: {
+      handle_unusual:
+        if (tag == 0) {
+          goto success;
+        }
+        DO_(::google::protobuf::internal::WireFormat::SkipField(
+              input, tag, _internal_metadata_.mutable_unknown_fields()));
+        break;
+      }
+    }
+  }
+success:
+  // @@protoc_insertion_point(parse_success:Communication_proto.Communication_parameter)
+  return true;
+failure:
+  // @@protoc_insertion_point(parse_failure:Communication_proto.Communication_parameter)
+  return false;
+#undef DO_
+}
+
+void Communication_parameter::SerializeWithCachedSizes(
+    ::google::protobuf::io::CodedOutputStream* output) const {
+  // @@protoc_insertion_point(serialize_start:Communication_proto.Communication_parameter)
+  ::google::protobuf::uint32 cached_has_bits = 0;
+  (void) cached_has_bits;
+
+  cached_has_bits = _has_bits_[0];
+  // optional string bind_string = 1;
+  if (cached_has_bits & 0x00000001u) {
+    ::google::protobuf::internal::WireFormat::VerifyUTF8StringNamedField(
+      this->bind_string().data(), static_cast<int>(this->bind_string().length()),
+      ::google::protobuf::internal::WireFormat::SERIALIZE,
+      "Communication_proto.Communication_parameter.bind_string");
+    ::google::protobuf::internal::WireFormatLite::WriteStringMaybeAliased(
+      1, this->bind_string(), output);
+  }
+
+  // repeated string connect_string_vector = 2;
+  for (int i = 0, n = this->connect_string_vector_size(); i < n; i++) {
+    ::google::protobuf::internal::WireFormat::VerifyUTF8StringNamedField(
+      this->connect_string_vector(i).data(), static_cast<int>(this->connect_string_vector(i).length()),
+      ::google::protobuf::internal::WireFormat::SERIALIZE,
+      "Communication_proto.Communication_parameter.connect_string_vector");
+    ::google::protobuf::internal::WireFormatLite::WriteString(
+      2, this->connect_string_vector(i), output);
+  }
+
+  if (_internal_metadata_.have_unknown_fields()) {
+    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
+        _internal_metadata_.unknown_fields(), output);
+  }
+  // @@protoc_insertion_point(serialize_end:Communication_proto.Communication_parameter)
+}
+
+::google::protobuf::uint8* Communication_parameter::InternalSerializeWithCachedSizesToArray(
+    bool deterministic, ::google::protobuf::uint8* target) const {
+  (void)deterministic; // Unused
+  // @@protoc_insertion_point(serialize_to_array_start:Communication_proto.Communication_parameter)
+  ::google::protobuf::uint32 cached_has_bits = 0;
+  (void) cached_has_bits;
+
+  cached_has_bits = _has_bits_[0];
+  // optional string bind_string = 1;
+  if (cached_has_bits & 0x00000001u) {
+    ::google::protobuf::internal::WireFormat::VerifyUTF8StringNamedField(
+      this->bind_string().data(), static_cast<int>(this->bind_string().length()),
+      ::google::protobuf::internal::WireFormat::SERIALIZE,
+      "Communication_proto.Communication_parameter.bind_string");
+    target =
+      ::google::protobuf::internal::WireFormatLite::WriteStringToArray(
+        1, this->bind_string(), target);
+  }
+
+  // repeated string connect_string_vector = 2;
+  for (int i = 0, n = this->connect_string_vector_size(); i < n; i++) {
+    ::google::protobuf::internal::WireFormat::VerifyUTF8StringNamedField(
+      this->connect_string_vector(i).data(), static_cast<int>(this->connect_string_vector(i).length()),
+      ::google::protobuf::internal::WireFormat::SERIALIZE,
+      "Communication_proto.Communication_parameter.connect_string_vector");
+    target = ::google::protobuf::internal::WireFormatLite::
+      WriteStringToArray(2, this->connect_string_vector(i), target);
+  }
+
+  if (_internal_metadata_.have_unknown_fields()) {
+    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
+        _internal_metadata_.unknown_fields(), target);
+  }
+  // @@protoc_insertion_point(serialize_to_array_end:Communication_proto.Communication_parameter)
+  return target;
+}
+
+size_t Communication_parameter::ByteSizeLong() const {
+// @@protoc_insertion_point(message_byte_size_start:Communication_proto.Communication_parameter)
+  size_t total_size = 0;
+
+  if (_internal_metadata_.have_unknown_fields()) {
+    total_size +=
+      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
+        _internal_metadata_.unknown_fields());
+  }
+  // repeated string connect_string_vector = 2;
+  total_size += 1 *
+      ::google::protobuf::internal::FromIntSize(this->connect_string_vector_size());
+  for (int i = 0, n = this->connect_string_vector_size(); i < n; i++) {
+    total_size += ::google::protobuf::internal::WireFormatLite::StringSize(
+      this->connect_string_vector(i));
+  }
+
+  // optional string bind_string = 1;
+  if (has_bind_string()) {
+    total_size += 1 +
+      ::google::protobuf::internal::WireFormatLite::StringSize(
+        this->bind_string());
+  }
+
+  int cached_size = ::google::protobuf::internal::ToCachedSize(total_size);
+  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
+  _cached_size_ = cached_size;
+  GOOGLE_SAFE_CONCURRENT_WRITES_END();
+  return total_size;
+}
+
+void Communication_parameter::MergeFrom(const ::google::protobuf::Message& from) {
+// @@protoc_insertion_point(generalized_merge_from_start:Communication_proto.Communication_parameter)
+  GOOGLE_DCHECK_NE(&from, this);
+  const Communication_parameter* source =
+      ::google::protobuf::internal::DynamicCastToGenerated<const Communication_parameter>(
+          &from);
+  if (source == NULL) {
+  // @@protoc_insertion_point(generalized_merge_from_cast_fail:Communication_proto.Communication_parameter)
+    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
+  } else {
+  // @@protoc_insertion_point(generalized_merge_from_cast_success:Communication_proto.Communication_parameter)
+    MergeFrom(*source);
+  }
+}
+
+void Communication_parameter::MergeFrom(const Communication_parameter& from) {
+// @@protoc_insertion_point(class_specific_merge_from_start:Communication_proto.Communication_parameter)
+  GOOGLE_DCHECK_NE(&from, this);
+  _internal_metadata_.MergeFrom(from._internal_metadata_);
+  ::google::protobuf::uint32 cached_has_bits = 0;
+  (void) cached_has_bits;
+
+  connect_string_vector_.MergeFrom(from.connect_string_vector_);
+  if (from.has_bind_string()) {
+    set_has_bind_string();
+    bind_string_.AssignWithDefault(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), from.bind_string_);
+  }
+}
+
+void Communication_parameter::CopyFrom(const ::google::protobuf::Message& from) {
+// @@protoc_insertion_point(generalized_copy_from_start:Communication_proto.Communication_parameter)
+  if (&from == this) return;
+  Clear();
+  MergeFrom(from);
+}
+
+void Communication_parameter::CopyFrom(const Communication_parameter& from) {
+// @@protoc_insertion_point(class_specific_copy_from_start:Communication_proto.Communication_parameter)
+  if (&from == this) return;
+  Clear();
+  MergeFrom(from);
+}
+
+bool Communication_parameter::IsInitialized() const {
+  return true;
+}
+
+void Communication_parameter::Swap(Communication_parameter* other) {
+  if (other == this) return;
+  InternalSwap(other);
+}
+void Communication_parameter::InternalSwap(Communication_parameter* other) {
+  using std::swap;
+  connect_string_vector_.InternalSwap(&other->connect_string_vector_);
+  bind_string_.Swap(&other->bind_string_);
+  swap(_has_bits_[0], other->_has_bits_[0]);
+  _internal_metadata_.Swap(&other->_internal_metadata_);
+  swap(_cached_size_, other->_cached_size_);
+}
+
+::google::protobuf::Metadata Communication_parameter::GetMetadata() const {
+  protobuf_communication_2eproto::protobuf_AssignDescriptorsOnce();
+  return ::protobuf_communication_2eproto::file_level_metadata[kIndexInFileMessages];
+}
+
+
+// ===================================================================
+
+void Communication_parameter_all::InitAsDefaultInstance() {
+  ::Communication_proto::_Communication_parameter_all_default_instance_._instance.get_mutable()->communication_parameters_ = const_cast< ::Communication_proto::Communication_parameter*>(
+      ::Communication_proto::Communication_parameter::internal_default_instance());
+}
+#if !defined(_MSC_VER) || _MSC_VER >= 1900
+const int Communication_parameter_all::kCommunicationParametersFieldNumber;
+#endif  // !defined(_MSC_VER) || _MSC_VER >= 1900
+
+Communication_parameter_all::Communication_parameter_all()
+  : ::google::protobuf::Message(), _internal_metadata_(NULL) {
+  if (GOOGLE_PREDICT_TRUE(this != internal_default_instance())) {
+    ::protobuf_communication_2eproto::InitDefaultsCommunication_parameter_all();
+  }
+  SharedCtor();
+  // @@protoc_insertion_point(constructor:Communication_proto.Communication_parameter_all)
+}
+Communication_parameter_all::Communication_parameter_all(const Communication_parameter_all& from)
+  : ::google::protobuf::Message(),
+      _internal_metadata_(NULL),
+      _has_bits_(from._has_bits_),
+      _cached_size_(0) {
+  _internal_metadata_.MergeFrom(from._internal_metadata_);
+  if (from.has_communication_parameters()) {
+    communication_parameters_ = new ::Communication_proto::Communication_parameter(*from.communication_parameters_);
+  } else {
+    communication_parameters_ = NULL;
+  }
+  // @@protoc_insertion_point(copy_constructor:Communication_proto.Communication_parameter_all)
+}
+
+void Communication_parameter_all::SharedCtor() {
+  _cached_size_ = 0;
+  communication_parameters_ = NULL;
+}
+
+Communication_parameter_all::~Communication_parameter_all() {
+  // @@protoc_insertion_point(destructor:Communication_proto.Communication_parameter_all)
+  SharedDtor();
+}
+
+void Communication_parameter_all::SharedDtor() {
+  if (this != internal_default_instance()) delete communication_parameters_;
+}
+
+void Communication_parameter_all::SetCachedSize(int size) const {
+  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
+  _cached_size_ = size;
+  GOOGLE_SAFE_CONCURRENT_WRITES_END();
+}
+const ::google::protobuf::Descriptor* Communication_parameter_all::descriptor() {
+  ::protobuf_communication_2eproto::protobuf_AssignDescriptorsOnce();
+  return ::protobuf_communication_2eproto::file_level_metadata[kIndexInFileMessages].descriptor;
+}
+
+const Communication_parameter_all& Communication_parameter_all::default_instance() {
+  ::protobuf_communication_2eproto::InitDefaultsCommunication_parameter_all();
+  return *internal_default_instance();
+}
+
+Communication_parameter_all* Communication_parameter_all::New(::google::protobuf::Arena* arena) const {
+  Communication_parameter_all* n = new Communication_parameter_all;
+  if (arena != NULL) {
+    arena->Own(n);
+  }
+  return n;
+}
+
+void Communication_parameter_all::Clear() {
+// @@protoc_insertion_point(message_clear_start:Communication_proto.Communication_parameter_all)
+  ::google::protobuf::uint32 cached_has_bits = 0;
+  // Prevent compiler warnings about cached_has_bits being unused
+  (void) cached_has_bits;
+
+  cached_has_bits = _has_bits_[0];
+  if (cached_has_bits & 0x00000001u) {
+    GOOGLE_DCHECK(communication_parameters_ != NULL);
+    communication_parameters_->Clear();
+  }
+  _has_bits_.Clear();
+  _internal_metadata_.Clear();
+}
+
+bool Communication_parameter_all::MergePartialFromCodedStream(
+    ::google::protobuf::io::CodedInputStream* input) {
+#define DO_(EXPRESSION) if (!GOOGLE_PREDICT_TRUE(EXPRESSION)) goto failure
+  ::google::protobuf::uint32 tag;
+  // @@protoc_insertion_point(parse_start:Communication_proto.Communication_parameter_all)
+  for (;;) {
+    ::std::pair< ::google::protobuf::uint32, bool> p = input->ReadTagWithCutoffNoLastTag(127u);
+    tag = p.first;
+    if (!p.second) goto handle_unusual;
+    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
+      // optional .Communication_proto.Communication_parameter communication_parameters = 1;
+      case 1: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(10u /* 10 & 0xFF */)) {
+          DO_(::google::protobuf::internal::WireFormatLite::ReadMessage(
+               input, mutable_communication_parameters()));
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
+      default: {
+      handle_unusual:
+        if (tag == 0) {
+          goto success;
+        }
+        DO_(::google::protobuf::internal::WireFormat::SkipField(
+              input, tag, _internal_metadata_.mutable_unknown_fields()));
+        break;
+      }
+    }
+  }
+success:
+  // @@protoc_insertion_point(parse_success:Communication_proto.Communication_parameter_all)
+  return true;
+failure:
+  // @@protoc_insertion_point(parse_failure:Communication_proto.Communication_parameter_all)
+  return false;
+#undef DO_
+}
+
+void Communication_parameter_all::SerializeWithCachedSizes(
+    ::google::protobuf::io::CodedOutputStream* output) const {
+  // @@protoc_insertion_point(serialize_start:Communication_proto.Communication_parameter_all)
+  ::google::protobuf::uint32 cached_has_bits = 0;
+  (void) cached_has_bits;
+
+  cached_has_bits = _has_bits_[0];
+  // optional .Communication_proto.Communication_parameter communication_parameters = 1;
+  if (cached_has_bits & 0x00000001u) {
+    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
+      1, *this->communication_parameters_, output);
+  }
+
+  if (_internal_metadata_.have_unknown_fields()) {
+    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
+        _internal_metadata_.unknown_fields(), output);
+  }
+  // @@protoc_insertion_point(serialize_end:Communication_proto.Communication_parameter_all)
+}
+
+::google::protobuf::uint8* Communication_parameter_all::InternalSerializeWithCachedSizesToArray(
+    bool deterministic, ::google::protobuf::uint8* target) const {
+  (void)deterministic; // Unused
+  // @@protoc_insertion_point(serialize_to_array_start:Communication_proto.Communication_parameter_all)
+  ::google::protobuf::uint32 cached_has_bits = 0;
+  (void) cached_has_bits;
+
+  cached_has_bits = _has_bits_[0];
+  // optional .Communication_proto.Communication_parameter communication_parameters = 1;
+  if (cached_has_bits & 0x00000001u) {
+    target = ::google::protobuf::internal::WireFormatLite::
+      InternalWriteMessageToArray(
+        1, *this->communication_parameters_, deterministic, target);
+  }
+
+  if (_internal_metadata_.have_unknown_fields()) {
+    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
+        _internal_metadata_.unknown_fields(), target);
+  }
+  // @@protoc_insertion_point(serialize_to_array_end:Communication_proto.Communication_parameter_all)
+  return target;
+}
+
+size_t Communication_parameter_all::ByteSizeLong() const {
+// @@protoc_insertion_point(message_byte_size_start:Communication_proto.Communication_parameter_all)
+  size_t total_size = 0;
+
+  if (_internal_metadata_.have_unknown_fields()) {
+    total_size +=
+      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
+        _internal_metadata_.unknown_fields());
+  }
+  // optional .Communication_proto.Communication_parameter communication_parameters = 1;
+  if (has_communication_parameters()) {
+    total_size += 1 +
+      ::google::protobuf::internal::WireFormatLite::MessageSize(
+        *this->communication_parameters_);
+  }
+
+  int cached_size = ::google::protobuf::internal::ToCachedSize(total_size);
+  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
+  _cached_size_ = cached_size;
+  GOOGLE_SAFE_CONCURRENT_WRITES_END();
+  return total_size;
+}
+
+void Communication_parameter_all::MergeFrom(const ::google::protobuf::Message& from) {
+// @@protoc_insertion_point(generalized_merge_from_start:Communication_proto.Communication_parameter_all)
+  GOOGLE_DCHECK_NE(&from, this);
+  const Communication_parameter_all* source =
+      ::google::protobuf::internal::DynamicCastToGenerated<const Communication_parameter_all>(
+          &from);
+  if (source == NULL) {
+  // @@protoc_insertion_point(generalized_merge_from_cast_fail:Communication_proto.Communication_parameter_all)
+    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
+  } else {
+  // @@protoc_insertion_point(generalized_merge_from_cast_success:Communication_proto.Communication_parameter_all)
+    MergeFrom(*source);
+  }
+}
+
+void Communication_parameter_all::MergeFrom(const Communication_parameter_all& from) {
+// @@protoc_insertion_point(class_specific_merge_from_start:Communication_proto.Communication_parameter_all)
+  GOOGLE_DCHECK_NE(&from, this);
+  _internal_metadata_.MergeFrom(from._internal_metadata_);
+  ::google::protobuf::uint32 cached_has_bits = 0;
+  (void) cached_has_bits;
+
+  if (from.has_communication_parameters()) {
+    mutable_communication_parameters()->::Communication_proto::Communication_parameter::MergeFrom(from.communication_parameters());
+  }
+}
+
+void Communication_parameter_all::CopyFrom(const ::google::protobuf::Message& from) {
+// @@protoc_insertion_point(generalized_copy_from_start:Communication_proto.Communication_parameter_all)
+  if (&from == this) return;
+  Clear();
+  MergeFrom(from);
+}
+
+void Communication_parameter_all::CopyFrom(const Communication_parameter_all& from) {
+// @@protoc_insertion_point(class_specific_copy_from_start:Communication_proto.Communication_parameter_all)
+  if (&from == this) return;
+  Clear();
+  MergeFrom(from);
+}
+
+bool Communication_parameter_all::IsInitialized() const {
+  return true;
+}
+
+void Communication_parameter_all::Swap(Communication_parameter_all* other) {
+  if (other == this) return;
+  InternalSwap(other);
+}
+void Communication_parameter_all::InternalSwap(Communication_parameter_all* other) {
+  using std::swap;
+  swap(communication_parameters_, other->communication_parameters_);
+  swap(_has_bits_[0], other->_has_bits_[0]);
+  _internal_metadata_.Swap(&other->_internal_metadata_);
+  swap(_cached_size_, other->_cached_size_);
+}
+
+::google::protobuf::Metadata Communication_parameter_all::GetMetadata() const {
+  protobuf_communication_2eproto::protobuf_AssignDescriptorsOnce();
+  return ::protobuf_communication_2eproto::file_level_metadata[kIndexInFileMessages];
+}
+
+
+// @@protoc_insertion_point(namespace_scope)
+}  // namespace Communication_proto
+
+// @@protoc_insertion_point(global_scope)

src/pkgname/src/communication/communication.pb.h

@@ -0,0 +1,530 @@
+// Generated by the protocol buffer compiler.  DO NOT EDIT!
+// source: communication.proto
+
+#ifndef PROTOBUF_communication_2eproto__INCLUDED
+#define PROTOBUF_communication_2eproto__INCLUDED
+
+#include <string>
+
+#include <google/protobuf/stubs/common.h>
+
+#if GOOGLE_PROTOBUF_VERSION < 3005000
+#error This file was generated by a newer version of protoc which is
+#error incompatible with your Protocol Buffer headers.  Please update
+#error your headers.
+#endif
+#if 3005000 < GOOGLE_PROTOBUF_MIN_PROTOC_VERSION
+#error This file was generated by an older version of protoc which is
+#error incompatible with your Protocol Buffer headers.  Please
+#error regenerate this file with a newer version of protoc.
+#endif
+
+#include <google/protobuf/io/coded_stream.h>
+#include <google/protobuf/arena.h>
+#include <google/protobuf/arenastring.h>
+#include <google/protobuf/generated_message_table_driven.h>
+#include <google/protobuf/generated_message_util.h>
+#include <google/protobuf/metadata.h>
+#include <google/protobuf/message.h>
+#include <google/protobuf/repeated_field.h>  // IWYU pragma: export
+#include <google/protobuf/extension_set.h>  // IWYU pragma: export
+#include <google/protobuf/unknown_field_set.h>
+// @@protoc_insertion_point(includes)
+
+namespace protobuf_communication_2eproto {
+// Internal implementation detail -- do not use these members.
+struct TableStruct {
+  static const ::google::protobuf::internal::ParseTableField entries[];
+  static const ::google::protobuf::internal::AuxillaryParseTableField aux[];
+  static const ::google::protobuf::internal::ParseTable schema[2];
+  static const ::google::protobuf::internal::FieldMetadata field_metadata[];
+  static const ::google::protobuf::internal::SerializationTable serialization_table[];
+  static const ::google::protobuf::uint32 offsets[];
+};
+void AddDescriptors();
+void InitDefaultsCommunication_parameterImpl();
+void InitDefaultsCommunication_parameter();
+void InitDefaultsCommunication_parameter_allImpl();
+void InitDefaultsCommunication_parameter_all();
+inline void InitDefaults() {
+  InitDefaultsCommunication_parameter();
+  InitDefaultsCommunication_parameter_all();
+}
+}  // namespace protobuf_communication_2eproto
+namespace Communication_proto {
+class Communication_parameter;
+class Communication_parameterDefaultTypeInternal;
+extern Communication_parameterDefaultTypeInternal _Communication_parameter_default_instance_;
+class Communication_parameter_all;
+class Communication_parameter_allDefaultTypeInternal;
+extern Communication_parameter_allDefaultTypeInternal _Communication_parameter_all_default_instance_;
+}  // namespace Communication_proto
+namespace Communication_proto {
+
+// ===================================================================
+
+class Communication_parameter : public ::google::protobuf::Message /* @@protoc_insertion_point(class_definition:Communication_proto.Communication_parameter) */ {
+ public:
+  Communication_parameter();
+  virtual ~Communication_parameter();
+
+  Communication_parameter(const Communication_parameter& from);
+
+  inline Communication_parameter& operator=(const Communication_parameter& from) {
+    CopyFrom(from);
+    return *this;
+  }
+  #if LANG_CXX11
+  Communication_parameter(Communication_parameter&& from) noexcept
+    : Communication_parameter() {
+    *this = ::std::move(from);
+  }
+
+  inline Communication_parameter& operator=(Communication_parameter&& from) noexcept {
+    if (GetArenaNoVirtual() == from.GetArenaNoVirtual()) {
+      if (this != &from) InternalSwap(&from);
+    } else {
+      CopyFrom(from);
+    }
+    return *this;
+  }
+  #endif
+  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
+    return _internal_metadata_.unknown_fields();
+  }
+  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
+    return _internal_metadata_.mutable_unknown_fields();
+  }
+
+  static const ::google::protobuf::Descriptor* descriptor();
+  static const Communication_parameter& default_instance();
+
+  static void InitAsDefaultInstance();  // FOR INTERNAL USE ONLY
+  static inline const Communication_parameter* internal_default_instance() {
+    return reinterpret_cast<const Communication_parameter*>(
+               &_Communication_parameter_default_instance_);
+  }
+  static PROTOBUF_CONSTEXPR int const kIndexInFileMessages =
+    0;
+
+  void Swap(Communication_parameter* other);
+  friend void swap(Communication_parameter& a, Communication_parameter& b) {
+    a.Swap(&b);
+  }
+
+  // implements Message ----------------------------------------------
+
+  inline Communication_parameter* New() const PROTOBUF_FINAL { return New(NULL); }
+
+  Communication_parameter* New(::google::protobuf::Arena* arena) const PROTOBUF_FINAL;
+  void CopyFrom(const ::google::protobuf::Message& from) PROTOBUF_FINAL;
+  void MergeFrom(const ::google::protobuf::Message& from) PROTOBUF_FINAL;
+  void CopyFrom(const Communication_parameter& from);
+  void MergeFrom(const Communication_parameter& from);
+  void Clear() PROTOBUF_FINAL;
+  bool IsInitialized() const PROTOBUF_FINAL;
+
+  size_t ByteSizeLong() const PROTOBUF_FINAL;
+  bool MergePartialFromCodedStream(
+      ::google::protobuf::io::CodedInputStream* input) PROTOBUF_FINAL;
+  void SerializeWithCachedSizes(
+      ::google::protobuf::io::CodedOutputStream* output) const PROTOBUF_FINAL;
+  ::google::protobuf::uint8* InternalSerializeWithCachedSizesToArray(
+      bool deterministic, ::google::protobuf::uint8* target) const PROTOBUF_FINAL;
+  int GetCachedSize() const PROTOBUF_FINAL { return _cached_size_; }
+  private:
+  void SharedCtor();
+  void SharedDtor();
+  void SetCachedSize(int size) const PROTOBUF_FINAL;
+  void InternalSwap(Communication_parameter* other);
+  private:
+  inline ::google::protobuf::Arena* GetArenaNoVirtual() const {
+    return NULL;
+  }
+  inline void* MaybeArenaPtr() const {
+    return NULL;
+  }
+  public:
+
+  ::google::protobuf::Metadata GetMetadata() const PROTOBUF_FINAL;
+
+  // nested types ----------------------------------------------------
+
+  // accessors -------------------------------------------------------
+
+  // repeated string connect_string_vector = 2;
+  int connect_string_vector_size() const;
+  void clear_connect_string_vector();
+  static const int kConnectStringVectorFieldNumber = 2;
+  const ::std::string& connect_string_vector(int index) const;
+  ::std::string* mutable_connect_string_vector(int index);
+  void set_connect_string_vector(int index, const ::std::string& value);
+  #if LANG_CXX11
+  void set_connect_string_vector(int index, ::std::string&& value);
+  #endif
+  void set_connect_string_vector(int index, const char* value);
+  void set_connect_string_vector(int index, const char* value, size_t size);
+  ::std::string* add_connect_string_vector();
+  void add_connect_string_vector(const ::std::string& value);
+  #if LANG_CXX11
+  void add_connect_string_vector(::std::string&& value);
+  #endif
+  void add_connect_string_vector(const char* value);
+  void add_connect_string_vector(const char* value, size_t size);
+  const ::google::protobuf::RepeatedPtrField< ::std::string>& connect_string_vector() const;
+  ::google::protobuf::RepeatedPtrField< ::std::string>* mutable_connect_string_vector();
+
+  // optional string bind_string = 1;
+  bool has_bind_string() const;
+  void clear_bind_string();
+  static const int kBindStringFieldNumber = 1;
+  const ::std::string& bind_string() const;
+  void set_bind_string(const ::std::string& value);
+  #if LANG_CXX11
+  void set_bind_string(::std::string&& value);
+  #endif
+  void set_bind_string(const char* value);
+  void set_bind_string(const char* value, size_t size);
+  ::std::string* mutable_bind_string();
+  ::std::string* release_bind_string();
+  void set_allocated_bind_string(::std::string* bind_string);
+
+  // @@protoc_insertion_point(class_scope:Communication_proto.Communication_parameter)
+ private:
+  void set_has_bind_string();
+  void clear_has_bind_string();
+
+  ::google::protobuf::internal::InternalMetadataWithArena _internal_metadata_;
+  ::google::protobuf::internal::HasBits<1> _has_bits_;
+  mutable int _cached_size_;
+  ::google::protobuf::RepeatedPtrField< ::std::string> connect_string_vector_;
+  ::google::protobuf::internal::ArenaStringPtr bind_string_;
+  friend struct ::protobuf_communication_2eproto::TableStruct;
+  friend void ::protobuf_communication_2eproto::InitDefaultsCommunication_parameterImpl();
+};
+// -------------------------------------------------------------------
+
+class Communication_parameter_all : public ::google::protobuf::Message /* @@protoc_insertion_point(class_definition:Communication_proto.Communication_parameter_all) */ {
+ public:
+  Communication_parameter_all();
+  virtual ~Communication_parameter_all();
+
+  Communication_parameter_all(const Communication_parameter_all& from);
+
+  inline Communication_parameter_all& operator=(const Communication_parameter_all& from) {
+    CopyFrom(from);
+    return *this;
+  }
+  #if LANG_CXX11
+  Communication_parameter_all(Communication_parameter_all&& from) noexcept
+    : Communication_parameter_all() {
+    *this = ::std::move(from);
+  }
+
+  inline Communication_parameter_all& operator=(Communication_parameter_all&& from) noexcept {
+    if (GetArenaNoVirtual() == from.GetArenaNoVirtual()) {
+      if (this != &from) InternalSwap(&from);
+    } else {
+      CopyFrom(from);
+    }
+    return *this;
+  }
+  #endif
+  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
+    return _internal_metadata_.unknown_fields();
+  }
+  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
+    return _internal_metadata_.mutable_unknown_fields();
+  }
+
+  static const ::google::protobuf::Descriptor* descriptor();
+  static const Communication_parameter_all& default_instance();
+
+  static void InitAsDefaultInstance();  // FOR INTERNAL USE ONLY
+  static inline const Communication_parameter_all* internal_default_instance() {
+    return reinterpret_cast<const Communication_parameter_all*>(
+               &_Communication_parameter_all_default_instance_);
+  }
+  static PROTOBUF_CONSTEXPR int const kIndexInFileMessages =
+    1;
+
+  void Swap(Communication_parameter_all* other);
+  friend void swap(Communication_parameter_all& a, Communication_parameter_all& b) {
+    a.Swap(&b);
+  }
+
+  // implements Message ----------------------------------------------
+
+  inline Communication_parameter_all* New() const PROTOBUF_FINAL { return New(NULL); }
+
+  Communication_parameter_all* New(::google::protobuf::Arena* arena) const PROTOBUF_FINAL;
+  void CopyFrom(const ::google::protobuf::Message& from) PROTOBUF_FINAL;
+  void MergeFrom(const ::google::protobuf::Message& from) PROTOBUF_FINAL;
+  void CopyFrom(const Communication_parameter_all& from);
+  void MergeFrom(const Communication_parameter_all& from);
+  void Clear() PROTOBUF_FINAL;
+  bool IsInitialized() const PROTOBUF_FINAL;
+
+  size_t ByteSizeLong() const PROTOBUF_FINAL;
+  bool MergePartialFromCodedStream(
+      ::google::protobuf::io::CodedInputStream* input) PROTOBUF_FINAL;
+  void SerializeWithCachedSizes(
+      ::google::protobuf::io::CodedOutputStream* output) const PROTOBUF_FINAL;
+  ::google::protobuf::uint8* InternalSerializeWithCachedSizesToArray(
+      bool deterministic, ::google::protobuf::uint8* target) const PROTOBUF_FINAL;
+  int GetCachedSize() const PROTOBUF_FINAL { return _cached_size_; }
+  private:
+  void SharedCtor();
+  void SharedDtor();
+  void SetCachedSize(int size) const PROTOBUF_FINAL;
+  void InternalSwap(Communication_parameter_all* other);
+  private:
+  inline ::google::protobuf::Arena* GetArenaNoVirtual() const {
+    return NULL;
+  }
+  inline void* MaybeArenaPtr() const {
+    return NULL;
+  }
+  public:
+
+  ::google::protobuf::Metadata GetMetadata() const PROTOBUF_FINAL;
+
+  // nested types ----------------------------------------------------
+
+  // accessors -------------------------------------------------------
+
+  // optional .Communication_proto.Communication_parameter communication_parameters = 1;
+  bool has_communication_parameters() const;
+  void clear_communication_parameters();
+  static const int kCommunicationParametersFieldNumber = 1;
+  const ::Communication_proto::Communication_parameter& communication_parameters() const;
+  ::Communication_proto::Communication_parameter* release_communication_parameters();
+  ::Communication_proto::Communication_parameter* mutable_communication_parameters();
+  void set_allocated_communication_parameters(::Communication_proto::Communication_parameter* communication_parameters);
+
+  // @@protoc_insertion_point(class_scope:Communication_proto.Communication_parameter_all)
+ private:
+  void set_has_communication_parameters();
+  void clear_has_communication_parameters();
+
+  ::google::protobuf::internal::InternalMetadataWithArena _internal_metadata_;
+  ::google::protobuf::internal::HasBits<1> _has_bits_;
+  mutable int _cached_size_;
+  ::Communication_proto::Communication_parameter* communication_parameters_;
+  friend struct ::protobuf_communication_2eproto::TableStruct;
+  friend void ::protobuf_communication_2eproto::InitDefaultsCommunication_parameter_allImpl();
+};
+// ===================================================================
+
+
+// ===================================================================
+
+#ifdef __GNUC__
+  #pragma GCC diagnostic push
+  #pragma GCC diagnostic ignored "-Wstrict-aliasing"
+#endif  // __GNUC__
+// Communication_parameter
+
+// optional string bind_string = 1;
+inline bool Communication_parameter::has_bind_string() const {
+  return (_has_bits_[0] & 0x00000001u) != 0;
+}
+inline void Communication_parameter::set_has_bind_string() {
+  _has_bits_[0] |= 0x00000001u;
+}
+inline void Communication_parameter::clear_has_bind_string() {
+  _has_bits_[0] &= ~0x00000001u;
+}
+inline void Communication_parameter::clear_bind_string() {
+  bind_string_.ClearToEmptyNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
+  clear_has_bind_string();
+}
+inline const ::std::string& Communication_parameter::bind_string() const {
+  // @@protoc_insertion_point(field_get:Communication_proto.Communication_parameter.bind_string)
+  return bind_string_.GetNoArena();
+}
+inline void Communication_parameter::set_bind_string(const ::std::string& value) {
+  set_has_bind_string();
+  bind_string_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), value);
+  // @@protoc_insertion_point(field_set:Communication_proto.Communication_parameter.bind_string)
+}
+#if LANG_CXX11
+inline void Communication_parameter::set_bind_string(::std::string&& value) {
+  set_has_bind_string();
+  bind_string_.SetNoArena(
+    &::google::protobuf::internal::GetEmptyStringAlreadyInited(), ::std::move(value));
+  // @@protoc_insertion_point(field_set_rvalue:Communication_proto.Communication_parameter.bind_string)
+}
+#endif
+inline void Communication_parameter::set_bind_string(const char* value) {
+  GOOGLE_DCHECK(value != NULL);
+  set_has_bind_string();
+  bind_string_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), ::std::string(value));
+  // @@protoc_insertion_point(field_set_char:Communication_proto.Communication_parameter.bind_string)
+}
+inline void Communication_parameter::set_bind_string(const char* value, size_t size) {
+  set_has_bind_string();
+  bind_string_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(),
+      ::std::string(reinterpret_cast<const char*>(value), size));
+  // @@protoc_insertion_point(field_set_pointer:Communication_proto.Communication_parameter.bind_string)
+}
+inline ::std::string* Communication_parameter::mutable_bind_string() {
+  set_has_bind_string();
+  // @@protoc_insertion_point(field_mutable:Communication_proto.Communication_parameter.bind_string)
+  return bind_string_.MutableNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
+}
+inline ::std::string* Communication_parameter::release_bind_string() {
+  // @@protoc_insertion_point(field_release:Communication_proto.Communication_parameter.bind_string)
+  clear_has_bind_string();
+  return bind_string_.ReleaseNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
+}
+inline void Communication_parameter::set_allocated_bind_string(::std::string* bind_string) {
+  if (bind_string != NULL) {
+    set_has_bind_string();
+  } else {
+    clear_has_bind_string();
+  }
+  bind_string_.SetAllocatedNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), bind_string);
+  // @@protoc_insertion_point(field_set_allocated:Communication_proto.Communication_parameter.bind_string)
+}
+
+// repeated string connect_string_vector = 2;
+inline int Communication_parameter::connect_string_vector_size() const {
+  return connect_string_vector_.size();
+}
+inline void Communication_parameter::clear_connect_string_vector() {
+  connect_string_vector_.Clear();
+}
+inline const ::std::string& Communication_parameter::connect_string_vector(int index) const {
+  // @@protoc_insertion_point(field_get:Communication_proto.Communication_parameter.connect_string_vector)
+  return connect_string_vector_.Get(index);
+}
+inline ::std::string* Communication_parameter::mutable_connect_string_vector(int index) {
+  // @@protoc_insertion_point(field_mutable:Communication_proto.Communication_parameter.connect_string_vector)
+  return connect_string_vector_.Mutable(index);
+}
+inline void Communication_parameter::set_connect_string_vector(int index, const ::std::string& value) {
+  // @@protoc_insertion_point(field_set:Communication_proto.Communication_parameter.connect_string_vector)
+  connect_string_vector_.Mutable(index)->assign(value);
+}
+#if LANG_CXX11
+inline void Communication_parameter::set_connect_string_vector(int index, ::std::string&& value) {
+  // @@protoc_insertion_point(field_set:Communication_proto.Communication_parameter.connect_string_vector)
+  connect_string_vector_.Mutable(index)->assign(std::move(value));
+}
+#endif
+inline void Communication_parameter::set_connect_string_vector(int index, const char* value) {
+  GOOGLE_DCHECK(value != NULL);
+  connect_string_vector_.Mutable(index)->assign(value);
+  // @@protoc_insertion_point(field_set_char:Communication_proto.Communication_parameter.connect_string_vector)
+}
+inline void Communication_parameter::set_connect_string_vector(int index, const char* value, size_t size) {
+  connect_string_vector_.Mutable(index)->assign(
+    reinterpret_cast<const char*>(value), size);
+  // @@protoc_insertion_point(field_set_pointer:Communication_proto.Communication_parameter.connect_string_vector)
+}
+inline ::std::string* Communication_parameter::add_connect_string_vector() {
+  // @@protoc_insertion_point(field_add_mutable:Communication_proto.Communication_parameter.connect_string_vector)
+  return connect_string_vector_.Add();
+}
+inline void Communication_parameter::add_connect_string_vector(const ::std::string& value) {
+  connect_string_vector_.Add()->assign(value);
+  // @@protoc_insertion_point(field_add:Communication_proto.Communication_parameter.connect_string_vector)
+}
+#if LANG_CXX11
+inline void Communication_parameter::add_connect_string_vector(::std::string&& value) {
+  connect_string_vector_.Add(std::move(value));
+  // @@protoc_insertion_point(field_add:Communication_proto.Communication_parameter.connect_string_vector)
+}
+#endif
+inline void Communication_parameter::add_connect_string_vector(const char* value) {
+  GOOGLE_DCHECK(value != NULL);
+  connect_string_vector_.Add()->assign(value);
+  // @@protoc_insertion_point(field_add_char:Communication_proto.Communication_parameter.connect_string_vector)
+}
+inline void Communication_parameter::add_connect_string_vector(const char* value, size_t size) {
+  connect_string_vector_.Add()->assign(reinterpret_cast<const char*>(value), size);
+  // @@protoc_insertion_point(field_add_pointer:Communication_proto.Communication_parameter.connect_string_vector)
+}
+inline const ::google::protobuf::RepeatedPtrField< ::std::string>&
+Communication_parameter::connect_string_vector() const {
+  // @@protoc_insertion_point(field_list:Communication_proto.Communication_parameter.connect_string_vector)
+  return connect_string_vector_;
+}
+inline ::google::protobuf::RepeatedPtrField< ::std::string>*
+Communication_parameter::mutable_connect_string_vector() {
+  // @@protoc_insertion_point(field_mutable_list:Communication_proto.Communication_parameter.connect_string_vector)
+  return &connect_string_vector_;
+}
+
+// -------------------------------------------------------------------
+
+// Communication_parameter_all
+
+// optional .Communication_proto.Communication_parameter communication_parameters = 1;
+inline bool Communication_parameter_all::has_communication_parameters() const {
+  return (_has_bits_[0] & 0x00000001u) != 0;
+}
+inline void Communication_parameter_all::set_has_communication_parameters() {
+  _has_bits_[0] |= 0x00000001u;
+}
+inline void Communication_parameter_all::clear_has_communication_parameters() {
+  _has_bits_[0] &= ~0x00000001u;
+}
+inline void Communication_parameter_all::clear_communication_parameters() {
+  if (communication_parameters_ != NULL) communication_parameters_->Clear();
+  clear_has_communication_parameters();
+}
+inline const ::Communication_proto::Communication_parameter& Communication_parameter_all::communication_parameters() const {
+  const ::Communication_proto::Communication_parameter* p = communication_parameters_;
+  // @@protoc_insertion_point(field_get:Communication_proto.Communication_parameter_all.communication_parameters)
+  return p != NULL ? *p : *reinterpret_cast<const ::Communication_proto::Communication_parameter*>(
+      &::Communication_proto::_Communication_parameter_default_instance_);
+}
+inline ::Communication_proto::Communication_parameter* Communication_parameter_all::release_communication_parameters() {
+  // @@protoc_insertion_point(field_release:Communication_proto.Communication_parameter_all.communication_parameters)
+  clear_has_communication_parameters();
+  ::Communication_proto::Communication_parameter* temp = communication_parameters_;
+  communication_parameters_ = NULL;
+  return temp;
+}
+inline ::Communication_proto::Communication_parameter* Communication_parameter_all::mutable_communication_parameters() {
+  set_has_communication_parameters();
+  if (communication_parameters_ == NULL) {
+    communication_parameters_ = new ::Communication_proto::Communication_parameter;
+  }
+  // @@protoc_insertion_point(field_mutable:Communication_proto.Communication_parameter_all.communication_parameters)
+  return communication_parameters_;
+}
+inline void Communication_parameter_all::set_allocated_communication_parameters(::Communication_proto::Communication_parameter* communication_parameters) {
+  ::google::protobuf::Arena* message_arena = GetArenaNoVirtual();
+  if (message_arena == NULL) {
+    delete communication_parameters_;
+  }
+  if (communication_parameters) {
+    ::google::protobuf::Arena* submessage_arena = NULL;
+    if (message_arena != submessage_arena) {
+      communication_parameters = ::google::protobuf::internal::GetOwnedMessage(
+          message_arena, communication_parameters, submessage_arena);
+    }
+    set_has_communication_parameters();
+  } else {
+    clear_has_communication_parameters();
+  }
+  communication_parameters_ = communication_parameters;
+  // @@protoc_insertion_point(field_set_allocated:Communication_proto.Communication_parameter_all.communication_parameters)
+}
+
+#ifdef __GNUC__
+  #pragma GCC diagnostic pop
+#endif  // __GNUC__
+// -------------------------------------------------------------------
+
+
+// @@protoc_insertion_point(namespace_scope)
+
+}  // namespace Communication_proto
+
+// @@protoc_insertion_point(global_scope)
+
+#endif  // PROTOBUF_communication_2eproto__INCLUDED

src/pkgname/src/communication/communication.proto

@@ -0,0 +1,14 @@
+syntax = "proto2";
+package Communication_proto;
+
+message Communication_parameter
+{
+    optional string bind_string = 1;
+    repeated string connect_string_vector = 2;
+
+}
+
+message Communication_parameter_all
+{
+    optional Communication_parameter        communication_parameters=1;
+}

src/pkgname/src/communication/communication_message.cpp

@@ -0,0 +1,89 @@
+//
+// Created by huli on 2020/6/29.
+//
+
+#include "communication_message.h"
+
+
+Communication_message::Communication_message()
+{
+	m_message_type = eBase_msg;
+	m_receive_time = std::chrono::system_clock::now();
+	m_timeout_ms = std::chrono::milliseconds(5000);		//超时默认5秒
+	m_sender = eEmpty;
+	m_receiver = eEmpty;
+//	m_message_buf = "";
+}
+
+Communication_message::Communication_message(std::string message_buf)
+{
+	m_message_type = eBase_msg;
+	m_receive_time = std::chrono::system_clock::now();
+	m_timeout_ms = std::chrono::milliseconds(5000);		//超时默认5秒
+	m_sender = eEmpty;
+	m_receiver = eEmpty;
+	m_message_buf = message_buf;
+}
+
+Communication_message::Communication_message(char* p_buf, int size)
+{
+	m_message_type = eBase_msg;
+	m_receive_time = std::chrono::system_clock::now();
+	m_timeout_ms = std::chrono::milliseconds(5000);		//超时默认5秒
+	m_sender = eEmpty;
+	m_receiver = eEmpty;
+	m_message_buf = std::string(p_buf, size);
+}
+
+
+
+Communication_message::~Communication_message()
+{
+
+}
+
+bool Communication_message::is_over_time()
+{
+	if ( std::chrono::system_clock::now() - m_receive_time > m_timeout_ms)
+	{
+	    return true;
+	}
+	else
+	{
+		return false;
+	}
+}
+
+
+void Communication_message::reset(const message::Base_info& base_msg, std::string receive_string)
+{
+	m_message_type = (Message_type)(base_msg.msg_type());
+
+	m_receive_time = std::chrono::system_clock::now();
+	m_timeout_ms = std::chrono::milliseconds(base_msg.timeout_ms());
+	m_sender = (Communicator)(base_msg.sender());
+	m_receiver = (Communicator)(base_msg.receiver());
+	m_message_buf = receive_string;
+}
+
+Communication_message::Message_type Communication_message::get_message_type()
+{
+	return m_message_type;
+}
+Communication_message::Communicator Communication_message::get_sender()
+{
+	return m_sender;
+}
+Communication_message::Communicator Communication_message::get_receiver()
+{
+	return m_receiver;
+}
+std::string& Communication_message::get_message_buf()
+{
+	return m_message_buf;
+}
+
+
+
+
+

src/pkgname/src/communication/communication_message.h

@@ -0,0 +1,112 @@
+//
+// Created by huli on 2020/6/29.
+//
+
+#ifndef NNXX_TESTS_COMMUNICATION_MESSAGE_H
+#define NNXX_TESTS_COMMUNICATION_MESSAGE_H
+
+#include "../error_code/error_code.h"
+
+#include <time.h>
+#include <sys/time.h>
+#include <chrono>
+//#include <iosfwd>
+
+#include <string>
+#include "../message/message_base.pb.h"
+
+
+class Communication_message
+{
+public:
+	//消息类型定义,每个在网络上传输的消息必须含有这个属性
+	enum Message_type
+	{
+		eBase_msg=0x00,
+		eCommand_msg=0x01,                      //指令消息
+
+		eLocate_status_msg=0x11,                //定位模块状态消息
+		eLocate_request_msg=0x12,               //定位请求消息
+		eLocate_response_msg=0x13,              //定位反馈消息
+
+        eDispatch_status_msg=0x21,                //调度模块硬件状态消息
+        eDispatch_request_msg=0x22,              //请求调度消息
+        eDispatch_response_msg=0x23,             //调度结果反馈消息
+
+        eParkspace_allocation_status_msg = 0x31,   //车位分配模块状态消息，包括车位信息
+        eParkspace_allocation_request_msg = 0x32,  //请求分配车位消息
+        eParkspace_allocation_response_msg = 0x33, //分配车位结果反馈消息
+        eParkspace_search_request_msg = 0x34,    //查询车位请求消息
+        eParkspace_search_response_msg = 0x35,    //查询车位反馈消息
+        eParkspace_release_request_msg = 0x36,    //释放车位请求消息
+        eParkspace_release_response_msg = 0x37,    //释放车位反馈消息
+        eParkspace_force_update_request_msg = 0x38,	//手动修改车位消息
+        eParkspace_force_update_response_msg = 0x39,//手动修改车位反馈消息
+        eParkspace_confirm_alloc_request_msg = 0x3A,//确认分配车位请求消息
+        eParkspace_confirm_alloc_response_msg = 0x3B,//确认分配车位反馈消息
+
+        eStore_command_request_msg=0x41,                    //终端停车请求消息
+        eStore_command_response_msg=0x42,                   //停车请求反馈消息
+        ePickup_command_request_msg=0x43,                   //取车请求消息
+        ePickup_command_response_msg=0x44,                  //取车请求反馈消息
+
+        eStoring_process_statu_msg=0x90,                    //停车进度条消息
+        ePicking_process_statu_msg=0x91,                    //取车进度消息
+
+        eCentral_controller_statu_msg=0xa0,                 //中控状态消息
+
+        eEntrance_manual_operation_msg=0xb0,            //针对出入口状态操作的手动消息
+        eProcess_manual_operation_msg=0xb1,
+
+
+
+        eGoal_msg=0xf0,
+        ePointCloud_msg=0xf1,
+        ePose2d_msg=0xf2,
+    };
+
+//通讯单元
+	enum Communicator
+	{
+		eEmpty=0x0000,		//空
+		eMain=0x0001,    	//主流程
+		eTerminor=0x0100,	//终端
+		eTable=0x0200,		//数据表
+		eMeasurer=0x0300,	//测量单元
+		eDispatch=0x0400,	//调度机构
+		//...
+
+        eAGV=0x0f00,
+	};
+public:
+	Communication_message();
+	Communication_message(std::string message_buf);
+	Communication_message(char* p_buf, int size);
+	Communication_message(const Communication_message& other)= default;
+	Communication_message& operator =(const Communication_message& other)= default;
+	~Communication_message();
+public://API functions
+	bool is_over_time();
+public://get or set member variable
+	void reset(const message::Base_info& base_info, std::string receive_string);
+
+	Message_type get_message_type();
+	Communicator get_sender();
+	Communicator get_receiver();
+	std::string& get_message_buf();
+
+protected://member variable
+	Message_type								m_message_type;				//消息类型
+	std::chrono::system_clock::time_point		m_receive_time;				//接收消息的时间点
+	std::chrono::milliseconds					m_timeout_ms;				//超时时间, 整个软件都统一为毫秒
+	Communicator								m_sender;					//发送者
+	Communicator								m_receiver;					//接受者
+
+	std::string									m_message_buf;				//消息数据
+
+private:
+
+};
+
+
+#endif //NNXX_TESTS_COMMUNICATION_MESSAGE_H

src/pkgname/src/communication/communication_socket_base.cpp

@@ -0,0 +1,591 @@
+
+
+
+#include "communication_socket_base.h"
+#include "../tool/proto_tool.h"
+
+Communication_socket_base::Communication_socket_base()
+{
+	m_communication_statu = COMMUNICATION_UNKNOW;
+
+	mp_receive_data_thread = NULL;
+	mp_analysis_data_thread = NULL;
+	mp_send_data_thread = NULL;
+	mp_encapsulate_data_thread = NULL;
+}
+
+Communication_socket_base::~Communication_socket_base()
+{
+	communication_uninit();
+}
+
+//初始化 通信 模块。如下三选一
+Error_manager Communication_socket_base::communication_init()
+{
+	LOG(INFO) << " ---Communication_socket_base::communication_init() run--- "<< this;
+
+	return  communication_init_from_protobuf(COMMUNICATION_PARAMETER_PATH);
+}
+
+//初始化 通信 模块。从文件读取
+Error_manager Communication_socket_base::communication_init_from_protobuf(std::string prototxt_path)
+{
+	Communication_proto::Communication_parameter_all t_communication_parameter_all;
+	if(!  proto_tool::read_proto_param(prototxt_path,t_communication_parameter_all) )
+	{
+		return Error_manager(COMMUNICATION_READ_PROTOBUF_ERROR,MINOR_ERROR,
+		"Communication_socket_base read_proto_param  failed");
+	}
+
+	return communication_init_from_protobuf(t_communication_parameter_all);
+}
+
+//初始化 通信 模块。从protobuf读取
+Error_manager Communication_socket_base::communication_init_from_protobuf(Communication_proto::Communication_parameter_all& communication_parameter_all)
+{
+	LOG(INFO) << " ---Communication_socket_base::communication_init_from_protobuf() run--- "<< this;
+	Error_manager t_error;
+
+	if ( communication_parameter_all.communication_parameters().has_bind_string() )
+	{
+		t_error = communication_bind(communication_parameter_all.communication_parameters().bind_string());
+		if ( t_error != Error_code::SUCCESS )
+		{
+			return t_error;
+		}
+	}
+	std::cout << "communication_parameter_all.communication_parameters().connect_string_vector_size() " <<
+		communication_parameter_all.communication_parameters().connect_string_vector_size()<< std::endl;
+	for(int i=0;i<communication_parameter_all.communication_parameters().connect_string_vector_size();++i)
+	{
+		t_error = communication_connect( communication_parameter_all.communication_parameters().connect_string_vector(i) );
+		if ( t_error != Error_code::SUCCESS )
+		{
+			return t_error;
+		}
+	}
+
+	//启动通信, run thread
+	communication_run();
+
+	return Error_code::SUCCESS;
+}
+
+//初始化
+Error_manager Communication_socket_base::communication_init(std::string bind_string, std::vector<std::string>& connect_string_vector)
+{
+	LOG(INFO) << " ---Communication_socket_base::communication_init() run--- "<< this;
+	Error_manager t_error;
+
+	t_error = communication_bind(bind_string);
+	if ( t_error != Error_code::SUCCESS )
+	{
+		return t_error;
+	}
+
+	t_error = communication_connect(connect_string_vector);
+	if ( t_error != Error_code::SUCCESS )
+	{
+		return t_error;
+	}
+
+	//启动通信, run thread
+	communication_run();
+
+	return Error_code::SUCCESS;
+}
+//bind
+Error_manager Communication_socket_base::communication_bind(std::string bind_string)
+{
+	Error_manager t_error;
+	int t_socket_result;
+
+	//m_socket 自己作为一个服务器, 绑定一个端口
+	t_socket_result = m_socket.bind(bind_string);
+	if ( t_socket_result <0 )
+	{
+		return Error_manager(Error_code::COMMUNICATION_BIND_ERROR, Error_level::MINOR_ERROR,
+							 " m_socket.bind error ");
+	}
+	//LOG(INFO) << " ---Communication_socket_base::communication_bind() bind::  "<< bind_string << "  " << this;
+
+	return Error_code::SUCCESS;
+}
+//connect
+Error_manager Communication_socket_base::communication_connect(std::vector<std::string>& connect_string_vector)
+{
+	Error_manager t_error;
+	for (auto iter = connect_string_vector.begin(); iter != connect_string_vector.end(); ++iter)
+	{
+		t_error = communication_connect(*iter);
+		if ( t_error != Error_code::SUCCESS )
+		{
+			return t_error;
+		}
+	}
+	return Error_code::SUCCESS;
+}
+//connect
+Error_manager Communication_socket_base::communication_connect(std::string connect_string)
+{
+	Error_manager t_error;
+	int t_socket_result;
+	//m_socket 和远端通信, 连接远端服务器的端口
+	t_socket_result = m_socket.connect(connect_string);
+	if ( t_socket_result <0 )
+	{
+		return Error_manager(Error_code::COMMUNICATION_CONNECT_ERROR, Error_level::MINOR_ERROR,
+							 " m_socket.connect error ");
+	}
+	return Error_code::SUCCESS;
+}
+//启动通信, run thread
+Error_manager Communication_socket_base::communication_run()
+{
+	m_communication_statu = COMMUNICATION_READY;
+	//启动4个线程。
+	//接受线程默认循环, 内部的nn_recv进行等待, 超时1ms
+	m_receive_condition.reset(false, false, false);
+	mp_receive_data_thread = new std::thread(&Communication_socket_base::receive_data_thread, this);
+	//解析线程默认等待, 需要接受线程去唤醒, 超时1ms, 超时后主动遍历m_receive_data_list
+	m_analysis_data_condition.reset(false, false, false);
+	mp_analysis_data_thread = new std::thread(&Communication_socket_base::analysis_data_thread, this);
+	//发送线程默认循环, 内部的wait_and_pop进行等待,
+	m_send_data_condition.reset(false, true, false);
+	mp_send_data_thread = new std::thread(&Communication_socket_base::send_data_thread, this);
+	//封装线程默认等待, ...., 超时1ms, 超时后主动 封装心跳和状态信息,
+	m_encapsulate_data_condition.reset(false, false, false);
+	mp_encapsulate_data_thread = new std::thread(&Communication_socket_base::encapsulate_data_thread, this);
+
+	return Error_code::SUCCESS;
+}
+
+
+//反初始化 通信 模块。
+Error_manager Communication_socket_base::communication_uninit()
+{
+	//终止list，防止 wait_and_pop 阻塞线程。
+	m_receive_data_list.termination_list();
+	m_send_data_list.termination_list();
+
+	//杀死4个线程，强制退出
+	if (mp_receive_data_thread)
+	{
+		m_receive_condition.kill_all();
+	}
+	if (mp_analysis_data_thread)
+	{
+		m_analysis_data_condition.kill_all();
+	}
+	if (mp_send_data_thread)
+	{
+		m_send_data_condition.kill_all();
+	}
+	if (mp_encapsulate_data_thread)
+	{
+		m_encapsulate_data_condition.kill_all();
+	}
+	//回收4个线程的资源
+	if (mp_receive_data_thread)
+	{
+		mp_receive_data_thread->join();
+		delete mp_receive_data_thread;
+		mp_receive_data_thread = NULL;
+	}
+	if (mp_analysis_data_thread)
+	{
+		mp_analysis_data_thread->join();
+		delete mp_analysis_data_thread;
+		mp_analysis_data_thread = 0;
+	}
+	if (mp_send_data_thread)
+	{
+		mp_send_data_thread->join();
+		delete mp_send_data_thread;
+		mp_send_data_thread = NULL;
+	}
+	if (mp_encapsulate_data_thread)
+	{
+		mp_encapsulate_data_thread->join();
+		delete mp_encapsulate_data_thread;
+		mp_encapsulate_data_thread = NULL;
+	}
+
+	//清空list
+	m_receive_data_list.clear_and_delete();
+	m_send_data_list.clear_and_delete();
+
+	m_communication_statu = COMMUNICATION_UNKNOW;
+	m_socket.close();
+
+	return Error_code::SUCCESS;
+}
+
+
+
+
+
+
+
+//mp_receive_data_thread 接受线程执行函数，
+//receive_data_thread 内部线程负责接受消息
+void Communication_socket_base::receive_data_thread(Communication_socket_base* communicator)
+{
+	//LOG(INFO) << " Communication_socket_base::receive_data_thread start "<< communicator;
+
+	//通信接受线程, 负责接受socket消息, 并存入 m_receive_data_list
+	while (communicator->m_receive_condition.is_alive())
+	{
+        std::this_thread::yield();
+        //usleep(1);
+        communicator->m_receive_condition.wait_for_ex(std::chrono::microseconds(1));
+		if ( communicator->m_receive_condition.is_alive() )
+		{
+
+			std::unique_lock<std::mutex> lk(communicator->m_mutex);
+			//flags为1, 非阻塞接受消息, 如果接收到消息, 那么接受数据长度大于0
+			std::string t_receive_string = communicator->m_socket.recv<std::string>(1);
+			if ( t_receive_string.length()>0 )
+			{
+				//如果这里接受到了消息, 在这提前解析消息最前面的Base_msg (消息公共内容), 用于后续的check
+				message::Base_msg t_base_msg;
+				if( t_base_msg.ParseFromString(t_receive_string) )
+				{
+					//第一次解析之后转化为, Communication_message, 自定义的通信消息格式
+					Communication_message  * tp_communication_message = new Communication_message;
+					tp_communication_message->reset(t_base_msg.base_info(), t_receive_string);
+					//检查消息是否有效, 主要检查消息类型和接受者, 判断这条消息是不是给我的.
+					if ( communicator->check_msg(tp_communication_message) == SUCCESS )
+					{
+						bool is_push = communicator->m_receive_data_list.push(tp_communication_message);
+						//push成功之后, tp_communication_message内存的管理权限交给链表, 如果失败就要回收内存
+						if ( is_push )
+						{
+							//唤醒解析线程一次,
+                            communicator->m_analysis_data_condition.notify_all(false, true);
+						}
+						else
+						{
+//						push失败, 就要回收内存
+							delete(tp_communication_message);
+							tp_communication_message = NULL;
+//					return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+//										 " m_receive_data_list.push error ");
+						}
+					}
+					else
+					{
+						delete(tp_communication_message);
+						tp_communication_message = NULL;
+					}
+
+				}
+				//解析失败, 就当做什么也没发生, 认为接收消息无效,
+			}
+			//没有接受到消息, 返回空字符串
+		}
+	}
+
+	//LOG(INFO) << " Communication_socket_base::receive_data_thread end "<< communicator;
+	return;
+}
+
+//检查消息是否有效, 主要检查消息类型和接受者, 判断这条消息是不是给我的.
+Error_manager Communication_socket_base::check_msg(Communication_message*  p_msg)
+{
+	//通过 p_msg->get_message_type() 和 p_msg->get_receiver() 判断这条消息是不是给我的.
+	//子类重载时, 增加自己模块的判断逻辑, 以后再写.
+	if ( p_msg->get_message_type() == Communication_message::Message_type::eBase_msg
+		 && p_msg->get_receiver() == Communication_message::Communicator::eMain )
+	{
+		return Error_code::SUCCESS;
+	}
+	else
+	{
+		//认为接受人
+		return Error_code::INVALID_MESSAGE;
+	}
+}
+
+//mp_analysis_data_thread 解析线程执行函数，
+//analysis_data_thread 内部线程负责解析消息
+void Communication_socket_base::analysis_data_thread()
+{
+	//LOG(INFO) << " Communication_socket_base::analysis_data_thread start "<< this;
+
+	//通信解析线程, 负责巡检m_receive_data_list, 并解析和处理消息
+	while (m_analysis_data_condition.is_alive())
+	{
+        std::this_thread::yield();
+		bool t_pass_flag = m_analysis_data_condition.wait_for_millisecond(1000);
+		if ( m_analysis_data_condition.is_alive() )
+		{
+
+			//如果解析线程被主动唤醒, 那么就表示 收到新的消息, 那就遍历整个链表
+			if ( t_pass_flag )
+			{
+				analysis_receive_list();
+			}
+				//如果解析线程超时通过, 那么就定时处理链表残留的消息,
+			else
+			{
+				analysis_receive_list();
+			}
+		}
+	}
+
+	//LOG(INFO) << " Communication_socket_base::analysis_data_thread end "<< this;
+	return;
+}
+
+//循环接受链表, 解析消息,
+Error_manager Communication_socket_base::analysis_receive_list()
+{
+	Error_manager t_error;
+	if ( m_receive_data_list.m_termination_flag )
+	{
+		return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+							 " Communication_socket_base::analysis_receive_list error ");
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_receive_data_list.m_mutex);
+		for (auto iter = m_receive_data_list.m_data_list.begin(); iter != m_receive_data_list.m_data_list.end(); )
+		{
+			Communication_message* tp_msg = **iter;
+			if ( tp_msg == NULL )
+			{
+				iter = m_receive_data_list.m_data_list.erase(iter);
+				//注：erase 删除当前 iter 之后返回下一个节点，当前的 iter 无效化，
+			}
+			else
+			{
+				//检查消息是否可以被处理
+				t_error = check_executer(tp_msg);
+				if ( t_error == SUCCESS)
+				{
+					//处理消息
+					t_error = execute_msg(tp_msg);
+				if ( t_error!=SUCCESS )
+				{
+					fprintf(stderr,"%s\n",t_error.to_string().c_str());
+				}
+//				else
+//				{
+//					//执行结果不管
+//				}
+					delete(tp_msg);
+					tp_msg = NULL;
+					iter = m_receive_data_list.m_data_list.erase(iter);
+					//注：erase 删除当前 iter 之后返回下一个节点，当前的 iter 无效化，
+				}
+				else if( t_error == COMMUNICATION_EXCUTER_IS_BUSY)
+				{
+					//处理器正忙, 那就不做处理, 直接处理下一个
+					//注:这条消息就被保留了下来, wait_for_millisecond 超时通过之后, 会循环检查残留的消息.
+					iter++;
+				}
+				else //if( t_error == COMMUNICATION_ANALYSIS_TIME_OUT )
+				{
+					//超时了就直接删除
+					delete(tp_msg);
+					tp_msg = NULL;
+					iter = m_receive_data_list.m_data_list.erase(iter);
+					//注：erase 删除当前 iter 之后返回下一个节点，当前的 iter 无效化，
+
+					//注:消息删除之后, 不需要发送答复消息, 发送方也会有超时处理的,  只有 execute_msg 里面可以答复消息
+				}
+			}
+		}
+	}
+	return Error_code::SUCCESS;
+}
+
+
+
+//检查执行者的状态, 判断能否处理这条消息, 需要子类重载
+Error_manager Communication_socket_base::check_executer(Communication_message*  p_msg)
+{
+	//检查对应模块的状态, 判断是否可以处理这条消息
+	//同时也要判断是否超时, 超时返回 COMMUNICATION_ANALYSIS_TIME_OUT
+	//如果处理器正在忙别的, 那么返回 COMMUNICATION_EXCUTER_IS_BUSY
+
+	if ( p_msg->is_over_time() )
+	{
+		return Error_code::COMMUNICATION_ANALYSIS_TIME_OUT;
+	}
+	else
+	{
+		bool executer_is_ready = false;
+		//通过 p_msg->get_message_type() 和 p_msg->get_receiver() 找到处理模块的实例对象, 查询执行人是否可以处理这条消息
+		//这里子类重载时, 增加判断逻辑, 以后再写.
+//		executer_is_ready = true;
+
+		if ( executer_is_ready )
+		{
+			std::cout << "executer_is_ready , " << std::endl;
+			return Error_code::SUCCESS;
+		}
+		else
+		{
+			std::cout << "executer_is_busy , " << std::endl;
+			return Error_code::COMMUNICATION_EXCUTER_IS_BUSY;
+		}
+	}
+
+	return Error_code::SUCCESS;
+}
+
+//处理消息
+Error_manager Communication_socket_base::execute_msg(Communication_message*  p_msg)
+{
+	//先将 p_msg 转化为 对应的格式, 使用对应模块的protobuf来二次解析
+	// 不能一直使用 Communication_message*  p_msg, 这个是要销毁的
+	//然后处理这个消息, 就是调用对应模块的 execute 接口函数
+	//执行结果不管, 如果需要答复, 那么对应模块 在自己内部 封装一条消息发送即可.
+	//子类重载, 需要完全重写, 以后再写.
+
+	//注注注注注意了, 本模块只是用来做通信,
+	//在做处理消息的时候, 可能会调用执行者的接口函数,
+	//这里不应该长时间阻塞或者处理复杂的逻辑,
+	//请执行者另开线程来处理任务.
+
+	return Error_code::SUCCESS;
+}
+
+//mp_send_data_thread 发送线程执行函数，
+//send_data_thread 内部线程负责发送消息
+void Communication_socket_base::send_data_thread(Communication_socket_base* communicator)
+{
+	//LOG(INFO) << " Communication_socket_base::send_data_thread start "<< communicator;
+
+	//通信发送线程, 负责巡检m_send_data_list, 并发送消息
+	while (communicator->m_send_data_condition.is_alive())
+	{
+        std::this_thread::yield();
+        communicator->m_send_data_condition.wait();
+		if ( communicator->m_send_data_condition.is_alive() )
+		{
+
+
+			Communication_message* tp_msg = NULL;
+			//这里 wait_and_pop 会使用链表内部的 m_data_cond 条件变量来控制等待,
+			//封装线程使用push的时候, 会唤醒线程并通过等待, 此时 m_send_data_condition 是一直通过的.
+			//如果需要退出, 那么就要 m_send_data_list.termination_list();	和 m_send_data_condition.kill_all();
+			bool is_pop = communicator->m_send_data_list.wait_and_pop(tp_msg);
+			if ( is_pop )
+			{
+				if ( tp_msg != NULL )
+				{
+					std::unique_lock<std::mutex> lk(communicator->m_mutex);
+					int send_size = communicator->m_socket.send(tp_msg->get_message_buf());
+
+					delete(tp_msg);
+					tp_msg = NULL;
+				}
+			}
+			else
+			{
+				//没有取出, 那么应该就是 m_termination_flag 结束了
+//				return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+//									 " Communication_socket_base::send_data_thread() error ");
+			}
+		}
+	}
+
+	//LOG(INFO) << " Communication_socket_base::send_data_thread end "<< communicator;
+	return;
+}
+
+//mp_encapsulate_data_thread 封装线程执行函数，
+//encapsulate_data_thread 内部线程负责封装消息
+void Communication_socket_base::encapsulate_data_thread()
+{
+	//LOG(INFO) << " Communication_socket_base::encapsulate_data_thread start "<< this;
+
+	//通信封装线程, 负责定时封装消息, 并存入 m_send_data_list
+	while (m_encapsulate_data_condition.is_alive())
+	{
+        std::this_thread::yield();
+		bool t_pass_flag = m_encapsulate_data_condition.wait_for_millisecond(1000);
+		if ( m_encapsulate_data_condition.is_alive() )
+		{
+
+			//如果封装线程被主动唤醒, 那么就表示 需要主动发送消息,
+			if ( t_pass_flag )
+			{
+				//主动发送消息,
+			}
+				//如果封装线程超时通过, 那么就定时封装心跳和状态信息
+			else
+			{
+				encapsulate_send_data();
+			}
+		}
+	}
+
+	//LOG(INFO) << " Communication_socket_base::encapsulate_data_thread end "<< this;
+	return;
+}
+
+
+//定时封装发送消息, 一般为心跳和状态信息, 需要子类重载
+Error_manager Communication_socket_base::encapsulate_send_data()
+{
+//	char buf[256] = {0};
+//	static unsigned int t_heartbeat = 0;
+//	sprintf(buf, "Communication_socket_base, heartbeat = %d\0\0\0, test\0", t_heartbeat);
+//	t_heartbeat++;
+
+	message::Base_info t_base_msg;
+	t_base_msg.set_msg_type(message::Message_type::eBase_msg);
+	t_base_msg.set_timeout_ms(5000);
+	t_base_msg.set_sender(message::Communicator::eMain);
+	t_base_msg.set_receiver(message::Communicator::eMain);
+
+	Communication_message* tp_msg = new Communication_message(t_base_msg.SerializeAsString());
+	bool is_push = m_send_data_list.push(tp_msg);
+	if ( is_push == false )
+	{
+		delete(tp_msg);
+		tp_msg = NULL;
+		return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+							 " Communication_socket_base::encapsulate_msg error ");
+	}
+	return Error_code::SUCCESS;
+}
+
+
+
+//封装消息, 需要子类重载
+Error_manager Communication_socket_base::encapsulate_msg(std::string message)
+{
+	Communication_message* tp_msg = new Communication_message(message);
+	bool is_push = m_send_data_list.push(tp_msg);
+	if ( is_push == false )
+	{
+		delete(tp_msg);
+		tp_msg = NULL;
+		return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+							 " Communication_socket_base::encapsulate_msg error ");
+	}
+	return Error_code::SUCCESS;
+}
+
+//封装消息, 需要子类重载
+Error_manager Communication_socket_base::encapsulate_msg(Communication_message* p_msg)
+{
+	Communication_message* tp_msg = new Communication_message(*p_msg);
+
+	bool is_push = m_send_data_list.push(tp_msg);
+
+
+	if ( is_push == false )
+	{
+		delete(tp_msg);
+		tp_msg = NULL;
+		return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+							 " Communication_socket_base::encapsulate_msg error ");
+	}
+	return Error_code::SUCCESS;
+}

src/pkgname/src/communication/communication_socket_base.h

@@ -0,0 +1,143 @@
+
+
+/*
+ * communication_socket_base 通信模块的基类,
+ * 用户从这个基类继承, 初始化之后, 便可以自动进行通信
+ * 重载解析消息和封装消息,
+ *
+ *Thread_safe_list<Binary_buf*> 使用 Binary_buf , 而不是string
+ * 主要是为了支持直接发送数字0
+ * 
+ * 
+ * */
+
+#ifndef __COMMUNICATION_SOCKET_BASE__HH__
+#define __COMMUNICATION_SOCKET_BASE__HH__
+
+
+#include <nnxx/message>
+#include <nnxx/socket.h>
+#include <nnxx/bus.h>
+
+#include <glog/logging.h>
+#include "../error_code/error_code.h"
+#include "../tool/binary_buf.h"
+#include "../tool/thread_safe_list.h"
+#include "../tool/thread_condition.h"
+
+#include "../communication/communication.pb.h"
+#include "../communication/communication_message.h"
+
+#include "../message/message_base.pb.h"
+
+
+
+#define COMMUNICATION_PARAMETER_PATH "../setting/communication.prototxt"
+
+class Communication_socket_base
+{
+	//通信状态
+	enum Communication_statu
+	{
+		COMMUNICATION_UNKNOW		=0,	        //通信状态 未知
+		COMMUNICATION_READY			=1,			//通信状态 正常
+
+		COMMUNICATION_FAULT			=3,         //通信状态 错误
+	};
+
+public:
+	Communication_socket_base();
+	Communication_socket_base(const Communication_socket_base& other)= delete;
+	Communication_socket_base& operator =(const Communication_socket_base& other)= delete;
+	~Communication_socket_base();
+public://API functions
+	//初始化 通信 模块。如下三选一
+	virtual Error_manager communication_init();
+	//初始化 通信 模块。从文件读取
+	Error_manager communication_init_from_protobuf(std::string prototxt_path);
+	//初始化 通信 模块。从protobuf读取
+	Error_manager communication_init_from_protobuf(Communication_proto::Communication_parameter_all& communication_parameter_all);
+
+	//初始化
+	virtual Error_manager communication_init(std::string bind_string, std::vector<std::string>& connect_string_vector);
+	//bind
+	virtual Error_manager communication_bind(std::string bind_string);
+	//connect
+	virtual Error_manager communication_connect(std::vector<std::string>& connect_string_vector);
+	//connect
+	virtual Error_manager communication_connect(std::string connect_string);
+	//启动通信, run thread
+	virtual Error_manager communication_run();
+
+	//反初始化 通信 模块。
+	virtual Error_manager communication_uninit();
+	
+	
+public://get or set member variable
+
+protected:
+	//mp_receive_data_thread 接受线程执行函数，
+	//receive_data_thread 内部线程负责接受消息
+	static void receive_data_thread(Communication_socket_base* communicator);
+
+	//检查消息是否可以被解析, 需要子类重载
+	virtual Error_manager check_msg(Communication_message* p_msg);
+
+	//mp_analysis_data_thread 解析线程执行函数，
+	//analysis_data_thread 内部线程负责解析消息
+	void analysis_data_thread();
+
+	//遍历接受链表, 解析消息,
+	Error_manager analysis_receive_list();
+
+	//检查消息是否可以被解析, 需要子类重载
+	virtual Error_manager check_executer(Communication_message* p_msg);
+
+	//处理消息, 需要子类重载
+	virtual Error_manager execute_msg(Communication_message* p_msg);
+
+	//mp_send_data_thread 发送线程执行函数，
+	//send_data_thread 内部线程负责发送消息
+	static void send_data_thread(Communication_socket_base* communicator);
+
+	//mp_encapsulate_data_thread 封装线程执行函数，
+	//encapsulate_data_thread 内部线程负责封装消息
+	void encapsulate_data_thread();
+
+	//定时封装发送消息, 一般为心跳和状态信息, 需要子类重载
+	virtual Error_manager encapsulate_send_data();
+
+public:
+	//封装消息, 需要子类重载
+	virtual Error_manager encapsulate_msg(std::string message);
+	//封装消息, 需要子类重载
+	virtual Error_manager encapsulate_msg(Communication_message* p_msg);
+
+protected://member variable
+
+	//通用的网络编程接口, 默认使用总线模式, (网状结构)
+	nnxx::socket 						m_socket { nnxx::SP, nnxx::BUS };
+	std::mutex 							m_mutex;				//m_socket的锁
+
+	//通信状态
+	Communication_statu 				m_communication_statu;			//通信状态
+
+	//接受模块,
+	Thread_safe_list<Communication_message*>		m_receive_data_list; 			//接受的list容器
+	std::thread*						mp_receive_data_thread;    		//接受的线程指针
+	Thread_condition					m_receive_condition;			//接受的条件变量
+	std::thread*						mp_analysis_data_thread;    	//解析的线程指针
+	Thread_condition					m_analysis_data_condition;		//解析的条件变量
+
+	//发送模块,
+	Thread_safe_list<Communication_message*>		m_send_data_list;				//发送的list容器
+	std::thread*						mp_send_data_thread;    		//发送的线程指针
+	Thread_condition					m_send_data_condition;			//发送的条件变量
+	std::thread*						mp_encapsulate_data_thread;    	//封装的线程指针
+	Thread_condition					m_encapsulate_data_condition;	//封装的条件变量
+
+private:
+
+};
+
+#endif //__COMMUNICATION_SOCKET_BASE__HH__

src/pkgname/src/communication/message_communicator.cpp

@@ -0,0 +1,75 @@
+//
+// Created by zx on 2020/8/28.
+//
+
+#include "message_communicator.h"
+
+Message_communicator::Message_communicator()
+    :m_ros_data_callback(nullptr)
+{
+}
+
+Message_communicator::~Message_communicator()
+{
+}
+/*
+     * 发送消息
+     */
+Error_manager Message_communicator::send_msg(Communication_message* p_msg)
+{
+    return Communication_socket_base::encapsulate_msg(p_msg);
+}
+
+//定时封装发送消息, 一般为心跳和状态信息, 需要子类重载
+Error_manager Message_communicator::encapsulate_send_data()
+{
+    return Error_code::SUCCESS;
+}
+
+/*
+ * 检测消息是否合法
+ */
+Error_manager Message_communicator::check_msg(Communication_message* p_msg)
+{
+    if ( (p_msg->get_sender() == Communication_message::Communicator::eAGV
+            ||p_msg->get_sender() == Communication_message::Communicator::eDispatch) &&
+         (p_msg->get_message_type() == Communication_message::Message_type::ePointCloud_msg
+         ||p_msg->get_message_type() == Communication_message::Message_type::ePose2d_msg
+          ))
+    {
+        return Error_code::SUCCESS;
+    }
+    return Error_code::INVALID_MESSAGE;
+}
+
+//检查消息是否可以被处理, 需要重载
+Error_manager Message_communicator::check_executer(Communication_message* p_msg)
+{
+    return SUCCESS;
+}
+
+/*
+ * 处理接收到的消息
+ */
+Error_manager Message_communicator::execute_msg(Communication_message* p_msg)
+{
+    /*
+     * 接收终端指令, 生成流程
+     */
+    Error_manager code;
+
+    switch(p_msg->get_message_type())
+    {
+        case Communication_message::ePose2d_msg:
+        case Communication_message::ePointCloud_msg:
+        {
+            if (m_ros_data_callback != nullptr)
+            {
+                m_ros_data_callback(p_msg);
+            }
+            break;
+        }
+        default:break;
+    }
+    return SUCCESS;
+}

src/pkgname/src/communication/message_communicator.h

@@ -0,0 +1,52 @@
+//
+// Created by zx on 2020/8/28.
+//
+
+#ifndef NNXX_TESTS_MESSAGE_COMMUNICATOR_H
+#define NNXX_TESTS_MESSAGE_COMMUNICATOR_H
+
+#include "singleton.h"
+#include "communication_socket_base.h"
+#include "error_code.h"
+
+typedef Error_manager (*ros_data_callback)(Communication_message* p_msg);
+
+class Message_communicator :public Singleton<Message_communicator>, public Communication_socket_base
+{
+    friend class Singleton<Message_communicator>;
+public:
+    //必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+    Message_communicator(const Message_communicator& other) = delete;
+    Message_communicator& operator =(const Message_communicator& other) = delete;
+
+    /*
+     * 发送消息
+     */
+    Error_manager send_msg(Communication_message* p_msg);
+    Error_manager set_ros_data_callback(ros_data_callback callback){
+        m_ros_data_callback=callback ;
+        return SUCCESS;
+    };
+
+    ~Message_communicator();
+
+protected:
+    ros_data_callback                   m_ros_data_callback;
+    // 父类的构造函数必须保护，子类的构造函数必须私有。
+    Message_communicator();
+
+    virtual Error_manager execute_msg(Communication_message* p_msg);
+    /*
+     * 检测消息是否可被处理
+     */
+    virtual Error_manager check_msg(Communication_message* p_msg);
+    /*
+     * 心跳发送函数,重载
+     */
+    virtual Error_manager encapsulate_send_data();
+    //检查消息是否可以被解析, 需要重载
+    virtual Error_manager check_executer(Communication_message* p_msg);
+};
+
+
+#endif //NNXX_TESTS_MESSAGE_COMMUNICATOR_H

src/pkgname/src/error_code/error_code.cpp

@@ -0,0 +1,534 @@
+
+//Error_code是错误码的底层通用模块，
+//功能：用作故障分析和处理。
+
+//用法：所有的功能接口函数return错误管理类，
+//然后上层判断分析错误码，并进行故障处理。
+
+
+
+#include "error_code.h"
+
+/////////////////////////////////////////////
+//构造函数
+Error_manager::Error_manager()
+{
+    m_error_code = SUCCESS;
+    m_error_level = NORMAL;
+    pm_error_description = 0;
+    m_description_length = 0;
+    return ;
+}
+//拷贝构造
+Error_manager::Error_manager(const Error_manager & error_manager)
+{
+    this->m_error_code = error_manager.m_error_code;
+    this->m_error_level = error_manager.m_error_level;
+    pm_error_description = NULL;
+    m_description_length = 0;
+    reallocate_memory_and_copy_string(error_manager.pm_error_description, error_manager.m_description_length);
+    return ;
+}
+//赋值构造
+Error_manager::Error_manager(Error_code error_code, Error_level error_level,
+    const char* p_error_description, int description_length)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    pm_error_description = NULL;
+    m_description_length = 0;
+    reallocate_memory_and_copy_string(p_error_description, description_length);
+    return ;
+}
+//赋值构造
+Error_manager::Error_manager(Error_code error_code, Error_level error_level , std::string & error_aggregate_string)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    pm_error_description = NULL;
+    m_description_length = 0;
+    reallocate_memory_and_copy_string(error_aggregate_string);
+    return ;
+}
+//析构函数
+Error_manager::~Error_manager()
+{
+    free_description();
+}
+
+//初始化
+void Error_manager::error_manager_init()
+{
+    error_manager_clear_all();
+    return;
+}
+//初始化
+void Error_manager::error_manager_init(Error_code error_code, Error_level error_level, const char* p_error_description, int description_length)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    reallocate_memory_and_copy_string(p_error_description, description_length);
+    return ;
+}
+//初始化
+void Error_manager::error_manager_init(Error_code error_code, Error_level error_level , std::string & error_aggregate_string)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    reallocate_memory_and_copy_string(error_aggregate_string);
+    return ;
+}
+//重置
+void Error_manager::error_manager_reset(Error_code error_code, Error_level error_level, const char* p_error_description, int description_length)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    reallocate_memory_and_copy_string(p_error_description, description_length);
+    return ;
+}
+//重置
+void Error_manager::error_manager_reset(Error_code error_code, Error_level error_level , std::string & error_aggregate_string)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    reallocate_memory_and_copy_string(error_aggregate_string);
+    return ;
+}
+//重置
+void Error_manager::error_manager_reset(const Error_manager & error_manager)
+{
+    this->m_error_code = error_manager.m_error_code;
+    this->m_error_level = error_manager.m_error_level;
+    reallocate_memory_and_copy_string(error_manager.pm_error_description, error_manager.m_description_length);
+    return ;
+}
+//清除所有内容
+void Error_manager::error_manager_clear_all()
+{
+    m_error_code = SUCCESS;
+    m_error_level = NORMAL;
+    free_description();
+}
+
+//重载=
+Error_manager& Error_manager::operator=(const Error_manager & error_manager)
+{
+    error_manager_reset(error_manager);
+}
+//重载=，支持Error_manager和Error_code的直接转化，会清空错误等级和描述
+Error_manager& Error_manager::operator=(Error_code error_code)
+{
+    error_manager_clear_all();
+    set_error_code(error_code);
+}
+//重载==
+bool Error_manager::operator==(const Error_manager & error_manager)
+{
+    is_equal_error_manager(error_manager);
+}
+//重载==，支持Error_manager和Error_code的直接比较
+bool Error_manager::operator==(Error_code error_code)
+{
+    if(m_error_code == error_code)
+    {
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+//重载!=
+bool Error_manager::operator!=(const Error_manager & error_manager)
+{
+    ! is_equal_error_manager(error_manager);
+}
+//重载!=，支持Error_manager和Error_code的直接比较
+bool Error_manager::operator!=(Error_code error_code)
+{
+    if(m_error_code != error_code)
+    {
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+//重载<<，支持cout<<
+std::ostream & operator<<(std::ostream &out, Error_manager &error_manager)
+{
+	out << error_manager.to_string();
+	return out;
+}
+
+//获取错误码
+Error_code Error_manager::get_error_code()
+{
+    return m_error_code;
+}
+//获取错误等级
+Error_level Error_manager::get_error_level()
+{
+    return m_error_level;
+}
+//获取错误描述的指针，（浅拷贝）
+char* Error_manager::get_error_description()
+{
+    if(pm_error_description== nullptr)
+        return "";
+    return pm_error_description;
+}
+
+//复制错误描述，（深拷贝）
+//output：p_error_description     错误描述的字符串指针，不可以为NULL，必须要有实际的内存
+//output:description_length      错误描述的字符串长度，不可以为0，长度最好足够大，一般256即可。
+void Error_manager::copy_error_description(const char* p_error_description, int description_length)
+{
+    if(p_error_description != NULL && pm_error_description != NULL)
+    {
+        char *pt_source = (char *)p_error_description;
+        char* pt_destination = pm_error_description;
+
+        int t_length_min = m_description_length;
+        if(m_description_length > description_length)
+        {
+            t_length_min = description_length;
+        }
+
+        for(int i=0;i<t_length_min; i++)
+        {
+            *pt_destination = *pt_source;
+            pt_destination++;
+            pt_source++;
+        }
+    }
+
+    return;
+}
+//复制错误描述，（深拷贝）
+//output：error_description_string     错误描述的string
+void Error_manager::copy_error_description(std::string & error_description_string)
+{
+    if( (!error_description_string.empty() ) && pm_error_description != NULL)
+    {
+        error_description_string = pm_error_description;
+    }
+    return;
+}
+
+//设置错误码
+void Error_manager::set_error_code(Error_code error_code)
+{
+    m_error_code = error_code;
+    return;
+}
+//比较错误等级并升级，取高等级的结果
+void Error_manager::set_error_level_up(Error_level error_level)
+{
+    if(m_error_level < error_level)
+    {
+        m_error_level = error_level;
+    }
+    return;
+}
+//比较错误等级并降级，取低等级的结果
+void Error_manager::set_error_level_down(Error_level error_level)
+{
+    if(m_error_level > error_level)
+    {
+        m_error_level = error_level;
+    }
+    return;
+}
+//错误等级，设定到固定值
+void Error_manager::set_error_level_location(Error_level error_level)
+{
+    m_error_level = error_level;
+    return;
+}
+//设置错误描述
+void Error_manager::set_error_description(const char* p_error_description, int description_length)
+{
+    reallocate_memory_and_copy_string(p_error_description, description_length);
+    return ;
+}
+//设置错误描述
+void Error_manager::set_error_description(std::string & error_description_string)
+{
+    reallocate_memory_and_copy_string(error_description_string);
+    return ;
+}
+
+//尾部追加错误描述
+void Error_manager::add_error_description(const char* p_error_description, int description_length)
+{
+    if(p_error_description !=NULL)
+    {
+        char* pt_description_front = pm_error_description;
+        int t_description_front_length = m_description_length;
+        char* pt_description_back = (char *)p_error_description;
+        int t_description_back_length = 0;
+
+        if(description_length == 0)
+        {
+            t_description_back_length = 0;
+            while (*pt_description_back != '\0')
+            {
+                t_description_back_length++;
+                pt_description_back++;
+            }
+            t_description_back_length++;
+            pt_description_back = (char *)p_error_description;
+        }
+        else
+        {
+            t_description_back_length = description_length;
+        }
+
+        int t_description_new_length = t_description_front_length + 5 + t_description_back_length - 1;
+        char* pt_description_new =  (char*) malloc(t_description_new_length );
+
+        sprintf(pt_description_new, "%s ### %s", pt_description_front, pt_description_back);
+        free_description();
+        pm_error_description = pt_description_new;
+        m_description_length = t_description_new_length;
+    }
+    return ;
+}
+//尾部追加错误描述
+void Error_manager::add_error_description(std::string & error_description_string)
+{
+    if( ! error_description_string.empty() )
+    {
+        std::string t_description_string = pm_error_description ;
+        t_description_string += (" ### "+ error_description_string);
+
+        reallocate_memory_and_copy_string(t_description_string);
+    }
+}
+
+//比较错误是否相同，
+// 注：只比较错误码和等级
+bool Error_manager::is_equal_error_manager(const Error_manager & error_manager)
+{
+    if(this->m_error_code == error_manager.m_error_code
+       && this->m_error_level == error_manager.m_error_level)
+    {
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+//比较并覆盖错误，讲低级错误转为字符串存放于描述中，
+//如果错误相同，则保留this的，将输入参数转入描述。
+void Error_manager::compare_and_cover_error(const Error_manager & error_manager)
+{
+    if(this->m_error_code == SUCCESS)
+    {
+        error_manager_reset(error_manager);
+    }
+    else if (error_manager.m_error_code == SUCCESS)
+    {
+		return;
+    }
+    else
+    {
+        Error_manager t_error_manager_new;
+        char* pt_string_inside = NULL;
+        int t_string_inside_length = 0;
+        if(this->m_error_level < error_manager.m_error_level)
+        {
+            t_string_inside_length = ERROR_NAMAGER_TO_STRING_FRONT_LENGTH + this->m_description_length;
+            pt_string_inside = (char*)malloc(t_string_inside_length);
+            translate_error_to_string(pt_string_inside, t_string_inside_length);
+
+            error_manager_reset(error_manager);
+            add_error_description(pt_string_inside,t_string_inside_length);
+        }
+        else
+        {
+            t_string_inside_length = ERROR_NAMAGER_TO_STRING_FRONT_LENGTH + error_manager.m_description_length;
+            pt_string_inside = (char*)malloc(t_string_inside_length);
+			((Error_manager & )error_manager).translate_error_to_string(pt_string_inside, t_string_inside_length);
+
+            add_error_description(pt_string_inside,t_string_inside_length);
+        }
+    }
+}
+//比较并覆盖错误，讲低级错误转为字符串存放于描述中，
+//如果错误相同，则保留this的，将输入参数转入描述。
+void Error_manager::compare_and_cover_error( Error_manager * p_error_manager)
+{
+	if(this->m_error_code == SUCCESS)
+	{
+		error_manager_reset(*p_error_manager);
+	}
+	else if (p_error_manager->m_error_code == SUCCESS)
+	{
+		//
+	}
+	else
+	{
+		Error_manager t_error_manager_new;
+		char* pt_string_inside = NULL;
+		int t_string_inside_length = 0;
+		if(this->m_error_level < p_error_manager->m_error_level)
+		{
+			t_string_inside_length = ERROR_NAMAGER_TO_STRING_FRONT_LENGTH + this->m_description_length;
+			pt_string_inside = (char*)malloc(t_string_inside_length);
+			translate_error_to_string(pt_string_inside, t_string_inside_length);
+
+			error_manager_reset(*p_error_manager);
+			add_error_description(pt_string_inside,t_string_inside_length);
+		}
+		else
+		{
+			t_string_inside_length = ERROR_NAMAGER_TO_STRING_FRONT_LENGTH + p_error_manager->m_description_length;
+			pt_string_inside = (char*)malloc(t_string_inside_length);
+			p_error_manager->translate_error_to_string(pt_string_inside, t_string_inside_length);
+
+			add_error_description(pt_string_inside,t_string_inside_length);
+		}
+	}
+}
+
+//将所有的错误信息，格式化为字符串，用作日志打印。
+//output：p_error_description     错误汇总的字符串指针，不可以为NULL，必须要有实际的内存
+//output:description_length      错误汇总的字符串长度，不可以为0，长度最好足够大，一般256即可。
+void Error_manager::translate_error_to_string(char* p_error_aggregate, int aggregate_length )
+{
+    char t_string_array[ERROR_NAMAGER_TO_STRING_FRONT_LENGTH] = {0};
+    char* pt_index_front = t_string_array;
+    char* pt_index_back = pm_error_description;
+    char* pt_index = p_error_aggregate;
+
+    sprintf(t_string_array, "error_code:0x%08x, error_level:%02d, error_description:", m_error_code , m_error_level );
+
+    int t_length_min = m_description_length + ERROR_NAMAGER_TO_STRING_FRONT_LENGTH -1;
+    if(t_length_min > aggregate_length)
+    {
+        t_length_min = aggregate_length;
+    }
+
+    for(int i=0;i<t_length_min; i++)
+    {
+        if(i < ERROR_NAMAGER_TO_STRING_FRONT_LENGTH -1)
+        {
+            *pt_index = *pt_index_front;
+            pt_index++;
+            pt_index_front++;
+        }
+        else
+        {
+            *pt_index = *pt_index_back;
+            pt_index++;
+            pt_index_back++;
+        }
+    }
+}
+//output：error_description_string     错误汇总的string
+void Error_manager::translate_error_to_string(std::string & error_aggregate_string)
+{
+    char t_string_array[ERROR_NAMAGER_TO_STRING_FRONT_LENGTH] = {0};
+    sprintf(t_string_array, "error_code:0x%08x, error_level:%02d, error_description:", m_error_code , m_error_level );
+
+    error_aggregate_string = t_string_array ;
+    if(pm_error_description != NULL)
+    {
+        error_aggregate_string += pm_error_description;
+    }
+    else
+    {
+        //error_aggregate_string += "NULL";
+    }
+}
+//错误码转字符串的简易版，可支持cout<<
+//return     错误汇总的string
+std::string Error_manager::to_string()
+{
+    std::string t_string;
+    translate_error_to_string(t_string);
+    return t_string;
+}
+
+
+
+
+//释放错误描述的内存，
+void Error_manager::free_description()
+{
+    if(pm_error_description != NULL)
+    {
+        free (pm_error_description);
+        pm_error_description = NULL;
+    }
+    m_description_length = 0;
+}
+
+//重新分配错误描述的内存，并从外部拷贝新的（深拷贝）
+//input：p_error_description     错误描述的字符串指针，可以为NULL，
+//input:description_length      错误描述的字符串长度，如果为0，则从p_error_description里面获取有效的长度
+void Error_manager::reallocate_memory_and_copy_string(const char* p_error_description, int description_length)
+{
+    free_description();
+
+    if(p_error_description != NULL)
+    {
+        char* pt_source = (char *)p_error_description;
+        char* pt_destination = NULL;
+
+        if(description_length == 0)
+        {
+            m_description_length = 0;
+            while (*pt_source != '\0')
+            {
+                m_description_length++;
+                pt_source++;
+            }
+            m_description_length++;
+            pt_source = (char *)p_error_description;
+        }
+        else
+        {
+            m_description_length = description_length;
+        }
+
+        pm_error_description =  (char*) malloc(m_description_length );
+        pt_destination = pm_error_description;
+
+        for(int i=0;i<m_description_length; i++)
+        {
+            *pt_destination = *pt_source;
+            pt_destination++;
+            pt_source++;
+        }
+    }
+
+    return;
+}
+
+
+//重新分配错误描述的内存，并从外部拷贝新的（深拷贝）
+//input：error_aggregate_string     错误描述的string
+void Error_manager::reallocate_memory_and_copy_string(std::string & error_aggregate_string)
+{
+    free_description();
+
+    if( ! error_aggregate_string.empty())
+    {
+        m_description_length = error_aggregate_string.length()+1;
+
+        pm_error_description =  (char*) malloc( m_description_length );
+
+        strcpy(pm_error_description ,   error_aggregate_string.c_str()  );
+    }
+}
+
+
+
+
+

src/pkgname/src/error_code/error_code.h

@@ -0,0 +1,379 @@
+
+//Error_code是错误码的底层通用模块，
+//功能：用作故障分析和处理。
+
+//用法：所有的功能接口函数return错误管理类，
+//然后上层判断分析错误码，并进行故障处理。
+
+
+
+#ifndef TEST_ERROR_ERROR_CODE_H
+#define TEST_ERROR_ERROR_CODE_H
+
+#include <string>
+#include <string.h>
+#include<iostream>
+
+//错误管理类转化为字符串 的前缀，固定长度为58
+//这个是由显示格式来确定的，如果要修改格式或者 Error_code长度超过8位，Error_level长度超过2位，折需要重新计算
+#define ERROR_NAMAGER_TO_STRING_FRONT_LENGTH   58
+
+//进程加锁的状态，
+enum Lock_status
+{
+    UNLOCK      = 0,
+    LOCK        = 1,
+};
+
+//设备使能状态，
+enum Able_status
+{
+    UNABLE      = 0,
+    ENABLE      = 1,
+};
+
+//数据是否为空
+enum Empty_status
+{
+    NON_EMPTY   = 0,
+    EMPTY       = 1,
+};
+
+
+//错误码的枚举，用来做故障分析
+enum Error_code
+{
+    //成功，没有错误，默认值0
+    SUCCESS                         = 0x00000000,
+
+
+    //基本错误码，
+    FAILED                          = 0x00000001,//失败
+    ERROR                           = 0x00000002,//错误
+    WARNING                         = 0x00000003,//警告
+    PARTIAL_SUCCESS                 = 0x00000002,//部分成功
+
+
+    NO_DATA                         = 0x00000010,//没有数据，传入参数容器内部没有数据时，
+	INVALID_MESSAGE					= 0x00000011, //无效的消息,
+    RESPONSE_TIMEOUT                = 0x00000012,
+    PAUSE                           = 0x00000013,   //急停
+    TASK_CANCEL                     = 0x00000014,   //任务取消
+
+    DISCONNECT                      = 0x00000020,   //通讯中断/断开连接
+    UNKNOW_STATU                    = 0x00000021,   //未知状态
+
+    POINTER_IS_NULL                 = 0x00000101,//空指针
+    PARAMETER_ERROR                 = 0x00000102,//参数错误，传入参数不符合规范时，
+    POINTER_MALLOC_FAIL             = 0x00000103,//手动分配内存失败
+
+    CLASS_BASE_FUNCTION_CANNOT_USE  = 0x00000201,//基类函数不允许使用，必须使用子类的
+
+	CONTAINER_IS_TERMINATE			= 0x00000301,//容器被终止
+
+
+
+
+//    错误码的规范，
+//    错误码是int型，32位，十六进制。
+//    例如0x12345678
+//    12表示功能模块，例如:laser雷达模块               	框架制定
+//    34表示文件名称，例如:laser_livox.cpp             框架制定
+//    56表示具体的类，例如:class laser_livox           个人制定
+//    78表示类的函数，例如:laser_livox::start();       个人制定
+//    注：错误码的制定从1开始，不要从0开始，
+//        0用作错误码的基数，用来位运算，来判断错误码的范围。
+
+//    laser扫描模块
+    LASER_ERROR_BASE                = 0x01000000,
+
+//    laser_base基类
+	LASER_BASE_ERROR_BASE			= 0x01010000,
+    LASER_TASK_PARAMETER_ERROR      = 0x01010001,   //雷达基类模块, 任务输入参数错误
+    LASER_CONNECT_FAILED,							//雷达基类模块, 连接失败
+	LASER_START_FAILED,								//雷达基类模块, 开始扫描失败
+	LASER_CHECK_FAILED,								//雷达基类模块, 检查失败
+	LASER_STATUS_BUSY,								//雷达基类模块, 状态正忙
+	LASER_STATUS_ERROR,								//雷达基类模块, 状态错误
+	LASER_TASK_OVER_TIME,							//雷达基类模块, 任务超时
+	LASER_QUEUE_ERROR,								//雷达基类模块, 数据缓存错误
+
+
+//    laser_livox.cpp的错误码
+    LIVOX_ERROR_BASE                = 0x01020000,
+    LIVOX_START_FAILE,								//livox模块，开始扫描失败
+	LIVOX_TASK_TYPE_ERROR,							//livox模块，任务类型错误
+	lIVOX_CANNOT_PUSH_DATA,							//livox模块，不能添加扫描的数据
+	lIVOX_CHECK_FAILED,								//livox模块，检查失败
+	lIVOX_STATUS_BUSY,								//livox模块，状态正忙
+	lIVOX_STATUS_ERROR,								//livox模块，状态错误
+
+	//laser_manager 雷达管理模块
+	LASER_MANAGER_ERROR_BASE						= 0x01030000,
+	LASER_MANAGER_READ_PROTOBUF_ERROR,				//雷达管理模块，读取参数错误
+	LASER_MANAGER_STATUS_BUSY,						//雷达管理模块，状态正忙
+	LASER_MANAGER_STATUS_ERROR,						//雷达管理模块，状态错误
+	LASER_MANAGER_TASK_TYPE_ERROR,					//雷达管理模块，任务类型错误
+	LASER_MANAGER_IS_NOT_READY,						//雷达管理模块，不在准备状态
+	LASER_MANAGER_LASER_INDEX_ERRPR,				//雷达管理模块，雷达索引错误，编号错误。
+	LASER_MANAGER_TASK_OVER_TIME,					//雷达管理模块，任务超时
+	LASER_MANAGER_LASER_INDEX_REPEAT,				//雷达管理模块，需要扫描的雷达索引重复,可忽略的错误,提示作用
+
+//livox_driver 雷达livox驱动模块
+	LIVOX_DRIVER_ERROR_BASE							= 0x01040000,
+	LIVOX_DRIVER_SN_REPEAT,							//livox驱动模块, 雷达广播码重复
+	LIVOX_DRIVER_SN_ERROR,							//livox驱动模块, 雷达广播码错误
+	LIVOX_SKD_INIT_FAILED,							//livox驱动模块, livox_sdk初始化失败
+	LIVOX_DRIVER_NOT_READY,							//livox驱动模块, livox没有准备好.
+
+
+
+
+    //locate 定位模块,
+	LOCATER_ERROR_BASE                				= 0x03000000,
+
+	//LASER_MANAGER 定位管理模块
+	LOCATER_MANAGER_ERROR_BASE                		= 0x03010000,
+    LOCATER_MSG_TABLE_NOT_EXIST ,                                   //测量反馈未找到相应的请求(致命)
+    LOCATER_MSG_RESPONSE_TYPE_ERROR,                                //测量反馈消息类型错误(致命)
+    LOCATER_MSG_RESPONSE_INFO_ERROR,
+    LOCATER_MSG_REQUEST_CANCELED,
+    LOCATER_MSG_REQUEST_INVALID,
+    LOCATER_MSG_RESPONSE_HAS_NO_REQUEST,
+    LOCATER_MSG_REQUEST_REPEATED,
+
+   /*
+    * parkspace error code
+    */
+    PARKSPACE_REQUEST_MSG_TYPE_ERROR                = 0x04010000,
+    PARKSPACE_ALLOCMSG_RESPONSE_HAS_NO_REQUEST,
+    PARKSPACE_SEARCHMSG_RESPONSE_HAS_NO_REQUEST,
+    PARKSPACE_RELEASEMSG_RESPONSE_HAS_NO_REQUEST,
+    PARKSPACE_ALLOC_REQUEST_INVALID,
+    PARKSPACE_SEARCH_REQUEST_INVALID,
+    PARKSPACE_RELEASE_REQUEST_INVALID,
+
+    PARKSPACE_ALLOC_REQUEST_REPEATED,
+    PARKSPACE_SEARCH_REQUEST_REPEATED,
+    PARKSPACE_RELEASE_REQUEST_REPEATED,
+
+    PARKSPACE_ALLOC_RESPONSE_TYPE_ERROR,
+    PARKSPACE_SEARCH_RESPONSE_TYPE_ERROR,
+    PARKSPACE_RELEASE_RESPONSE_TYPE_ERROR,
+
+    PARKSPACE_ALLOC_RESPONSE_INFO_ERROR,
+    PARKSPACE_SEARCH_RESPONSE_INFO_ERROR,
+    PARKSPACE_RELEASE_RESPONSE_INFO_ERROR,
+
+    PARKSPACE_ALLOC_REQUEST_CANCELED,
+    PARKSPACE_SEARCH_REQUEST_CANCELED,
+    PARKSPACE_RELEASE_REQUEST_CANCELED,
+
+
+	//Communication module, 通信模块
+	COMMUNICATION_BASE_ERROR_BASE					= 0x11010000,
+	COMMUNICATION_READ_PROTOBUF_ERROR,				//模块，读取参数错误
+	COMMUNICATION_BIND_ERROR,
+	COMMUNICATION_CONNECT_ERROR,
+	COMMUNICATION_ANALYSIS_TIME_OUT,									//解析超时,
+	COMMUNICATION_EXCUTER_IS_BUSY,										//处理器正忙, 请稍等
+
+
+	//system module, 系统模块
+	SYSTEM_EXECUTOR_ERROR_BASE						= 0x12010000,		//系统执行模块,
+	SYSTEM_EXECUTOR_PARSE_ERROR,										//系统执行模块, 解析消息错误
+	SYSTEM_EXECUTOR_STATUS_BUSY,										//系统执行模块, 状态正忙
+	SYSTEM_EXECUTOR_STATUS_ERROR,										//系统执行模块, 状态错误
+	SYSTEM_EXECUTOR_CHECK_ERROR,										//系统执行模块, 检查错误
+
+
+
+	//Communication can module, can通信模块
+	CAN_BASE_ERROR_BASE								= 0x15010000,		//can通信模块错误基地址
+	CAN_READ_PROTOBUF_ERROR,											//can通信模块, 读取参数错误
+	CAN_INDEX_REPEAT,
+	CAN_INIT_ERROR,														//can初始化失败
+	CAN_RUN_ERROR,														//can启动失败
+
+	//motor 电机驱动器模块
+	MOTOR_DRIVER_ERROR_BASE							= 0x16010000,		//电机驱动器模块, 错误基地址
+	MOTOR_DRIVER_DEVICE_ERROR,											//电机驱动器模块, 设备故障
+	MOTOR_DRIVER_COMMUNICATION_ERROR,									//电机驱动器模块, 通信故障
+
+	//motor 电机管理模块
+	MOTOR_MANAGER_ERROR_BASE						= 0x16020000,		//电机管理模块, 错误基地址
+	MOTOR_MANAGER_READ_PROTOBUF_ERROR, 									//电机管理模块, 读取prototxt错误
+	MOTOR_MANAGER_INDEX_REPEAT,											//电机管理模块, 设备索引重复,可忽略的错误,提示作用
+	MOTOR_MANAGER_STATUS_ERROR,
+};
+
+//错误等级，用来做故障处理
+enum Error_level
+{
+//    正常，没有错误，默认值0
+    NORMAL                = 0,
+
+
+//    轻微故障,可忽略的故障，NEGLIGIBLE_ERROR
+//    提示作用，不做任何处理，不影响代码的流程，
+//    用作一些不重要的事件，即使出错也不会影响到系统功能，
+//    例如：文件保存错误，等
+    NEGLIGIBLE_ERROR      = 1,
+
+
+//    一般故障，MINOR_ERROR
+//    用作底层功能函数的错误返回，表示该功能函数执行失败，
+//    返回给应用层之后，需要做故障分析和处理，
+//    例如：雷达数据传输失败，应用层就需要进行重新扫描，或者重连，或者重置参数等。
+    MINOR_ERROR           = 2,
+
+
+//    严重故障，MAJOR_ERROR
+//    用作应用层的任务事件的结果，表示该功能模块失败。
+//    通常是底层函数返回一般故障之后，应用层无法处理并解决故障，此时就要进行故障升级，
+//    从一般故障升级为严重故障，然后进行回退流程，回退已经执行的操作，最终回到故障待机状态。
+//    需要外部清除故障，并复位至正常待机状态，才能恢复功能的使用。
+//    例如：雷达扫描任务失败，且无法自动恢复。
+    MAJOR_ERROR           = 3,
+
+
+//    致命故障，CRITICAL_ERROR
+//    系统出现致命错误。导致系统无法正常运行，
+//    此时系统应该紧急停机，执行紧急流程，快速停机。
+//    此时不允许再执行任何函数和任务指令，防止系统故障更加严重。
+//    也不需要做任何错误处理了，快速执行紧急流程。
+//    例如：内存错误，进程挂死，关键设备失控，监控设备报警，等
+    CRITICAL_ERROR        = 4,
+};
+
+
+class Error_manager
+{
+public://外部接口函数
+    //构造函数
+    Error_manager();
+    //拷贝构造
+    Error_manager(const Error_manager & error_manager);
+    //赋值构造
+    Error_manager(Error_code error_code, Error_level error_level = NORMAL,
+                  const char* p_error_description = NULL, int description_length = 0);
+    //赋值构造
+    Error_manager(Error_code error_code, Error_level error_level , std::string & error_aggregate_string);
+    //析构函数
+    ~Error_manager();
+
+    //初始化
+    void error_manager_init();
+    //初始化
+    void error_manager_init(Error_code error_code, Error_level error_level = NORMAL,
+                            const char* p_error_description = NULL, int description_length = 0);
+    //初始化
+    void error_manager_init(Error_code error_code, Error_level error_level , std::string & error_aggregate_string);
+    //重置
+    void error_manager_reset(Error_code error_code, Error_level error_level = NORMAL,
+                             const char* p_error_description = NULL, int description_length = 0);
+    //重置
+    void error_manager_reset(Error_code error_code, Error_level error_level , std::string & error_aggregate_string);
+    //重置
+    void error_manager_reset(const Error_manager & error_manager);
+    //清除所有内容
+    void error_manager_clear_all();
+
+    //重载=
+    Error_manager& operator=(const Error_manager & error_manager);
+    //重载=，支持Error_manager和Error_code的直接转化，会清空错误等级和描述
+    Error_manager& operator=(Error_code error_code);
+    //重载==
+    bool operator==(const Error_manager & error_manager);
+    //重载==，支持Error_manager和Error_code的直接比较
+    bool operator==(Error_code error_code);
+    //重载!=
+    bool operator!=(const Error_manager & error_manager);
+    //重载!=，支持Error_manager和Error_code的直接比较
+    bool operator!=(Error_code error_code);
+	//重载<<，支持cout<<
+	friend std::ostream & operator<<(std::ostream &out, Error_manager &error_manager);
+
+
+    //获取错误码
+    Error_code get_error_code();
+    //获取错误等级
+    Error_level get_error_level();
+    //获取错误描述的指针，（浅拷贝）
+    char* get_error_description();
+
+    //复制错误描述，（深拷贝）
+    //output：p_error_description     错误描述的字符串指针，不可以为NULL，必须要有实际的内存
+    //output:description_length      错误描述的字符串长度，不可以为0，长度最好足够大，一般256即可。
+    void copy_error_description(const char* p_error_description, int description_length);
+    //复制错误描述，（深拷贝）
+    //output：error_description_string     错误描述的string
+    void copy_error_description(std::string & error_description_string);
+
+    //设置错误码
+    void set_error_code(Error_code error_code);
+    //比较错误等级并升级，取高等级的结果
+    void set_error_level_up(Error_level error_level);
+    //比较错误等级并降级，取低等级的结果
+    void set_error_level_down(Error_level error_level);
+    //错误等级，设定到固定值
+    void set_error_level_location(Error_level error_level);
+    //设置错误描述
+    void set_error_description(const char* p_error_description, int description_length = 0);
+    //设置错误描述
+    void set_error_description(std::string & error_description_string);
+
+    //尾部追加错误描述
+    void add_error_description(const char* p_error_description, int description_length = 0);
+    //尾部追加错误描述
+    void add_error_description(std::string & error_description_string);
+
+    //比较错误是否相同，
+    // 注：只比较错误码和等级
+	bool is_equal_error_manager(const Error_manager & error_manager);
+	//比较并覆盖错误，讲低级错误转为字符串存放于描述中，
+	//如果错误相同，则保留this的，将输入参数转入描述。
+	void compare_and_cover_error(const Error_manager & error_manager);
+	//比较并覆盖错误，讲低级错误转为字符串存放于描述中，
+	//如果错误相同，则保留this的，将输入参数转入描述。
+	void compare_and_cover_error( Error_manager * p_error_manager);
+
+	//将所有的错误信息，格式化为字符串，用作日志打印。
+    //output：p_error_description     错误汇总的字符串指针，不可以为NULL，必须要有实际的内存
+    //output:description_length      错误汇总的字符串长度，不可以为0，长度最好足够大，一般256即可。
+    void translate_error_to_string(char* p_error_aggregate, int aggregate_length);
+    //output：error_description_string     错误汇总的string
+    void translate_error_to_string(std::string & error_aggregate_string);
+    //错误码转字符串的简易版，可支持cout<<
+    //return     错误汇总的string
+    std::string to_string();
+
+
+
+protected:
+    Error_code              m_error_code;               //错误码
+    Error_level             m_error_level;              //错误等级
+    char*                   pm_error_description;       //错误描述
+    int                     m_description_length;       //错误描述的字符长度
+
+protected://内部功能函数
+public:
+    //释放错误描述的内存，
+    void free_description();
+
+    //重新分配错误描述的内存，并从外部拷贝新的（深拷贝）
+    //input：p_error_description     错误描述的字符串指针，可以为NULL，
+    //input:description_length      错误描述的字符串长度，如果为0，则从p_error_description里面获取有效的长度
+    void reallocate_memory_and_copy_string(const char* p_error_description, int description_length = 0);
+
+    //重新分配错误描述的内存，并从外部拷贝新的（深拷贝）
+    //input：error_aggregate_string     错误描述的string
+    void reallocate_memory_and_copy_string(std::string & error_aggregate_string);
+};
+
+
+
+
+#endif //TEST_ERROR_ERROR_CODE_H
+
+

src/pkgname/src/main.cpp

@@ -0,0 +1,239 @@
+#include <iostream>
+
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <ros_message.pb.h>
+#include "message_communicator.h"
+#include "pathcreator.h"
+#include <math.h>
+
+using google::protobuf::io::ZeroCopyInputStream;
+using google::protobuf::io::CodedInputStream;
+using google::protobuf::io::ZeroCopyOutputStream;
+using google::protobuf::io::CodedOutputStream;
+using google::protobuf::Message;
+
+GOOGLE_GLOG_DLL_DECL void shut_down_logging(const char* data, int size);
+void init_glog();
+
+ros::NodeHandle* gp_nh= nullptr;
+ros::Subscriber subGoal;
+std::map<std::string,ros::Publisher*>       g_topic_map;
+
+sensor_msgs::PointCloud create_cloudmsg(pcl::PointCloud<pcl::PointXYZ> cloud_pcl,std::string frame_id)
+{
+    sensor_msgs::PointCloud cloud;
+    cloud.header.stamp = ros::Time::now();
+    cloud.header.frame_id = frame_id;
+    cloud.points.resize(cloud_pcl.size());
+    //we'll also add an intensity channel to the cloud
+    cloud.channels.resize(1);
+    cloud.channels[0].name = "xyz";
+    cloud.channels[0].values.resize(cloud_pcl.size());
+
+    //generate some fake data for our point cloud
+    for(unsigned int i = 0; i < cloud_pcl.size(); ++i){
+        cloud.points[i].x = cloud_pcl[i].x;
+        cloud.points[i].y = cloud_pcl[i].y;
+        cloud.points[i].z = cloud_pcl[i].z;
+        cloud.channels[0].values[i] = 255;
+    }
+    return cloud;
+}
+
+
+Error_manager Ros_data_callback(Communication_message* p_msg)
+{
+    message::Base_msg msg;
+    msg.ParseFromString(p_msg->get_message_buf());
+
+    switch (msg.base_info().msg_type())
+    {
+        case message::ePointCloud_msg:
+        {
+            message::PointCloud_msg cloud_msg;
+            if(cloud_msg.ParseFromString(p_msg->get_message_buf()))
+            {
+                sensor_msgs::PointCloud cloud;
+                cloud.header.stamp = ros::Time::now();
+                cloud.header.frame_id = cloud_msg.frame_id();
+                cloud.points.resize(cloud_msg.points().size());
+                //we'll also add an intensity channel to the cloud
+                cloud.channels.resize(1);
+                cloud.channels[0].name = "xyz";
+                cloud.channels[0].values.resize(cloud_msg.points().size());
+
+                //generate some fake data for our point cloud
+                for (unsigned int i = 0; i < cloud_msg.points().size(); ++i)
+                {
+                    cloud.points[i].x = cloud_msg.points(i).x();
+                    cloud.points[i].y = cloud_msg.points(i).y();
+                    cloud.points[i].z = cloud_msg.points(i).z();
+                    cloud.channels[0].values[i] = cloud_msg.points(i).intensity();
+                }
+                std::string topic=cloud_msg.topic();
+                if(g_topic_map.find(topic)==g_topic_map.end())
+                {
+                    ros::Publisher* publisher=new ros::Publisher();
+                    *publisher = gp_nh->advertise<sensor_msgs::PointCloud> (topic, 1);
+                    g_topic_map[topic]=publisher;
+                }
+                g_topic_map[topic]->publish(cloud);
+            }
+            break;
+        }
+        case message::ePose2d_msg:
+        {
+            message::Pose2d_msg pose_msg;
+            if(pose_msg.ParseFromString(p_msg->get_message_buf()))
+            {
+                geometry_msgs::PoseStamped pose_stamped;
+                pose_stamped.header.stamp = ros::Time::now();
+                pose_stamped.header.frame_id = pose_msg.frame_id();
+                pose_stamped.pose.position.x = pose_msg.x();
+                pose_stamped.pose.position.y = pose_msg.y();
+                pose_stamped.pose.position.z = 0;
+                pose_stamped.pose.orientation.x = 0;
+                pose_stamped.pose.orientation.y = 0;
+                pose_stamped.pose.orientation.z = sin(pose_msg.theta() / 2.0);
+                pose_stamped.pose.orientation.w = cos(pose_msg.theta() / 2.0);
+
+                std::string topic=pose_msg.topic();
+                if(g_topic_map.find(topic)==g_topic_map.end())
+                {
+                    ros::Publisher* publisher=new ros::Publisher();
+                    *publisher = gp_nh->advertise<geometry_msgs::PoseStamped> (topic, 1);
+                    g_topic_map[topic]=publisher;
+                }
+                g_topic_map[topic]->publish(pose_stamped);
+
+            }
+            break;
+        }
+
+    }
+    //ros::spinOnce();
+    return SUCCESS;
+
+}
+
+void goalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
+{
+
+    double x=msg->pose.position.x;
+    double y=msg->pose.position.y;
+    double z=msg->pose.position.z;
+    double qx=msg->pose.orientation.x;
+    double qy=msg->pose.orientation.y;
+    double qz=msg->pose.orientation.z;
+    double qw=msg->pose.orientation.w;
+
+    std::cout<<"  post goal "<<qx<<","<<qy<<","<<qz<<","<<qw<<std::endl;
+    double theta=acos(qw)*2.0;
+    theta=(qz>0)?theta:-theta;
+
+    message::Goal_msg goal;
+    message::Base_info base_info;
+    base_info.set_sender(message::eEmpty);
+    base_info.set_receiver(message::eAGV);
+    base_info.set_msg_type(message::eGoal_msg);
+    goal.mutable_base_info()->CopyFrom(base_info);
+
+    goal.set_x(x);
+    goal.set_y(y);
+    goal.set_theta(theta);
+
+
+    Communication_message message;
+    message.reset(goal.base_info(),goal.SerializeAsString());
+    Message_communicator::get_instance_pointer()->send_msg(&message);
+    //发送目标点
+}
+
+int main(int argc,char** argv)
+{
+    init_glog();
+
+	ros::init(argc, argv, "agv_rviz");
+    ros::NodeHandle nh;
+    gp_nh=&nh;
+    subGoal = nh.subscribe("/move_base_simple/goal",1,goalCallback);//move_base_simple
+
+    Message_communicator::get_instance_pointer()->communication_connect("tcp://127.0.0.1:30001");
+    Message_communicator::get_instance_pointer()->set_ros_data_callback(Ros_data_callback);
+    Message_communicator::get_instance_pointer()->communication_run();
+
+
+    pcl::PointCloud<pcl::PointXYZ>    cloud;
+    for(int i=0;i<100;++i)
+    {
+        pcl::PointXYZ point(0.02*i,0,0);
+        cloud.push_back(point);
+    }
+
+    ros::spin();
+
+
+return 0;
+}
+
+
+
+GOOGLE_GLOG_DLL_DECL void shut_down_logging(const char* data, int size)
+{
+    time_t tt;
+    time( &tt );
+    tt = tt + 8*3600;  // transform the time zone
+    tm* t= gmtime( &tt );
+    char buf[255]={0};
+    sprintf(buf,"./%d%02d%02d-%02d%02d%02d-dump.txt",
+            t->tm_year + 1900,
+            t->tm_mon + 1,
+            t->tm_mday,
+            t->tm_hour,
+            t->tm_min,
+            t->tm_sec);
+
+    FILE* tp_file=fopen(buf,"w");
+    fprintf(tp_file,data,strlen(data));
+    fclose(tp_file);
+
+}
+void init_glog()
+{
+    time_t tt = time(0);//时间cuo
+    struct tm* t = localtime(&tt);
+
+    char strYear[255]={0};
+    char strMonth[255]={0};
+    char strDay[255]={0};
+
+    sprintf(strYear,"%04d", t->tm_year+1900);
+    sprintf(strMonth,"%02d", t->tm_mon+1);
+    sprintf(strDay,"%02d", t->tm_mday);
+
+    char buf[255]={0};
+    getcwd(buf,255);
+    char strdir[255]={0};
+    sprintf(strdir,"%s/log/%s/%s/%s", buf,strYear,strMonth,strDay);
+    PathCreator creator;
+    creator.Mkdir(strdir);
+
+    char logPath[255] = { 0 };
+    sprintf(logPath, "%s/", strdir);
+    FLAGS_max_log_size = 100;
+    FLAGS_logbufsecs = 0;
+    google::InitGoogleLogging("node");
+    google::SetStderrLogging(google::INFO);
+    google::SetLogDestination(0, logPath);
+    google::SetLogFilenameExtension("zxlog");
+    google::InstallFailureSignalHandler();
+    google::InstallFailureWriter(&shut_down_logging);
+    FLAGS_colorlogtostderr = true;        // Set log color
+    FLAGS_logbufsecs = 0;                // Set log output speed(s)
+    FLAGS_max_log_size = 1024;            // Set max log file size(GB)
+    FLAGS_stop_logging_if_full_disk = true;
+}

src/pkgname/src/message/message_base.proto

@@ -0,0 +1,169 @@
+syntax = "proto2";
+package message;
+
+//消息类型定义;每个在网络上传输的消息必须含有这个属性
+enum Message_type
+{
+    eBase_msg=0x00;
+    eCommand_msg=0x01;                      //指令消息
+
+    //本项目消息从 0xf0
+    eGoal_msg=0xf0;
+    ePointCloud_msg=0xf1;
+    ePose2d_msg=0xf2;
+
+
+}
+
+//通讯单元
+enum Communicator
+{
+    eEmpty=0x0000;
+    eMain=0x0001;    //主流程
+
+    eTerminor=0x0100;
+    //车位表
+    eParkspace=0x0200;
+    //测量单元
+    eMeasurer=0x0300;
+    //调度机构
+    eDispatch=0x0400;
+    //...
+
+    eAGV=0x0f00;
+
+}
+////base message 用于解析未知类型的消息
+message Base_info
+{
+    required Message_type               msg_type=1;
+    optional int32                      timeout_ms=2;
+    required Communicator               sender=3;                       //发送者
+    required Communicator               receiver=4;                     //接收者
+}
+
+// 事件,停车或者取车
+enum Process_type
+{
+    eStoring=1;
+    ePicking=2;
+}
+
+
+message Base_msg
+{
+    required Base_info                  base_info=1;
+}
+
+//错误等级，用来做故障处理
+enum Error_level
+{
+    NORMAL                = 0;      //    正常，没有错误，默认值0
+
+    NEGLIGIBLE_ERROR      = 1;      //    轻微故障;可忽略的故障，NEGLIGIBLE_ERROR
+
+    MINOR_ERROR           = 2;      //    一般故障，MINOR_ERROR
+
+    MAJOR_ERROR           = 3;      //    严重故障，MAJOR_ERROR
+
+    CRITICAL_ERROR        = 4;      //    致命故障，CRITICAL_ERROR
+
+}
+
+message Error_manager
+{
+    required int32                      error_code = 1;
+    optional Error_level                error_level = 2;
+    optional string                     error_description = 3;
+}
+
+//测量结果结构体
+message Locate_information
+{    
+    optional float locate_x = 1;				//整车的中心点x值; 四轮的中心
+    optional float locate_y = 2;				//整车的中心点y值; 四轮的中心
+    optional float locate_angle = 3;			//整车的旋转角; 四轮的旋转角
+    optional float locate_length = 4;		    //整车的长度; 用于规避碰撞
+    optional float locate_width = 5;			//整车的宽度; 用于规避碰撞
+    optional float locate_height = 6;		    //整车的高度; 用于规避碰撞
+    optional float locate_wheel_base = 7;	    //整车的轮距; 前后轮的距离; 用于机器人或agv的抓车
+    optional float locate_wheel_width = 8;	    //整车的轮距; 左右轮的距离; 用于机器人或agv的抓车
+    optional bool locate_correct = 9;		    //整车的校准标记位
+}
+
+//车辆基本信息
+message Car_info
+{
+    optional float                      car_length=1;           //车长
+    optional float                      car_width=2;            //车宽
+    optional float                      car_height=3;           //车高
+    optional string                     license=4;              //车辆凭证号
+}
+
+//车位状态枚举
+enum Parkspace_status
+{
+    eParkspace_empty            = 0;         //空闲，可分配
+    eParkspace_occupied         = 1;         //被占用，不可分配
+    eParkspace_reserverd        = 2;         //被预约，预约车辆可分配
+    eParkspace_error            = 3;         //车位机械结构或硬件故障
+}
+
+enum Direction
+{
+    eForward = 1;
+    eBackward = 2;
+}
+
+//单个车位基本信息与状态信息,车位信息以及车位上的车辆信息
+message Parkspace_info
+{
+    optional int32              parkspace_id=1;         //车位编号
+    optional int32              index=2;                //同层编号
+    optional Direction          direction=3;            //前后
+    optional int32              floor=4;                //楼层
+    optional float              length=5;               //车位长
+    optional float              width=6;                //车位宽
+    optional float              height=7;               //车位高
+    optional Parkspace_status   parkspace_status=8;     //车位当前状态
+    optional Car_info           car_info=9;              //当前车位存入车辆的凭证号
+    optional string             entry_time=10;          //入场时间
+    optional string             leave_time=11;          //离场时间
+}
+
+/*
+*流程中的步骤类型, 例如:停车流程包含5个步骤 , 分配车位-测量-检验结果-搬运-更新车位表
+*/
+enum Step_type
+{
+    eAlloc_step=0;
+    eMeasure_step=1;
+    eCompare_step=2;
+    eDispatch_step=3;
+    eConfirm_step=4;
+
+    eSearch_step=5;        //查询数据库
+    eWait_step=6;             //等待车辆离开
+    eRelease_step=7;          //释放车位
+
+    eComplete=8;              //完成
+
+    eBackConfirm_step=9;
+    eBack_compare_step=10;
+    eBackMeasure_step=11;
+    eBackAlloc_step=12;
+
+    eBackWait_step=13;
+    eBackDispatch_step=14;
+    eBackSearch_step=15;
+
+    eBackComplete=16;
+}
+//步骤状态,每个步骤有四中可能状态 ,等待中-执行中-完成或者错误  四个状态
+enum Step_statu
+{
+    eWaiting=0;               //空闲
+    eWorking=1;
+    eError=2;
+    eFinished=3;
+}

src/pkgname/src/message/ros_message.proto

@@ -0,0 +1,38 @@
+syntax = "proto2";
+package message;
+import "message_base.proto";
+
+message PointXYZ
+{
+    optional float x=1 [default=0];
+    optional float y=2 [default=0];
+    optional float z=3 [default=0];
+    optional float intensity=4 [default=0];
+}
+
+message PointCloud_msg
+{
+    required Base_info                  base_info=1;
+    required string                     frame_id=2;
+    required string                     topic=3;
+    repeated PointXYZ                   points=4;
+}
+
+message Pose2d_msg
+{
+    required Base_info                  base_info=1;
+    required string                     frame_id=2;
+    required string                     topic=3;
+    required float                      x=4 [default=0];
+    required float                      y=5 [default=0];
+    required float                      theta=6 [default=0];
+}
+
+
+message Goal_msg
+{
+    required Base_info                  base_info=1;
+    required float                      x=2 [default=0];
+    required float                      y=3 [default=0];
+    required float                      theta=4 [default=0];
+}

src/pkgname/src/proto.sh

@@ -0,0 +1,3 @@
+protoc -I=./communication communication.proto --cpp_out=./communication
+protoc -I=./message message_base.proto --cpp_out=./message
+protoc -I=./message ros_message.proto --cpp_out=./message

src/pkgname/src/tool/binary_buf.cpp

@@ -0,0 +1,341 @@
+
+/*
+ * binary_buf是二进制缓存
+ * 这里用字符串，来存储雷达的通信消息的原始数据
+ * Binary_buf 的内容格式：消息类型 + 消息数据
+ *
+ * 例如思科的雷达的消息类型
+ * ready->ready->start->data->data->data->stop->ready->ready
+ *
+ * 提供了 is_equal 系列的函数，来进行判断前面的消息类型
+ * 
+ * 注意了：m_buf是中间可以允许有‘\0’的，不是单纯的字符串格式
+ * 			末尾也不一定是‘\0’
+ */
+
+#include "binary_buf.h"
+
+#include <string>
+#include <string.h>
+
+Binary_buf::Binary_buf()
+{
+	mp_buf = NULL;
+	m_length = 0;
+}
+
+Binary_buf::Binary_buf(const Binary_buf& other)
+{
+	mp_buf = NULL;
+	m_length = 0;
+
+	if ( other.m_length > 0 && other.mp_buf != NULL)
+	{
+		mp_buf = (char*)malloc(other.m_length);
+		memcpy(mp_buf, other.mp_buf, other.m_length);
+		m_length = other.m_length;
+	}
+}
+
+Binary_buf::~Binary_buf()
+{
+	if ( mp_buf )
+	{
+		free(mp_buf);
+		mp_buf = NULL;
+	}
+	m_length = 0;
+
+//	std::cout << "Binary_buf::~Binary_buf()" << std::endl;
+}
+
+
+//使用参数构造，深拷贝，len为0时，使用strlen(buf)，不存储结束符'\0'
+Binary_buf::Binary_buf(const char* p_buf, int len)
+{
+	mp_buf = NULL;
+	m_length = 0;
+
+	if ( p_buf != NULL)
+	{
+		if (len <= 0)
+		{
+			len = strlen(p_buf);
+		}
+
+		mp_buf = (char*)malloc(len);
+		memcpy(mp_buf, p_buf, len);
+		m_length = len;
+	}
+}
+
+
+//使用参数构造，深拷贝，len为0时，使用strlen(buf)，不存储结束符'\0'
+Binary_buf::Binary_buf(char* p_buf, int len)
+{
+	mp_buf = NULL;
+	m_length = 0;
+
+	if ( p_buf != NULL)
+	{
+		if (len <= 0)
+		{
+			len = strlen(p_buf);
+		}
+
+		mp_buf = (char*)malloc(len);
+		memcpy(mp_buf, p_buf, len);
+		m_length = len;
+	}
+}
+
+//重载=，深拷贝，
+Binary_buf& Binary_buf::operator=(const Binary_buf& other)
+{
+	clear();
+
+	if ( other.m_length > 0 && other.mp_buf != NULL)
+	{
+		mp_buf = (char*)malloc(other.m_length);
+		memcpy(mp_buf, other.mp_buf, other.m_length);
+		m_length = other.m_length;
+	}
+	return *this;
+}
+
+//重载=，深拷贝，使用strlen(buf)，不存储结束符'\0'
+Binary_buf& Binary_buf::operator=(const char* p_buf)
+{
+	clear();
+
+	if ( p_buf != NULL)
+	{
+		int len = strlen(p_buf);
+		mp_buf = (char*)malloc(len);
+		memcpy(mp_buf, p_buf, len);
+		m_length = len;
+	}
+	return *this;
+}
+
+//重载+，other追加在this的后面，
+Binary_buf& Binary_buf::operator+(Binary_buf& other)
+{
+	if (other.mp_buf != NULL && other.m_length > 0)
+	{
+		int t_length_total = m_length + other.m_length;
+		char* tp_buf_total = (char*)malloc(t_length_total);
+		memcpy(tp_buf_total, mp_buf, m_length);
+		memcpy(tp_buf_total + m_length, other.mp_buf, other.m_length);
+		free(mp_buf);
+		mp_buf = tp_buf_total;
+		m_length = t_length_total;
+	}
+	return *this;
+}
+
+//重载+，追加在this的后面，使用strlen(buf)，不存储结束符'\0'
+Binary_buf& Binary_buf::operator+(const char* p_buf)
+{
+	if (p_buf != NULL )
+	{
+		int t_length_back = strlen(p_buf);
+		int t_length_total = m_length + t_length_back;
+		char* tp_buf_total = (char*)malloc(t_length_total);
+		memcpy(tp_buf_total, mp_buf, m_length);
+		memcpy(tp_buf_total + m_length, p_buf, t_length_back);
+		free(mp_buf);
+		mp_buf = tp_buf_total;
+		m_length = t_length_total;
+	}
+	return *this;
+}
+
+//重载[]，允许直接使用数组的形式，直接访问buf的内存。注意，n值必须在0～m_length之间，
+char& Binary_buf::operator[](int n)
+{
+	if (n >= 0 && n < m_length)
+	{
+		return mp_buf[n];
+	}
+	else
+	{
+		throw (n);
+	}
+}
+
+
+//判空
+bool Binary_buf::is_empty()
+{
+	if ( mp_buf != NULL && m_length > 0 )
+	{
+		return false;
+	}
+	else
+	{
+		return true;
+	}
+}
+
+//清空
+void Binary_buf::clear()
+{
+	if ( mp_buf )
+	{
+		free(mp_buf);
+		mp_buf = NULL;
+	}
+	m_length = 0;
+}
+
+
+//比较前面部分的buf是否相等，使用 other.m_length 为标准
+bool Binary_buf::is_equal_front(const Binary_buf& other)
+{
+	if ( other.mp_buf == NULL || other.m_length <= 0 )
+	{
+		if ( mp_buf == NULL || m_length <= 0 )
+		{
+			return true;
+		}
+		else
+		{
+			return false;
+		}
+	}
+	else
+	{
+		if ( mp_buf != NULL && m_length > 0 )
+		{
+			if ( other.m_length > m_length )
+			{
+				return false;
+			}
+			return  (strncmp((const char*)mp_buf, other.mp_buf, other.m_length) == 0);
+		}
+		else
+		{
+			return false;
+		}
+
+	}
+}
+
+//比较前面部分的buf是否相等，len为0时，使用strlen(buf)为标准，不比较结束符'\0'
+bool Binary_buf::is_equal_front(const char* p_buf, int len)
+{
+	if ( p_buf == NULL )
+	{
+		if ( mp_buf == NULL || m_length <= 0 )
+		{
+			return true;
+		}
+		else
+		{
+			return false;
+		}
+	}
+	else
+	{
+		if ( mp_buf != NULL && m_length > 0 )
+		{
+			if ( len == 0 )
+			{
+				len = strlen(p_buf);
+			}
+			if ( len > m_length )
+			{
+				return false;
+			}
+			return  (strncmp((const char*)mp_buf, p_buf, len) == 0);
+		}
+		else
+		{
+			return false;
+		}
+
+	}
+}
+
+//比较的buf是否全部相等，
+bool Binary_buf::is_equal_all(const Binary_buf& other)
+{
+	if ( other.mp_buf == NULL || other.m_length <= 0 )
+	{
+		if ( mp_buf == NULL || m_length <= 0 )
+		{
+			return true;
+		}
+		else
+		{
+			return false;
+		}
+	}
+	else
+	{
+		if ( mp_buf != NULL && m_length > 0 )
+		{
+			if ( other.m_length != m_length )
+			{
+				return false;
+			}
+			return  (strncmp((const char*)mp_buf, other.mp_buf, other.m_length) == 0);
+		}
+		else
+		{
+			return false;
+		}
+
+	}
+}
+//比较的buf是否全部相等，不比较结束符'\0'
+bool Binary_buf::is_equal_all(const char* p_buf)
+{
+	if ( p_buf == NULL )
+	{
+		if ( mp_buf == NULL || m_length <= 0 )
+		{
+			return true;
+		}
+		else
+		{
+			return false;
+		}
+	}
+	else
+	{
+		if ( mp_buf != NULL && m_length > 0 )
+		{
+			int	len = strlen(p_buf);
+			if ( len != m_length )
+			{
+				return false;
+			}
+			return  (strncmp((const char*)mp_buf, p_buf, len) == 0);
+		}
+		else
+		{
+			return false;
+		}
+
+	}
+}
+
+
+
+
+char*	Binary_buf::get_buf()const
+{
+	return mp_buf;
+}
+
+int		Binary_buf::get_length()const
+{
+	return m_length;
+}
+
+
+
+
+

src/pkgname/src/tool/binary_buf.h

@@ -0,0 +1,91 @@
+
+/*
+ * binary_buf是二进制缓存
+ * 这里用字符串，来存储雷达的通信消息的原始数据
+ * Binary_buf 的内容格式：消息类型 + 消息数据
+ *
+ * 例如思科的雷达的消息类型
+ * ready->ready->start->data->data->data->stop->ready->ready
+ *
+ * 提供了 is_equal 系列的函数，来进行判断前面的消息类型
+ *
+ * 注意了：m_buf是中间可以允许有‘\0’的，不是单纯的字符串格式
+ * 			末尾也不一定是‘\0’
+ */
+
+#ifndef LIDARMEASURE_BINARY_BUF_H
+#define LIDARMEASURE_BINARY_BUF_H
+#include <iostream>
+
+
+//雷达消息的类型
+//在通信消息的前面一部分字符串，表示这条消息的类型。
+//在解析消息的时候，先解析前面的消息类型，来判断这条消息的功用
+enum Buf_type
+{
+	//默认值 BUF_UNKNOW = 0
+	BUF_UNKNOW   		=0,	//未知消息
+	BUF_READY  			=1,	//待机消息
+	BUF_START 			=2,	//开始消息
+	BUF_DATA   			=3,	//数据消息
+	BUF_STOP  			=4,	//结束消息
+	BUF_ERROR   		=5,	//错误消息
+};
+
+
+//二进制缓存，
+class Binary_buf
+{
+public:
+	Binary_buf();
+	Binary_buf(const Binary_buf& other);
+	~Binary_buf();
+
+	//使用参数构造，深拷贝，len为0时，使用strlen(buf)，不存储结束符'\0'
+	Binary_buf(const char* p_buf, int len = 0);
+	//使用参数构造，深拷贝，len为0时，使用strlen(buf)，不存储结束符'\0'
+	Binary_buf(char* p_buf, int len = 0);
+	//重载=，深拷贝，
+	Binary_buf& operator=(const Binary_buf& other);
+	//重载=，深拷贝，使用strlen(buf)，不存储结束符'\0'
+	Binary_buf& operator=(const char* p_buf);
+	//重载+，other追加在this的后面，
+	Binary_buf& operator+(Binary_buf& other);
+	//重载+，追加在this的后面，使用strlen(buf)，不存储结束符'\0'
+	Binary_buf& operator+(const char* p_buf);
+	//重载[]，允许直接使用数组的形式，直接访问buf的内存。注意，n值必须在0～m_length之间，
+	char& operator[](int n);
+
+	//判空
+	bool is_empty();
+	//清空
+	void clear();
+
+	//比较前面部分的buf是否相等，使用 other.m_length 为标准
+	bool is_equal_front(const Binary_buf& other);
+	//比较前面部分的buf是否相等，len为0时，使用strlen(buf)为标准，不比较结束符'\0'
+	bool is_equal_front(const char* p_buf, int len = 0);
+
+	//比较的buf是否全部相等，
+	bool is_equal_all(const Binary_buf& other);
+	//比较的buf是否全部相等，不比较结束符'\0'
+	bool is_equal_all(const char* p_buf);
+
+
+
+public:
+	char* get_buf()const;
+	int	get_length()const;
+
+protected:
+	char*		mp_buf;				//二进制缓存指针
+	int			m_length;			//二进制缓存长度
+
+private:
+
+};
+
+
+
+
+#endif //LIDARMEASURE_BINARY_BUF_H

src/pkgname/src/tool/binary_buf.puml

@@ -0,0 +1,85 @@
+@startuml
+skinparam classAttributeIconSize 0
+
+title  binary_buf是二进制缓存
+
+note left of Binary_buf
+/*
+ * binary_buf是二进制缓存
+ * 这里用字符串，来存储雷达的通信消息的原始数据
+ * Binary_buf 的内容格式：消息类型 + 消息数据
+ *
+ * 例如思科的雷达的消息类型
+ * ready->ready->start->data->data->data->stop->ready->ready
+ *
+ * 提供了 is_equal 系列的函数，来进行判断前面的消息类型
+ *
+ * 注意了：m_buf是中间可以允许有‘\0’的，不是单纯的字符串格式
+ * 			末尾也不一定是‘\0’
+ */
+end note
+
+
+
+enum Buf_type
+{
+//雷达消息的类型
+//在通信消息的前面一部分字符串，表示这条消息的类型。
+//在解析消息的时候，先解析前面的消息类型，来判断这条消息的功用
+	//默认值 BUF_UNKNOW = 0
+	BUF_UNKNOW   		=0,	//未知消息
+	BUF_READY  			=1,	//待机消息
+	BUF_START 			=2,	//开始消息
+	BUF_DATA   			=3,	//数据消息
+	BUF_STOP  			=4,	//结束消息
+	BUF_ERROR   		=5,	//错误消息
+}
+
+
+
+class Binary_buf
+{
+//二进制缓存，
+==public:==
+	Binary_buf();
+	Binary_buf(const Binary_buf& other);
+	~Binary_buf();
+..
+	//使用参数构造，深拷贝，len为0时，使用strlen(buf)，不存储结束符'\0'
+	Binary_buf(const char* p_buf, int len = 0);
+	//重载=，深拷贝，
+	Binary_buf& operator=(const Binary_buf& other);
+	//重载=，深拷贝，使用strlen(buf)，不存储结束符'\0'
+	Binary_buf& operator=(const char* p_buf);
+	//重载+，other追加在this的后面，
+	Binary_buf& operator+(Binary_buf& other);
+	//重载+，追加在this的后面，使用strlen(buf)，不存储结束符'\0'
+	Binary_buf& operator+(const char* p_buf);
+	//重载[]，允许直接使用数组的形式，直接访问buf的内存。注意，n值必须在0～m_length之间，
+	char& operator[](int n);
+..
+	//判空
+	bool is_empty();
+	//清空
+	void clear();
+..
+	//比较前面部分的buf是否相等，使用 other.m_length 为标准
+	bool is_equal_front(const Binary_buf& other);
+	//比较前面部分的buf是否相等，len为0时，使用strlen(buf)为标准，不比较结束符'\0'
+	bool is_equal_front(const char* p_buf, int len = 0);
+
+	//比较的buf是否全部相等，
+	bool is_equal_all(const Binary_buf& other);
+	//比较的buf是否全部相等，不比较结束符'\0'
+	bool is_equal_all(const char* p_buf);
+==public:==
+	char* get_buf()const;
+	int	get_length()const;
+==protected:==
+	char*		mp_buf;				//二进制缓存指针
+	int			m_length;			//二进制缓存长度
+==private:==
+}
+
+
+@enduml

src/pkgname/src/tool/pathcreator.cpp

@@ -0,0 +1,94 @@
+#include "pathcreator.h"
+
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <time.h>
+#include <stdint.h>
+#include <stdio.h>
+
+PathCreator::PathCreator()
+{
+
+}
+
+PathCreator::~PathCreator()
+{
+
+}
+
+std::string PathCreator::GetCurPath()
+{
+    return m_current_path;
+}
+bool PathCreator::Mkdir(std::string dirName)
+{
+    uint32_t beginCmpPath = 0;
+    uint32_t endCmpPath = 0;
+    std::string fullPath = "";
+
+    if('/' != dirName[0])
+    {
+        fullPath = getcwd(nullptr, 0);
+        beginCmpPath = fullPath.size();
+        fullPath = fullPath + "/" + dirName;
+    }
+    else
+    {
+        //Absolute path
+        fullPath = dirName;
+        beginCmpPath = 1;
+    }
+    if (fullPath[fullPath.size() - 1] != '/')
+    {
+        fullPath += "/";
+    }
+    endCmpPath = fullPath.size();
+
+    //create dirs;
+    for(uint32_t i = beginCmpPath; i < endCmpPath ; i++ )
+    {
+        if('/' == fullPath[i])
+        {
+            std::string curPath = fullPath.substr(0, i);
+            if(access(curPath.c_str(), F_OK) != 0)
+            {
+                if(mkdir(curPath.c_str(), /*S_IRUSR|S_IRGRP|S_IROTH|S_IWUSR|S_IWGRP|S_IWOTH*/0777) == -1)
+                {
+                    printf("mkdir(%s) failed\n", curPath.c_str());
+                    return false;
+                }
+            }
+        }
+    }
+    m_current_path=fullPath;
+    return true;
+
+}
+
+bool PathCreator::CreateDatePath(std::string root, bool add_time)
+{
+    time_t tt;
+    time( &tt );
+    tt = tt + 8*3600;  // transform the time zone
+    tm* t= gmtime( &tt );
+    char buf[255]={0};
+    if (add_time)
+    {
+        sprintf(buf, "%s/%d%02d%02d-%02d%02d%02d", root.c_str(),
+                t->tm_year + 1900,
+                t->tm_mon + 1,
+                t->tm_mday,
+                t->tm_hour,
+                t->tm_min,
+                t->tm_sec);
+    }
+    else
+    {
+        sprintf(buf, "%s/%d%02d%02d", root.c_str(),
+                t->tm_year + 1900,
+                t->tm_mon + 1,
+                t->tm_mday);
+    }
+    return Mkdir(buf);
+}

src/pkgname/src/tool/pathcreator.h

@@ -0,0 +1,18 @@
+#ifndef PATHCREATOR_H
+#define PATHCREATOR_H
+#include <string>
+
+class PathCreator
+{
+public:
+    PathCreator();
+    ~PathCreator();
+    std::string GetCurPath();
+    bool Mkdir(std::string dir);
+    bool CreateDatePath(std::string root, bool add_time = true);
+protected:
+    std::string m_current_path;
+};
+
+
+#endif // PATHCREATOR_H

src/pkgname/src/tool/pcl_cloud_with_lock.h

@@ -0,0 +1,30 @@
+
+
+
+#ifndef PCL_CLOUD_WITH_LOCK_H
+#define PCL_CLOUD_WITH_LOCK_H
+
+
+class Pcl_cloud_with_lock
+{
+public:
+	Pcl_cloud_with_lock();
+	Pcl_cloud_with_lock(const Pcl_cloud_with_lock& other);
+	~Pcl_cloud_with_lock();
+public://API functions
+
+public://get or set member variable
+
+
+protected://member variable
+//	//三维点云的数据保护锁，
+//	std::mutex*                     mp_task_cloud_lock;
+//	//三维点云容器的智能指针，这里不直接分配内存,
+//	pcl::PointCloud<pcl::PointXYZ>::Ptr        mp_task_point_cloud;
+
+private:
+
+};
+
+
+#endif //PCL_CLOUD_WITH_LOCK_H

src/pkgname/src/tool/pose2d.cpp

@@ -0,0 +1,30 @@
+//
+// Created by zx on 2020/9/9.
+//
+
+#include "pose2d.h"
+#include <math.h>
+Pose2d::Pose2d()
+    :m_x(0),m_y(0),m_theta(0)
+{
+}
+Pose2d::Pose2d(float x,float y,float theta)
+    :m_x(x),m_y(y),m_theta(theta)
+{
+}
+
+Pose2d::~Pose2d()
+{}
+
+std::ostream& operator<<(std::ostream &out,const Pose2d& point)
+{
+    out<<"[x:"<<point.x()<<", y:"<<point.y()<<", theta:"<<point.theta()<<"]";
+    return out;
+}
+float Pose2d::gridient() const
+{
+    double gradient=tanf(m_theta);
+    if(fabs(gradient)>200)
+        return 200.0*(gradient/fabs(gradient));
+    return gradient;
+}

src/pkgname/src/tool/pose2d.h

@@ -0,0 +1,49 @@
+//
+// Created by zx on 2020/9/9.
+//
+
+#ifndef CAN_TEST_TRAJECTORY_Pose2d_H_
+#define CAN_TEST_TRAJECTORY_Pose2d_H_
+#include <iostream>
+#include <atomic>
+/*
+ * 带direction的二维点,与x轴正方向逆时针为正方向
+ */
+
+class Pose2d
+{
+ public:
+    Pose2d();
+    Pose2d(float x,float y,float theta);
+    Pose2d(const Pose2d& point)
+    {
+        m_x=point.x();
+        m_y=point.y();
+        m_theta=point.theta();
+    }
+    ~Pose2d();
+    Pose2d& operator=(const Pose2d& point)
+    {
+        m_x=point.x();
+        m_y=point.y();
+        m_theta=point.theta();
+        return *this;
+    }
+
+    friend std::ostream& operator<<(std::ostream &out,const Pose2d& point);
+
+
+    float x() const {return m_x;};
+    float y() const {return m_y;};
+    float theta() const {return m_theta;}
+
+    float gridient()const;
+
+ protected:
+    std::atomic<float>                      m_x;
+    std::atomic<float>                      m_y;
+    std::atomic<float>                      m_theta;        //梯度角，与x轴正方形逆时针为正，单位弧度
+};
+
+
+#endif //CAN_TEST_TRAJECTORY_Pose2d_H_

src/pkgname/src/tool/proto_tool.cpp

@@ -0,0 +1,42 @@
+
+
+
+#include "proto_tool.h"
+#include <fcntl.h>
+#include<unistd.h>
+#include <google/protobuf/io/zero_copy_stream_impl.h>
+#include <google/protobuf/text_format.h>
+using google::protobuf::io::FileInputStream;
+using google::protobuf::io::FileOutputStream;
+using google::protobuf::io::ZeroCopyInputStream;
+using google::protobuf::io::CodedInputStream;
+using google::protobuf::io::ZeroCopyOutputStream;
+using google::protobuf::io::CodedOutputStream;
+using google::protobuf::Message;
+
+
+//读取protobuf 配置文件，转化为protobuf参数形式
+//input：	prototxt_path ：prototxt文件路径
+//ouput：	parameter: protobuf参数，这里是消息基类，实际调用时传入对应的子类即可。
+bool proto_tool::read_proto_param(std::string prototxt_path, ::google::protobuf::Message& protobuf_parameter)
+{
+	int fd = open(prototxt_path.c_str(), O_RDONLY);
+	if (fd == -1) return false;
+	FileInputStream* input = new FileInputStream(fd);
+	bool success = google::protobuf::TextFormat::Parse(input, &protobuf_parameter);
+	delete input;
+	close(fd);
+	return success;
+}
+
+
+
+
+
+
+
+
+
+
+
+

src/pkgname/src/tool/proto_tool.h

@@ -0,0 +1,56 @@
+
+
+
+
+
+#ifndef __PROTO_TOOL_H
+#define __PROTO_TOOL_H
+
+#include "../tool/singleton.h"
+#include <istream>
+#include <google/protobuf/message.h>
+
+class proto_tool:public Singleton<proto_tool>
+{
+	// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+	friend class Singleton<proto_tool>;
+public:
+	// 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+	proto_tool(const proto_tool&)=delete;
+	proto_tool& operator =(const proto_tool&)= delete;
+	~proto_tool()=default;
+private:
+	// 父类的构造函数必须保护，子类的构造函数必须私有。
+	proto_tool()=default;
+
+
+public:
+	//读取protobuf 配置文件，转化为protobuf参数形式
+	//input：	prototxt_path ：prototxt文件路径
+	//ouput：	parameter: protobuf参数，这里是消息基类，实际调用时传入对应的子类即可。
+	static bool read_proto_param(std::string prototxt_path, ::google::protobuf::Message& protobuf_parameter);
+};
+
+
+
+
+#endif //__PROTO_TOOL_H
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

src/pkgname/src/tool/singleton.cpp

@@ -0,0 +1,4 @@
+
+#include "singleton.h"
+
+

src/pkgname/src/tool/singleton.h

@@ -0,0 +1,81 @@
+
+/* Singleton 是单例类的模板。
+ * https://www.cnblogs.com/sunchaothu/p/10389842.html
+ * 单例 Singleton 是设计模式的一种，其特点是只提供唯一一个类的实例,具有全局变量的特点，在任何位置都可以通过接口获取到那个唯一实例;
+ * 全局只有一个实例：static 特性，同时禁止用户自己声明并定义实例（把构造函数设为 private 或者 protected）
+ * Singleton 模板类对这种方法进行了一层封装。
+ * 单例类需要从Singleton继承。
+ * 子类需要将自己作为模板参数T 传递给 Singleton<T> 模板;
+ * 同时需要将基类声明为友元，这样Singleton才能调用子类的私有构造函数。
+// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+// 父类的构造函数必须保护，子类的构造函数必须私有。
+// 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来操作 唯一的实例。
+ * */
+
+#ifndef __SINGLIETON_H
+#define __SINGLIETON_H
+
+//#include <iostream>
+
+template<typename T>
+class Singleton
+{
+public:
+	//获取单例的引用
+	static T& get_instance_references()
+	{
+		static T instance;
+		return instance;
+	}
+	//获取单例的指针
+	static T* get_instance_pointer()
+	{
+		return &(get_instance_references());
+	}
+
+	virtual ~Singleton()
+	{
+//		std::cout<<"destructor called!"<<std::endl;
+	}
+
+	Singleton(const Singleton&)=delete;					//关闭拷贝构造函数
+	Singleton& operator =(const Singleton&)=delete;		//关闭赋值函数
+
+protected:
+	//构造函数需要是 protected，这样子类才能继承；
+	Singleton()
+	{
+//		std::cout<<"constructor called!"<<std::endl;
+	}
+
+};
+
+/*
+// 如下是 使用样例：
+// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+// 父类的构造函数必须保护，子类的构造函数必须私有。
+// 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+
+class DerivedSingle:public Singleton<DerivedSingle>
+{
+ // 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+	friend class Singleton<DerivedSingle>;
+public:
+ // 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+	DerivedSingle(const DerivedSingle&)=delete;
+	DerivedSingle& operator =(const DerivedSingle&)= delete;
+ 	~DerivedSingle()=default;
+private:
+ // 父类的构造函数必须保护，子类的构造函数必须私有。
+	DerivedSingle()=default;
+};
+
+int main(int argc, char* argv[]){
+	DerivedSingle& instance1 = DerivedSingle::get_instance_references();
+	DerivedSingle* p_instance2 = DerivedSingle::get_instance_pointer();
+	return 0;
+}
+
+ */
+
+#endif

src/pkgname/src/tool/thread_condition.cpp

@@ -0,0 +1,175 @@
+
+
+/* Thread_condition 是多线程的条件控制类，主要是控制线程的启停和退出
+ * 线程创建后，一般是循环运行，
+ * 为了防止线程暂满整个cpu，那么需要线程在不工作的是否进入等待状态。
+ * Thread_condition 就可以控制线程的运行状态。
+ *
+	std::atomic<bool> m_pass_ever		//线程能否直接通过等待，对后面的线程也生效。
+	std::atomic<bool> m_pass_once		//线程能否直接通过等待，一次（通过一次之后，wait里面自动改为false）
+ * 外部调用notify系列的函数，唤醒等待的线程，让线程执行功能函数。
+ * 如果需要线程循环多次执行功能函数，那么就使用 notify_all(true)，后面的线程可以直接通过等待了。
+ * 再使用 notify_all(false) ，即可停止线程，让其继续等待。
+ * 如果只想要线程执行一次，那就使用 notify_all(false, true)
+ * 注：notify_all(false, true)和notify_one(false, true) 一样，只能让其中一个线程执行一次
+ *
+ * m_kill_flag //是否杀死线程，让线程强制退出，
+ * 外部调用 kill_all() 函数，可以直接通知线程自动退出。
+	//杀死所有的线程，强制退出线程函数，只是操作受当前Thread_condition影响的所有线程
+	//唤醒所有线程，使其通过等待，但是不能运行功能函数，必须直接return
+	// 注：只是修改m_kill为true，需要线程函数实时检测kill的状态，来return线程。
+	//		通过等待之后，也要检查kill的状态，如果为真，那么就不能执行功能函数，应该直接return
+
+ 注：notify唤醒线程之后，wait里面的判断函数会重新判断。
+ */
+
+#include "thread_condition.h"
+
+Thread_condition::Thread_condition()
+{
+	m_kill_flag = false;
+	m_pass_ever = false;
+	m_pass_once = false;
+	m_working_flag = false;
+}
+Thread_condition::~Thread_condition()
+{
+	kill_all();
+}
+
+//无限等待，由 is_pass_wait 决定是否阻塞。
+//返回m_pass，
+bool Thread_condition::wait()
+{
+	std::unique_lock<std::mutex> loc(m_mutex);
+	m_condition_variable.wait(loc,std::bind(is_pass_wait,this));
+	bool t_pass = is_pass_wait(this);
+	m_pass_once = false;
+
+	//只要前面通过了, 那就进入工作状态
+	m_working_flag = true;
+
+	return t_pass;
+}
+//等待一定的时间（默认时间单位：毫秒ms），由 is_pass_wait 决定是否阻塞。
+//return：is_pass_wait的结果，	true：线程直接通过等待，false：线程超时了，然后通过等待。
+//注意了：线程阻塞期间，是不会return的。
+bool Thread_condition::wait_for_millisecond(unsigned int millisecond)
+{
+	std::unique_lock<std::mutex> loc(m_mutex);
+	m_condition_variable.wait_for(loc, std::chrono::milliseconds(millisecond), std::bind(is_pass_wait, this));
+	bool t_pass = is_pass_wait(this);
+	m_pass_once = false;
+
+	//只要前面通过了, 那就进入工作状态 , 超时通过也算通过
+	m_working_flag = true;
+
+	return t_pass;
+}
+
+
+//唤醒已经阻塞的线程，唤醒一个线程
+//pass_ever 或者 pass_once 为真时，才能唤醒线程。都为假时，线程进入等待。
+void Thread_condition::notify_one(bool pass_ever, bool pass_once)
+{
+	std::unique_lock<std::mutex> loc(m_mutex);
+	m_pass_ever = pass_ever;
+	m_pass_once = pass_once;
+	m_condition_variable.notify_one();
+}
+//唤醒已经阻塞的线程，唤醒全部线程
+//pass_ever 或者 pass_once 为真时，才能唤醒线程。都为假时，线程进入等待。
+void Thread_condition::notify_all(bool pass_ever, bool pass_once)
+{
+	std::unique_lock<std::mutex> loc(m_mutex);
+	m_pass_ever = pass_ever;
+	m_pass_once = pass_once;
+	m_condition_variable.notify_all();
+}
+//注：notify_all(false, true)和notify_one(false, true) 一样，只能让其中一个线程执行一次
+
+
+//杀死所有的线程，强制退出线程函数，只是操作受当前Thread_condition影响的所有线程
+//唤醒所有线程，使其通过等待，但是不能运行功能函数，必须直接return
+// 注：只是修改m_kill为true，需要线程函数实时检测kill的状态，来return线程。
+//		通过等待之后，也要检查kill的状态，如果为真，那么就不能执行功能函数，应该直接return
+void Thread_condition::kill_all()
+{
+	std::unique_lock<std::mutex> loc(m_mutex);
+	m_kill_flag = true;
+	m_condition_variable.notify_all();
+}
+
+//判断是否存活，只有活着才能继续支持子线程从功能函数，否则需要强制退出函数并结束子线程
+bool Thread_condition::is_alive()
+{
+	return !m_kill_flag;
+}
+
+
+//判断是否等待, 外部线程通过这个函数来查询this线程的工作状态,
+bool Thread_condition::is_waiting()
+{
+	return !m_working_flag;
+}
+//判断是否工作, 外部线程通过这个函数来查询this线程的工作状态,
+bool Thread_condition::is_working()
+{
+	return m_working_flag;
+}
+
+
+bool Thread_condition::get_kill_flag()
+{
+	return m_kill_flag;
+}
+bool Thread_condition::get_pass_ever()
+{
+	return m_pass_ever;
+}
+bool Thread_condition::get_pass_once()
+{
+	return m_pass_once;
+}
+void Thread_condition::set_kill_flag(bool kill)
+{
+	m_kill_flag = kill;
+}
+void Thread_condition::set_pass_ever(bool pass_ever)
+{
+	m_pass_ever = pass_ever;
+}
+void Thread_condition::set_pass_once(bool pass_once)
+{
+	m_pass_once = pass_once;
+}
+void Thread_condition::reset(bool kill, bool pass_ever, bool pass_once)
+{
+	m_kill_flag = kill;
+	m_pass_ever = pass_ever;
+	m_pass_once = pass_once;
+}
+
+
+//判断线程是否可以通过等待，wait系列函数的判断标志
+//注：m_kill或者m_pass为真时，return true
+bool Thread_condition::is_pass_wait(Thread_condition * other)
+{
+	if ( other == NULL )
+	{
+		throw (other);
+		return false;
+	}
+
+	bool result = (other->m_kill_flag || other->m_pass_ever || other->m_pass_once);
+
+	//如果不能通过等待, 那么线程状态改为等待中,
+	if ( !result )
+	{
+		other->m_working_flag = false;
+	}
+
+
+	return result;
+}
+

src/pkgname/src/tool/thread_condition.h

@@ -0,0 +1,183 @@
+
+
+/* Thread_condition 是多线程的条件控制类，主要是控制线程的启停和退出
+ * 线程创建后，一般是循环运行，
+ * 为了防止线程暂满整个cpu，那么需要线程在不工作的是否进入等待状态。
+ * Thread_condition 就可以控制线程的运行状态。
+ *
+	std::atomic<bool> m_pass_ever		//线程能否直接通过等待，对后面的线程也生效。
+	std::atomic<bool> m_pass_once		//线程能否直接通过等待，一次（通过一次之后，wait里面自动改为false）
+ * 外部调用notify系列的函数，唤醒等待的线程，让线程执行功能函数。
+ * 如果需要线程循环多次执行功能函数，那么就使用 notify_all(true)，后面的线程可以直接通过等待了。
+ * 再使用 notify_all(false) ，即可停止线程，让其继续等待。
+ * 如果只想要线程执行一次，那就使用 notify_all(false, true)
+ * 注：notify_all(false, true)和notify_one(false, true) 一样，只能让其中一个线程执行一次
+ *
+ * m_kill_flag //是否杀死线程，让线程强制退出，
+ * 外部调用 kill_all() 函数，可以直接通知线程自动退出。
+	//杀死所有的线程，强制退出线程函数，只是操作受当前Thread_condition影响的所有线程
+	//唤醒所有线程，使其通过等待，但是不能运行功能函数，必须直接return
+	// 注：只是修改m_kill为true，需要线程函数实时检测kill的状态，来return线程。
+	//		通过等待之后，也要检查kill的状态，如果为真，那么就不能执行功能函数，应该直接return
+
+ 注：notify唤醒线程之后，wait里面的判断函数会重新判断。
+
+
+ 最下面有使用样例,
+
+ */
+
+#ifndef LIDARMEASURE_THREAD_CONDITION_H
+#define LIDARMEASURE_THREAD_CONDITION_H
+
+#include <ratio>
+#include <chrono>
+#include <thread>
+#include <atomic>
+#include <mutex>
+#include <condition_variable>
+#include <functional>
+
+class Thread_condition
+{
+public:
+	Thread_condition();
+	Thread_condition(const Thread_condition& other) = delete;
+	~Thread_condition();
+
+	//无限等待，由 is_pass_wait 决定是否阻塞。
+	//返回m_pass，
+	bool wait();
+	//等待一定的时间（默认时间单位：毫秒ms），由 is_pass_wait 决定是否阻塞。
+	//return：is_pass_wait的结果，	true：线程直接通过等待，false：线程超时了，然后通过等待。
+	//注意了：线程阻塞期间，是不会return的。
+	bool wait_for_millisecond(unsigned int millisecond);
+
+	//等待一定的时间（时间单位可调），由 is_pass_wait 决定是否阻塞。
+	//return：is_pass_wait的结果，	true：线程直接通过等待，false：线程超时了，然后通过等待。
+	//注意了：线程阻塞期间，是不会return的。
+	template<typename _Rep, typename _Period>
+	bool wait_for_ex(const std::chrono::duration<_Rep, _Period>& time_duration);
+
+	//唤醒已经阻塞的线程，唤醒一个线程
+	//pass_ever 或者 pass_once 为真时，才能唤醒线程。都为假时，线程进入等待。
+	void notify_one(bool pass_ever, bool pass_once = false);
+	//唤醒已经阻塞的线程，唤醒全部线程
+	//pass_ever 或者 pass_once 为真时，才能唤醒线程。都为假时，线程进入等待。
+	void notify_all(bool pass_ever, bool pass_once = false);
+	//注：notify_all(false, true)和notify_one(false, true) 一样，只能让其中一个线程执行一次
+
+	//杀死所有的线程，强制退出线程函数，只是操作受当前Thread_condition影响的所有线程
+	//唤醒所有线程，使其通过等待，但是不能运行功能函数，必须直接return
+	// 注：只是修改m_kill为true，需要线程函数实时检测kill的状态，来return线程。
+	//		通过等待之后，也要检查kill的状态，如果为真，那么就不能执行功能函数，应该直接return
+	void kill_all();
+
+	//判断是否存活，只有活着才能继续支持子线程从功能函数，否则需要强制退出函数并结束子线程
+	bool is_alive();
+
+	//判断是否等待, 外部线程通过这个函数来查询this线程的工作状态,
+	bool is_waiting();
+	//判断是否工作, 外部线程通过这个函数来查询this线程的工作状态,
+	bool is_working();
+
+public:
+
+	bool get_kill_flag();
+	bool get_pass_ever();
+	bool get_pass_once();
+	void set_kill_flag(bool kill);
+	void set_pass_ever(bool pass_ever);
+	void set_pass_once(bool pass_once);
+	void reset(bool kill = false, bool pass_ever = false, bool pass_once = false);
+
+protected:
+	//判断线程是否可以通过等待，wait系列函数的判断标志
+	//注：m_kill或者m_pass为真时，return true
+	static bool is_pass_wait(Thread_condition * other);
+
+	std::atomic<bool> 		m_kill_flag;			//是否杀死线程，让线程强制退出，
+	std::atomic<bool> 		m_pass_ever;			//线程能否直接通过等待，对后面的线程也生效。
+	std::atomic<bool> 		m_pass_once;			//线程能否直接通过等待，一次（通过一次之后，wait里面自动改为false）
+
+	std::atomic<bool> 		m_working_flag;			//线程是否进入工作的标志位, false:表示线程进行进入wait等待, true:表示线程仍然在运行中,
+
+	std::mutex 				m_mutex;				//线程的锁
+	std::condition_variable m_condition_variable;	//线程的条件变量
+
+private:
+
+};
+
+//等待一定的时间（时间单位可调），由 is_pass_wait 决定是否阻塞。
+//return：is_pass_wait的结果，	true：线程直接通过等待，false：线程超时了，然后通过等待。
+//注意了：线程阻塞期间，是不会return的。
+template<typename _Rep, typename _Period>
+bool Thread_condition::wait_for_ex(const std::chrono::duration<_Rep, _Period>& time_duration)
+{
+	std::unique_lock<std::mutex> loc(m_mutex);
+	m_condition_variable.wait_for(loc, std::chrono::duration<_Rep, _Period>(time_duration), std::bind(is_pass_wait, this));
+	bool t_pass = is_pass_wait(this);
+	m_pass_once = false;
+	return t_pass;
+}
+
+#endif //LIDARMEASURE_THREAD_CONDITION_H
+
+
+
+
+
+/*
+//使用样例:
+std::thread*						mp_thread_receive;    		//接受缓存的线程指针
+Thread_condition					m_condition_receive;		//接受缓存的条件变量
+
+void thread_receive()
+{
+	while (m_condition_receive.is_alive())
+	{
+		m_condition_receive.wait();
+		if ( m_condition_receive.is_alive() )
+		{
+			//do everything
+
+		}
+	}
+
+	return;
+}
+
+//main函数的主线程
+int main(int argc,char* argv[])
+{
+ 	//线程创建之后, 默认等待
+	m_condition_receive.reset(false, false, false);
+	mp_thread_receive = new std::thread(& thread_receive );
+
+
+	//唤醒所有线程, 然后线程可以一直通过wait等待, 线程进入无限制的循环工作.
+	m_condition_receive.notify_all(true);
+
+	//暂停所有线程, 然后线程还是继续工作, 直到下一次循环, 进入wait等待
+	m_condition_receive.notify_all(false);
+
+	//如果线程单次循环运行时间较长, 需要等待线程完全停止, 才能读写公共的内存,
+	if ( m_condition_receive.is_waiting() )
+	{
+	    // 读写公共的内存,
+	}
+
+	//唤醒一个线程, 然后线程循环一次, 然后下次循环进入等待
+	m_condition_receive.notify_all(false, true);
+
+
+	//杀死线程,
+	m_condition_receive.kill_all();
+
+	//在线程join结束之后, 就可以可以回收线程内存资源
+	mp_thread_receive->join();
+	delete mp_thread_receive;
+	mp_thread_receive = NULL;
+}
+*/

src/pkgname/src/tool/thread_condition.puml

@@ -0,0 +1,96 @@
+@startuml
+skinparam classAttributeIconSize 0
+
+title  Thread_condition 是多线程的条件控制类，主要是控制线程的启停和退出
+note left of Thread_condition
+/* Thread_condition 是多线程的条件控制类，主要是控制线程的启停和退出
+ * 线程创建后，一般是循环运行，
+ * 为了防止线程暂满整个cpu，那么需要线程在不工作的是否进入等待状态。
+ * Thread_condition 就可以控制线程的运行状态。
+ *
+	std::atomic<bool> m_pass_ever		//线程能否直接通过等待，对后面的线程也生效。
+	std::atomic<bool> m_pass_once		//线程能否直接通过等待，一次（通过一次之后，wait里面自动改为false）
+ * 外部调用notify系列的函数，唤醒等待的线程，让线程执行功能函数。
+ * 如果需要线程循环多次执行功能函数，那么就使用 notify_all(true)，后面的线程可以直接通过等待了。
+ * 再使用 notify_all(false) ，即可停止线程，让其继续等待。
+ * 如果只想要线程执行一次，那就使用 notify_all(false, true)
+ * 注：notify_all(false, true)和notify_one(false, true) 一样，只能让其中一个线程执行一次
+ *
+ * m_kill_flag //是否杀死线程，让线程强制退出，
+ * 外部调用 kill_all() 函数，可以直接通知线程自动退出。
+	//杀死所有的线程，强制退出线程函数，只是操作受当前Thread_condition影响的所有线程
+	//唤醒所有线程，使其通过等待，但是不能运行功能函数，必须直接return
+	// 注：只是修改m_kill为true，需要线程函数实时检测kill的状态，来return线程。
+	//		通过等待之后，也要检查kill的状态，如果为真，那么就不能执行功能函数，应该直接return
+
+ 注：notify唤醒线程之后，wait里面的判断函数会重新判断。
+ */
+end note
+
+
+
+class Thread_condition
+{
+==public:==
+	Thread_condition();
+	Thread_condition(const Thread_condition& other) = delete;
+	~Thread_condition();
+..
+	//无限等待，由 is_pass_wait 决定是否阻塞。
+	//返回m_pass，
+	bool wait();
+..
+	//等待一定的时间（默认时间单位：毫秒ms），由 is_pass_wait 决定是否阻塞。
+	//return：is_pass_wait的结果，	true：线程直接通过等待，false：线程超时了，然后通过等待。
+	//注意了：线程阻塞期间，是不会return的。
+	bool wait_for_millisecond(unsigned int millisecond);
+..
+	//等待一定的时间（时间单位可调），由 is_pass_wait 决定是否阻塞。
+	//return：is_pass_wait的结果，	true：线程直接通过等待，false：线程超时了，然后通过等待。
+	//注意了：线程阻塞期间，是不会return的。
+	template<typename _Rep, typename _Period>
+	bool wait_for_ex(const std::chrono::duration<_Rep, _Period>& time_duration);
+..
+	//唤醒已经阻塞的线程，唤醒一个线程
+	//pass_ever 或者 pass_once 为真时，才能唤醒线程。都为假时，线程进入等待。
+	void notify_one(bool pass_ever, bool pass_once = false);
+..
+	//唤醒已经阻塞的线程，唤醒全部线程
+	//pass_ever 或者 pass_once 为真时，才能唤醒线程。都为假时，线程进入等待。
+	void notify_all(bool pass_ever, bool pass_once = false);
+	//注：notify_all(false, true)和notify_one(false, true) 一样，只能让其中一个线程执行一次
+..
+	//杀死所有的线程，强制退出线程函数，只是操作受当前Thread_condition影响的所有线程
+	//唤醒所有线程，使其通过等待，但是不能运行功能函数，必须直接return
+	// 注：只是修改m_kill为true，需要线程函数实时检测kill的状态，来return线程。
+	//		通过等待之后，也要检查kill的状态，如果为真，那么就不能执行功能函数，应该直接return
+	void kill_all();
+..
+	//判断是否或者，return !m_kill_flag
+	bool is_alive();
+==public:==
+	bool get_kill_flag();
+	bool get_pass_ever();
+	bool get_pass_once();
+	void set_kill_flag(bool kill);
+	void set_pass_ever(bool pass_ever);
+	void set_pass_once(bool pass_once);
+	void reset(bool kill = false, bool pass_ever = false, bool pass_once = false);
+==protected:==
+	//判断线程是否可以通过等待，wait系列函数的判断标志
+	//注：m_kill或者m_pass为真时，return true
+	static bool is_pass_wait(Thread_condition * other);
+
+	std::atomic<bool> 		m_kill_flag;			//是否杀死线程，让线程强制退出，
+	std::atomic<bool> 		m_pass_ever;			//线程能否直接通过等待，对后面的线程也生效。
+	std::atomic<bool> 		m_pass_once;			//线程能否直接通过等待，一次（通过一次之后，wait里面自动改为false）
+
+	std::mutex 				m_mutex;				//线程的锁
+	std::condition_variable m_condition_variable;	//线程的条件变量
+
+==private:==
+
+}
+
+
+@enduml

src/pkgname/src/tool/thread_pool.h

@@ -0,0 +1,262 @@
+/*
+ * Thread_pool 线程池,
+ *
+ * */
+
+
+
+//例如
+// Thread_pool thread_pool(4);
+// std::future<int> x =  thread_pool.enqueue( []{return 0;} );
+// std::cout << x.get() << std::endl;
+
+//例如
+/*
+Thread_pool pool(4);
+std::vector< std::future<int> > results;
+
+for(int i = 0; i < 8; ++i) {
+results.emplace_back(
+pool.enqueue([i] {
+std::cout << "hello " << i << std::endl;
+std::this_thread::sleep_for(std::chrono::seconds(1));
+std::cout << "world " << i << std::endl;
+return i*i;
+})
+);
+}
+
+for(auto && result: results)
+std::cout << result.get() << ' ';
+std::cout << std::endl;
+return 0;
+*/
+
+#ifndef THREAD_POOL_H
+#define THREAD_POOL_H
+
+#include <vector>
+#include <queue>
+#include <memory>
+#include <thread>
+#include <mutex>
+#include <condition_variable>
+#include <future>
+#include <functional>
+#include <stdexcept>
+
+class Thread_pool {
+public:
+	//构造函数, 会自动初始化 threads_size 数量的线程
+    Thread_pool(size_t threads_size);
+
+	//构造函数,没有初始化的,后续需要调用init才能正常使用
+	Thread_pool();
+	//初始化,初始化 threads_size 数量的线程
+	void thread_pool_init(size_t threads_size);
+
+	//反初始化
+	void thread_pool_uninit();
+
+	//往线程池添加执行任务, 之后会唤醒一个线程去执行他.
+	//input: F&& f  			函数指针(函数名)
+	//input: Args&&... args		函数的参数, 自定义
+    template<class F, class... Args>
+    auto enqueue(F&& f, Args&&... args) 
+        -> std::future<typename std::result_of<F(Args...)>::type>;
+
+
+    ~Thread_pool();
+
+	//判断线程池是否超载
+    bool thread_is_full_load();
+
+private:
+    // 线程数组
+    std::vector< std::thread > workers;
+	//每个线程的工作状态, true:线程正在执行任务,  false:线程空闲等待
+	std::vector< bool > working_flag_vector;
+    // 任务函数 队列, 里面存入的是任务函数的指针
+    std::queue< std::function<void()> > tasks;
+    
+    // 线程锁和条件变量
+    std::mutex queue_mutex;
+    std::condition_variable condition;
+
+    //终止标志位
+    bool stop;
+};
+
+//构造函数, 会自动初始化 threads_size 数量的线程
+inline Thread_pool::Thread_pool(size_t threads_size)
+    :   stop(false)
+{
+	//每个线程的工作状态
+	for(size_t i = 0;i<threads_size;++i)
+	{
+		working_flag_vector.push_back(false);
+	}
+
+	//初始化 threads_size 数量的线程
+    for(size_t i = 0;i<threads_size;++i)
+        workers.emplace_back(
+            [i,this]   //每个线程的执行的基本函数,
+            {
+                for(;;)
+                {
+                    std::function<void()> task;
+
+					{
+						std::unique_lock<std::mutex> lock(this->queue_mutex);
+						this->working_flag_vector[i] = false;//线程等待
+						this->condition.wait(lock,
+											 [this]    //线程等待的判断函数
+											 { return this->stop || !this->tasks.empty(); });
+						if (this->stop )//&& this->tasks.empty()) //这里修改了, 不需要把任务池都执行完才退出, stop之后就可以退了.
+						{
+							return;//只有在终止标志位true, 那么就退出线程执行函数
+						}
+						this->working_flag_vector[i] = true;//线程工作
+						//从 任务池 里面取出 执行函数
+						task = std::move(this->tasks.front());
+						this->tasks.pop();
+					}
+
+					//运行执行函数
+                    task();
+                }
+            }
+        );
+}
+
+
+//构造函数,没有初始化的,后续需要调用init才能正常使用
+inline Thread_pool::Thread_pool()
+:   stop(false)
+{
+
+}
+//初始化,初始化 threads_size 数量的线程
+inline void Thread_pool::thread_pool_init(size_t threads_size)
+{
+	stop = false;
+
+	//每个线程的工作状态
+	for(size_t i = 0;i<threads_size;++i)
+	{
+		working_flag_vector.push_back(false);
+	}
+
+	//初始化 threads_size 数量的线程
+	for(size_t i = 0;i<threads_size;++i)
+		workers.emplace_back(
+		[i,this]   //每个线程的执行的基本函数,
+		{
+			for(;;)
+			{
+				std::function<void()> task;
+
+				{
+					std::unique_lock<std::mutex> lock(this->queue_mutex);
+					this->working_flag_vector[i] = false;//线程等待
+					this->condition.wait(lock,
+										 [this]    //线程等待的判断函数
+										 { return this->stop || !this->tasks.empty(); });
+					if (this->stop )//&& this->tasks.empty()) //这里修改了, 不需要把任务池都执行完才退出, stop之后就可以退了.
+					{
+						return;//只有在终止标志位true, 那么就退出线程执行函数
+					}
+					this->working_flag_vector[i] = true;//线程工作
+					//从 任务池 里面取出 执行函数
+					task = std::move(this->tasks.front());
+					this->tasks.pop();
+				}
+
+				//运行执行函数
+				task();
+			}
+		}
+		);
+}
+
+//反初始化
+inline void Thread_pool::thread_pool_uninit()
+{
+	{
+		std::unique_lock<std::mutex> lock(queue_mutex);
+		stop = true;
+	}
+	condition.notify_all();
+
+	for (auto iter = workers.begin(); iter != workers.end(); )
+	{
+		iter->join();
+		iter = workers.erase(iter);
+	}
+	working_flag_vector.clear();
+}
+
+
+//往线程池添加执行任务, 之后会唤醒一个线程去执行他.
+//input: F&& f  			函数指针(函数名)
+//input: Args&&... args		函数的参数, 自定义
+//注注注注注意了:::::  res是enqueue的返回值, 由于线程异步, 使用future, 可以返回未来的一个值,
+// 在子线程执行完成之后, 将结果返回给外部主线程
+// 外部主线程 调用时, 必须使用 std::future<return_type> 格式去接受
+template<class F, class... Args>
+auto Thread_pool::enqueue(F&& f, Args&&... args) 
+    -> std::future<typename std::result_of<F(Args...)>::type>
+{
+    using return_type = typename std::result_of<F(Args...)>::type;
+
+    auto task = std::make_shared< std::packaged_task<return_type()> >(
+            std::bind(std::forward<F>(f), std::forward<Args>(args)...)
+        );
+
+
+    std::future<return_type> res = task->get_future();
+    {
+        std::unique_lock<std::mutex> lock(queue_mutex);
+
+        // don't allow enqueueing after stopping the pool
+        if(stop)
+            throw std::runtime_error("enqueue on stopped Thread_pool");
+
+        tasks.emplace([task](){ (*task)(); });
+    }
+    condition.notify_one();
+    return res;
+}
+
+// the destructor joins all threads
+inline Thread_pool::~Thread_pool()
+{
+    {
+        std::unique_lock<std::mutex> lock(queue_mutex);
+        stop = true;
+    }
+    condition.notify_all();
+    for(std::thread &worker: workers)
+	{  worker.join();}
+
+
+}
+
+//判断线程池是否超载
+inline bool Thread_pool::thread_is_full_load()
+{
+	//只要有一个线程wait, 那么就认为没有超载,
+	std::unique_lock<std::mutex> lock(queue_mutex);
+	bool result = true;
+	for(bool t_working_flag: working_flag_vector)
+	{
+		if ( !t_working_flag )
+		{
+			result = false;
+		}
+	}
+	return result;
+}
+
+
+#endif

src/pkgname/src/tool/thread_safe_list.cpp

@@ -0,0 +1,6 @@
+
+
+
+#include "thread_safe_queue.h"
+
+

src/pkgname/src/tool/thread_safe_list.h

@@ -0,0 +1,354 @@
+
+
+/*
+ * （1）这个实现要求构建工具支持C++11的atomic mutex condition_veriable功能。这是C++11的基础特性，一般2011年以后的C++编译器都能支持。 例如，visual studio 2012以上。
+
+（2）这个类的实现中有两处使用了unique_lock而不是lock_guard，这是data_cond.wait所需要的，unique_lock是lock_guard的增强版。
+
+通过std::move的使用（前提是我们实现的类型T定义了移动构造函数和移动赋值函数），能利用移动语义带来的性能优势。
+
+使用shared_ptr<T>返回元素，用户无需释放元素的内存。
+
+
+原文链接：https://blog.csdn.net/weixin_41855721/article/details/81703818
+ 增加了一些功能函数，
+ 补充了注释说明
+
+ termination_list
+ 	// 在退出状态下，所有的功能函数不可用，返回false或者null。
+	// wait_and_pop不会阻塞。让其直接通过，通过后直接return，不允许做其他的。
+
+ pop系列函数
+	//（1）没有调用termination时，每调用一次出队一个元素，直到队列为空本方法阻塞线程。
+	//（2）在调用了termination后，本方法永不阻塞，如果原本已经处于阻塞状态，解除阻塞状态。
+	//（3）返回true时，value值有效。返回false时，value值无效。调用了termination且队列为空时返回false.
+
+注注注注注意了：模板类不支持分离编译。 模板类的实现必须放在头文件
+ 为了方便阅读和编程规范，依然将声明和实现分开，就像是把cpp文件的代码复制到h文件的尾部。
+
+ 如果将实现放到cpp里面，那么就要为cpp文件加 ifndef define endif 防止重定义。
+ 然后在调用方include包含cpp文件，但是这样不好。
+
+
+ thread_safe_queue  就是在 Thread_safe_queue 的基础上修改的,
+
+ * */
+
+#ifndef __THREAD_SAFE_LIST_H__
+#define __THREAD_SAFE_LIST_H__
+
+#include <list>
+
+#include <atomic>
+#include <mutex>
+#include <condition_variable>
+
+
+template<class T>
+class Thread_safe_list
+{
+public:
+	Thread_safe_list();
+	Thread_safe_list(const Thread_safe_list& other);
+	~Thread_safe_list();
+
+	//（1）没有调用termination时，每调用一次出队一个元素，直到队列为空本方法阻塞线程。
+	//（2）在调用了termination后，本方法永不阻塞，如果原本已经处于阻塞状态，解除阻塞状态。
+	//（3）返回true时，value值有效。返回false时，value值无效。调用了termination且队列为空时返回false.
+
+	//等待并弹出数据，成功弹出则返回true
+	// 队列为空则无限等待，termination终止队列，则返回false
+	bool wait_and_pop(T& value);
+	//尝试弹出数据，成功弹出则返回true
+	//队列为空 或者 termination终止队列，返回false
+	bool try_pop(T& value);
+	//等待并弹出数据，成功弹出则返回true
+	// 队列为空则无限等待，termination终止队列，则返回false
+	std::shared_ptr<T> wait_and_pop();
+	//尝试弹出数据，成功弹出则返回true
+	//队列为空 或者 termination终止队列，返回false
+	std::shared_ptr<T> try_pop();
+	//插入一项，并唤醒一个线程，
+	//如果成功插入，则返回true，  失败则返回false
+	//注：只能唤醒一个线程，防止多线程误判empty()
+	bool push(T new_value);
+	//清除队列，只是将队列的实例抛出。T是实例内存，系统自动回收的。
+	bool clear();
+	//清除队列，抛出之后还要delete指针。T是动态内存，需要手动回收的。
+	bool clear_and_delete();
+
+public:
+	//判空
+	bool empty();
+	//获取队列大小
+	size_t size();
+	//设置队列为退出状态。并唤醒所有的线程，使其通过wait
+	// 在退出状态下，所有的功能函数不可用，必须直接返回false或者null。
+	// wait_and_pop不会阻塞。让其直接通过，通过后直接return，不允许做其他的。
+	void termination_list();
+	//唤醒队列，恢复所有的功能函数。wait_and_pop会继续阻塞。
+	void wake_list();
+	//获取退出状态
+	bool get_termination_flag();
+	//判断是否可以直接通过wait，  m_data_list不为空或者m_termination终止时都可以通过等待。
+	bool is_pass();
+
+public:
+	std::mutex 						m_mutex;				//队列的锁
+	std::list<std::shared_ptr<T>> 	m_data_list;			//队列数据，使用智能指针shared_ptr
+	std::condition_variable 		m_data_cond;			//条件变量
+	std::atomic<bool> 				m_termination_flag;		//终止标志位
+
+private:
+
+
+};
+
+
+
+
+
+
+
+
+template<class T>
+Thread_safe_list<T>::Thread_safe_list()
+{
+	m_termination_flag = false;
+}
+template<class T>
+Thread_safe_list<T>::Thread_safe_list(const Thread_safe_list& other)
+{
+	std::unique_lock<std::mutex> lock_this(m_mutex);
+	std::unique_lock<std::mutex> lock_other(other.m_mutex);
+	m_data_list = other.data_list;
+	m_termination_flag = other.m_termination_flag;
+}
+template<class T>
+Thread_safe_list<T>::~Thread_safe_list()
+{
+	//析构时，终止队列，让线程通过等待，方便线程推出。
+	termination_list();
+}
+
+//（1）没有调用termination时，每调用一次出队一个元素，直到队列为空本方法阻塞线程。
+//（2）在调用了termination后，本方法永不阻塞，如果原本已经处于阻塞状态，解除阻塞状态。
+//（3）返回true时，value值有效。返回false时，value值无效。调用了termination且队列为空时返回false.
+
+//等待并弹出数据，成功弹出则返回true
+// 队列为空则无限等待，termination终止队列，则返回false
+template<class T>
+bool Thread_safe_list<T>::wait_and_pop(T& value)
+{
+	if ( m_termination_flag )
+	{
+		return false;
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		//无限等待，一直阻塞，除非有新的数据加入或者终止队列
+		m_data_cond.wait(lk, [this]
+		{ return ((!m_data_list.empty()) || m_termination_flag); });
+		if (m_termination_flag)
+		{
+			return false;
+		}
+		else
+		{
+//			value = move(*m_data_list.front());
+			value = (*m_data_list.front());
+
+			m_data_list.pop_front();
+			return true;
+		}
+	}
+}
+//尝试弹出数据，成功弹出则返回true
+//队列为空 或者 termination终止队列，返回false
+template<class T>
+bool Thread_safe_list<T>::try_pop(T& value)
+{
+	if ( m_termination_flag )
+	{
+		return false;
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		if (m_data_list.empty())
+		{
+			return false;
+		}
+		else
+		{
+//			value = move(*m_data_list.front());
+			value = (*m_data_list.front());
+
+			m_data_list.pop();
+			return true;
+		}
+	}
+}
+
+
+
+//等待并弹出数据，成功弹出则返回true
+// 队列为空则无限等待，termination终止队列，则返回false
+template<class T>
+std::shared_ptr<T> Thread_safe_list<T>::wait_and_pop()
+{
+	if ( m_termination_flag )
+	{
+		return NULL;
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		//无限等待，一直阻塞，除非有新的数据加入或者终止队列
+		m_data_cond.wait(lk, [this]
+		{ return ((!m_data_list.empty()) || m_termination_flag); });
+		if (m_termination_flag)
+		{
+			return NULL;
+		}
+		else
+		{
+			std::shared_ptr<T> res = m_data_list.front();
+			m_data_list.pop();
+			return res;
+		}
+	}
+}
+//尝试弹出数据，成功弹出则返回true
+//队列为空 或者 termination终止队列，返回false
+template<class T>
+std::shared_ptr<T> Thread_safe_list<T>::try_pop()
+{
+	if ( m_termination_flag )
+	{
+		return NULL;
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		if (m_data_list.empty())
+		{
+			return NULL;
+		}
+		else
+		{
+			std::shared_ptr<T> res = m_data_list.front();
+			m_data_list.pop();
+			return res;
+		}
+	}
+}
+//插入一项，并唤醒一个线程，
+//如果成功插入，则返回true，  失败则返回false
+//注：只能唤醒一个线程，防止多线程误判empty()
+template<class T>
+bool Thread_safe_list<T>::push(T new_value)
+{
+	if (m_termination_flag)
+	{
+		return false;
+	}
+	else
+	{
+//		std::shared_ptr<T> data(std::make_shared<T>(move(new_value)));
+		std::shared_ptr<T> data(std::make_shared<T>((new_value)));
+		std::unique_lock<std::mutex> lk(m_mutex);
+		m_data_list.push_back(data);
+		m_data_cond.notify_one();
+		return true;
+	}
+}
+//清除队列，只是将队列的实例抛出。T是实例内存，系统自动回收的。
+template<class T>
+bool Thread_safe_list<T>::clear()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	while (!m_data_list.empty())
+	{
+		m_data_list.pop_front();
+	}
+	return true;
+
+}
+
+//清除队列，抛出之后还要delete指针。T是动态内存，需要手动回收的。
+template<class T>
+bool Thread_safe_list<T>::clear_and_delete()
+{
+
+	std::unique_lock<std::mutex> lk(m_mutex);
+	while (!m_data_list.empty())
+	{
+		T res = NULL;
+//		res = move(*m_data_list.front());
+		res = (*m_data_list.front());
+
+		m_data_list.pop_front();
+		if(res != NULL)
+		{
+			delete(res);
+
+		}
+	}
+	return true;
+}
+
+
+
+//判空
+template<class T>
+bool Thread_safe_list<T>::empty()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	return m_data_list.empty();
+}
+//获取队列大小
+template<class T>
+size_t Thread_safe_list<T>::size()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	return m_data_list.size();
+}
+//设置队列为退出状态。并唤醒所有的线程，使其通过wait
+// 在退出状态下，所有的功能函数不可用，必须直接返回false或者null。
+// wait_and_pop不会阻塞。让其直接通过，通过后直接return，不允许做其他的。
+template<class T>
+void Thread_safe_list<T>::termination_list()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	m_termination_flag = true;
+	m_data_cond.notify_all();
+}
+//唤醒队列，恢复所有的功能函数。wait_and_pop会继续阻塞。
+template<class T>
+void Thread_safe_list<T>::wake_list()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	m_termination_flag = false;
+	m_data_cond.notify_all();
+}
+//获取退出状态
+template<class T>
+bool Thread_safe_list<T>::get_termination_flag()
+{
+	return m_termination_flag;
+}
+//判断是否可以直接通过wait，  m_data_list不为空或者m_termination终止时都可以通过等待。
+template<class T>
+bool Thread_safe_list<T>::is_pass()
+{
+	return (!m_data_list.empty() || m_termination_flag);
+}
+
+
+
+
+
+
+#endif //__THREAD_SAFE_LIST_H__

src/pkgname/src/tool/thread_safe_queue.cpp

@@ -0,0 +1,34 @@
+
+
+/*
+ * （1）这个实现要求构建工具支持C++11的atomic std::mutex condition_veriable功能。这是C++11的基础特性，一般2011年以后的C++编译器都能支持。 例如，visual studio 2012以上。
+
+（2）这个类的实现中有两处使用了unique_lock而不是lock_guard，这是data_cond.wait所需要的，unique_lock是lock_guard的增强版。
+
+通过std::move的使用（前提是我们实现的类型T定义了移动构造函数和移动赋值函数），能利用移动语义带来的性能优势。
+
+使用shared_ptr<T>返回元素，用户无需释放元素的内存。
+
+
+原文链接：https://blog.csdn.net/weixin_41855721/article/details/81703818
+引用了他的思路，增加了一些功能函数， 补充了注释说明
+
+ termination_queue
+ 	// 在退出状态下，所有的功能函数不可用，返回false或者null。
+	// wait_and_pop不会阻塞。让其直接通过，通过后直接return，不允许做其他的。
+
+ pop系列函数
+	//（1）没有调用termination时，每调用一次出队一个元素，直到队列为空本方法阻塞线程。
+	//（2）在调用了termination后，本方法永不阻塞，如果原本已经处于阻塞状态，解除阻塞状态。
+	//（3）返回true时，value值有效。返回false时，value值无效。调用了termination且队列为空时返回false.
+
+注注注注注意了：模板类不支持分离编译。 模板类的实现必须放在头文件
+ 为了方便阅读和编程规范，依然将声明和实现分开，就像是把cpp文件的代码复制到h文件的尾部。
+
+ 如果将实现放到cpp里面，那么就要为cpp文件加 ifndef define endif
+ 然后在调用方include包含cpp文件，但是这样不好。
+ * */
+
+#include "thread_safe_queue.h"
+
+

src/pkgname/src/tool/thread_safe_queue.h

@@ -0,0 +1,339 @@
+
+
+/*
+ * （1）这个实现要求构建工具支持C++11的atomic mutex condition_veriable功能。这是C++11的基础特性，一般2011年以后的C++编译器都能支持。 例如，visual studio 2012以上。
+
+（2）这个类的实现中有两处使用了unique_lock而不是lock_guard，这是data_cond.wait所需要的，unique_lock是lock_guard的增强版。
+
+通过std::move的使用（前提是我们实现的类型T定义了移动构造函数和移动赋值函数），能利用移动语义带来的性能优势。
+
+使用shared_ptr<T>返回元素，用户无需释放元素的内存。
+
+
+原文链接：https://blog.csdn.net/weixin_41855721/article/details/81703818
+ 增加了一些功能函数，
+ 补充了注释说明
+
+ termination_queue
+ 	// 在退出状态下，所有的功能函数不可用，返回false或者null。
+	// wait_and_pop不会阻塞。让其直接通过，通过后直接return，不允许做其他的。
+
+ pop系列函数
+	//（1）没有调用termination时，每调用一次出队一个元素，直到队列为空本方法阻塞线程。
+	//（2）在调用了termination后，本方法永不阻塞，如果原本已经处于阻塞状态，解除阻塞状态。
+	//（3）返回true时，value值有效。返回false时，value值无效。调用了termination且队列为空时返回false.
+
+注注注注注意了：模板类不支持分离编译。 模板类的实现必须放在头文件
+ 为了方便阅读和编程规范，依然将声明和实现分开，就像是把cpp文件的代码复制到h文件的尾部。
+
+ 如果将实现放到cpp里面，那么就要为cpp文件加 ifndef define endif 防止重定义。
+ 然后在调用方include包含cpp文件，但是这样不好。
+
+
+ * */
+
+#ifndef LIDARMEASURE_THREAD_SAFE_QUEUE_H
+#define LIDARMEASURE_THREAD_SAFE_QUEUE_H
+
+#include <queue>
+
+#include <atomic>
+#include <mutex>
+#include <condition_variable>
+
+
+template<class T>
+class Thread_safe_queue
+{
+public:
+	Thread_safe_queue();
+	Thread_safe_queue(const Thread_safe_queue& other);
+	~Thread_safe_queue();
+
+	//（1）没有调用termination时，每调用一次出队一个元素，直到队列为空本方法阻塞线程。
+	//（2）在调用了termination后，本方法永不阻塞，如果原本已经处于阻塞状态，解除阻塞状态。
+	//（3）返回true时，value值有效。返回false时，value值无效。调用了termination且队列为空时返回false.
+
+	//等待并弹出数据，成功弹出则返回true
+	// 队列为空则无限等待，termination终止队列，则返回false
+	bool wait_and_pop(T& value);
+	//尝试弹出数据，成功弹出则返回true
+	//队列为空 或者 termination终止队列，返回false
+	bool try_pop(T& value);
+	//等待并弹出数据，成功弹出则返回true
+	// 队列为空则无限等待，termination终止队列，则返回false
+	std::shared_ptr<T> wait_and_pop();
+	//尝试弹出数据，成功弹出则返回true
+	//队列为空 或者 termination终止队列，返回false
+	std::shared_ptr<T> try_pop();
+	//插入一项，并唤醒一个线程，
+	//如果成功插入，则返回true，  失败则返回false
+	//注：只能唤醒一个线程，防止多线程误判empty()
+	bool push(T new_value);
+	//清除队列，只是将队列的实例抛出。T是实例内存，系统自动回收的。
+	bool clear();
+	//清除队列，抛出之后还要delete指针。T是动态内存，需要手动回收的。
+	bool clear_and_delete();
+
+public:
+	//判空
+	bool empty();
+	//获取队列大小
+	size_t size();
+	//设置队列为退出状态。并唤醒所有的线程，使其通过wait
+	// 在退出状态下，所有的功能函数不可用，必须直接返回false或者null。
+	// wait_and_pop不会阻塞。让其直接通过，通过后直接return，不允许做其他的。
+	void termination_queue();
+	//唤醒队列，恢复所有的功能函数。wait_and_pop会继续阻塞。
+	void wake_queue();
+	//获取退出状态
+	bool get_termination_flag();
+	//判断是否可以直接通过wait，  m_data_queue不为空或者m_termination终止时都可以通过等待。
+	bool is_pass();
+
+protected:
+	std::mutex 						m_mutex;				//队列的锁
+	std::queue<std::shared_ptr<T>> 	m_data_queue;			//队列数据，使用智能指针shared_ptr
+	std::condition_variable 		m_data_cond;			//条件变量
+	std::atomic<bool> 				m_termination_flag;		//终止标志位
+
+private:
+
+
+};
+
+
+
+
+
+
+
+
+template<class T>
+Thread_safe_queue<T>::Thread_safe_queue()
+{
+	m_termination_flag = false;
+}
+template<class T>
+Thread_safe_queue<T>::Thread_safe_queue(const Thread_safe_queue& other)
+{
+	std::unique_lock<std::mutex> lock_this(m_mutex);
+	std::unique_lock<std::mutex> lock_other(other.m_mutex);
+	m_data_queue = other.data_queue;
+	m_termination_flag = other.m_termination_flag;
+}
+template<class T>
+Thread_safe_queue<T>::~Thread_safe_queue()
+{
+	//析构时，终止队列，让线程通过等待，方便线程推出。
+	termination_queue();
+}
+
+//（1）没有调用termination时，每调用一次出队一个元素，直到队列为空本方法阻塞线程。
+//（2）在调用了termination后，本方法永不阻塞，如果原本已经处于阻塞状态，解除阻塞状态。
+//（3）返回true时，value值有效。返回false时，value值无效。调用了termination且队列为空时返回false.
+
+//等待并弹出数据，成功弹出则返回true
+// 队列为空则无限等待，termination终止队列，则返回false
+template<class T>
+bool Thread_safe_queue<T>::wait_and_pop(T& value)
+{
+	if ( m_termination_flag )
+	{
+		return false;
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		//无限等待，一直阻塞，除非有新的数据加入或者终止队列
+		m_data_cond.wait(lk, [this]
+		{ return ((!m_data_queue.empty()) || m_termination_flag); });
+		if (m_termination_flag)
+		{
+			return false;
+		}
+		else
+		{
+			value = move(*m_data_queue.front());
+			m_data_queue.pop();
+			return true;
+		}
+	}
+}
+//尝试弹出数据，成功弹出则返回true
+//队列为空 或者 termination终止队列，返回false
+template<class T>
+bool Thread_safe_queue<T>::try_pop(T& value)
+{
+	if ( m_termination_flag )
+	{
+		return false;
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		if (m_data_queue.empty())
+		{
+			return false;
+		}
+		else
+		{
+			value = move(*m_data_queue.front());
+			m_data_queue.pop();
+			return true;
+		}
+	}
+}
+//等待并弹出数据，成功弹出则返回true
+// 队列为空则无限等待，termination终止队列，则返回false
+template<class T>
+std::shared_ptr<T> Thread_safe_queue<T>::wait_and_pop()
+{
+	if ( m_termination_flag )
+	{
+		return NULL;
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		//无限等待，一直阻塞，除非有新的数据加入或者终止队列
+		m_data_cond.wait(lk, [this]
+		{ return ((!m_data_queue.empty()) || m_termination_flag); });
+		if (m_termination_flag)
+		{
+			return NULL;
+		}
+		else
+		{
+			std::shared_ptr<T> res = m_data_queue.front();
+			m_data_queue.pop();
+			return res;
+		}
+	}
+}
+//尝试弹出数据，成功弹出则返回true
+//队列为空 或者 termination终止队列，返回false
+template<class T>
+std::shared_ptr<T> Thread_safe_queue<T>::try_pop()
+{
+	if ( m_termination_flag )
+	{
+		return NULL;
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		if (m_data_queue.empty())
+		{
+			return NULL;
+		}
+		else
+		{
+			std::shared_ptr<T> res = m_data_queue.front();
+			m_data_queue.pop();
+			return res;
+		}
+	}
+}
+//插入一项，并唤醒一个线程，
+//如果成功插入，则返回true，  失败则返回false
+//注：只能唤醒一个线程，防止多线程误判empty()
+template<class T>
+bool Thread_safe_queue<T>::push(T new_value)
+{
+	if (m_termination_flag)
+	{
+		return false;
+	}
+	else
+	{
+		std::shared_ptr<T> data(std::make_shared<T>(move(new_value)));
+		std::unique_lock<std::mutex> lk(m_mutex);
+		m_data_queue.push(data);
+		m_data_cond.notify_one();
+		return true;
+	}
+}
+//清除队列，只是将队列的实例抛出。T是实例内存，系统自动回收的。
+template<class T>
+bool Thread_safe_queue<T>::clear()
+{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		while (!m_data_queue.empty())
+		{
+			m_data_queue.pop();
+		}
+		return true;
+
+}
+//清除队列，抛出之后还要delete指针。T是动态内存，需要手动回收的。
+template<class T>
+bool Thread_safe_queue<T>::clear_and_delete()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	while (!m_data_queue.empty())
+	{
+		T res = NULL;
+		res = move(*m_data_queue.front());
+		m_data_queue.pop();
+		if(res != NULL)
+		{
+			delete(res);
+
+		}
+	}
+	return true;
+
+}
+
+//判空
+template<class T>
+bool Thread_safe_queue<T>::empty()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	return m_data_queue.empty();
+}
+//获取队列大小
+template<class T>
+size_t Thread_safe_queue<T>::size()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	return m_data_queue.size();
+}
+//设置队列为退出状态。并唤醒所有的线程，使其通过wait
+// 在退出状态下，所有的功能函数不可用，必须直接返回false或者null。
+// wait_and_pop不会阻塞。让其直接通过，通过后直接return，不允许做其他的。
+template<class T>
+void Thread_safe_queue<T>::termination_queue()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	m_termination_flag = true;
+	m_data_cond.notify_all();
+}
+//唤醒队列，恢复所有的功能函数。wait_and_pop会继续阻塞。
+template<class T>
+void Thread_safe_queue<T>::wake_queue()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	m_termination_flag = false;
+	m_data_cond.notify_all();
+}
+//获取退出状态
+template<class T>
+bool Thread_safe_queue<T>::get_termination_flag()
+{
+	return m_termination_flag;
+}
+//判断是否可以直接通过wait，  m_data_queue不为空或者m_termination终止时都可以通过等待。
+template<class T>
+bool Thread_safe_queue<T>::is_pass()
+{
+	return (!m_data_queue.empty() || m_termination_flag);
+}
+
+
+
+
+
+
+#endif //LIDARMEASURE_THREAD_SAFE_QUEUE_H

src/pkgname/src/tool/thread_safe_queue.puml

@@ -0,0 +1,108 @@
+@startuml
+skinparam classAttributeIconSize 0
+
+title  Thread_safe_queue 安全线程队列
+
+note left of Thread_safe_queue
+
+/*
+ * （1）这个实现要求构建工具支持C++11的atomic mutex condition_veriable功能。这是C++11的基础特性，一般2011年以后的C++编译器都能支持。 例如，visual studio 2012以上。
+
+（2）这个类的实现中有两处使用了unique_lock而不是lock_guard，这是data_cond.wait所需要的，unique_lock是lock_guard的增强版。
+
+通过std::move的使用（前提是我们实现的类型T定义了移动构造函数和移动赋值函数），能利用移动语义带来的性能优势。
+
+使用shared_ptr<T>返回元素，用户无需释放元素的内存。
+
+
+原文链接：https://blog.csdn.net/weixin_41855721/article/details/81703818
+ 增加了一些功能函数，
+ 补充了注释说明
+
+ termination_queue
+ 	// 在退出状态下，所有的功能函数不可用，返回false或者null。
+	// wait_and_pop不会阻塞。让其直接通过，通过后直接return，不允许做其他的。
+
+ pop系列函数
+	//（1）没有调用termination时，每调用一次出队一个元素，直到队列为空本方法阻塞线程。
+	//（2）在调用了termination后，本方法永不阻塞，如果原本已经处于阻塞状态，解除阻塞状态。
+	//（3）返回true时，value值有效。返回false时，value值无效。调用了termination且队列为空时返回false.
+
+注注注注注意了：模板类不支持分离编译。 模板类的实现必须放在头文件
+ 为了方便阅读和编程规范，依然将声明和实现分开，就像是把cpp文件的代码复制到h文件的尾部。
+
+ 如果将实现放到cpp里面，那么就要为cpp文件加 ifndef define endif 防止重定义。
+ 然后在调用方include包含cpp文件，但是这样不好。
+
+
+ * */
+end note
+
+
+
+class Thread_safe_queue <<    template<class T>    >>
+{
+==public:==
+	Thread_safe_queue();
+	Thread_safe_queue(const Thread_safe_queue& other);
+	~Thread_safe_queue();
+..
+	//（1）没有调用termination时，每调用一次出队一个元素，直到队列为空本方法阻塞线程。
+	//（2）在调用了termination后，本方法永不阻塞，如果原本已经处于阻塞状态，解除阻塞状态。
+	//（3）返回true时，value值有效。返回false时，value值无效。调用了termination且队列为空时返回false.
+	//等待并弹出数据，成功弹出则返回true
+	// 队列为空则无限等待，termination终止队列，则返回false
+	bool wait_and_pop(T& value);
+..
+	//尝试弹出数据，成功弹出则返回true
+	//队列为空 或者 termination终止队列，返回false
+	bool try_pop(T& value);
+..
+	//等待并弹出数据，成功弹出则返回true
+	// 队列为空则无限等待，termination终止队列，则返回false
+	std::shared_ptr<T> wait_and_pop();
+..
+	//尝试弹出数据，成功弹出则返回true
+	//队列为空 或者 termination终止队列，返回false
+	std::shared_ptr<T> try_pop();
+..
+	//插入一项，并唤醒一个线程，
+	//如果成功插入，则返回true，  失败则返回false
+	//注：只能唤醒一个线程，防止多线程误判empty()
+	bool push(T new_value);
+..
+	//清除队列，只是将队列的实例抛出。T是实例内存，系统自动回收的。
+	bool clear();
+..
+	//清除队列，抛出之后还要delete指针。T是动态内存，需要手动回收的。
+	bool clear_and_delete();
+==public:==
+	//判空
+	bool empty();
+..
+	//获取队列大小
+	size_t size();
+..
+	//设置队列为退出状态。并唤醒所有的线程，使其通过wait
+	// 在退出状态下，所有的功能函数不可用，必须直接返回false或者null。
+	// wait_and_pop不会阻塞。让其直接通过，通过后直接return，不允许做其他的。
+	void termination_queue();
+..
+	//唤醒队列，恢复所有的功能函数。wait_and_pop会继续阻塞。
+	void wake_queue();
+..
+	//获取退出状态
+	bool get_termination_flag();
+..
+	//判断是否可以直接通过wait，  m_data_queue不为空或者m_termination终止时都可以通过等待。
+	bool is_pass();
+==protected:==
+	std::mutex 						m_mutex;				//队列的锁
+	std::queue<std::shared_ptr<T>> 	m_data_queue;			//队列数据，使用智能指针shared_ptr
+	std::condition_variable 		m_data_cond;			//条件变量
+	std::atomic<bool> 				m_termination_flag;		//终止标志位
+==private:==
+}
+
+
+@enduml

src/robot_control/CMakeLists.txt

@@ -3,6 +3,7 @@ project(robot_control)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
 add_compile_options(-std=c++11)
+FIND_PACKAGE(Protobuf REQUIRED)
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
@@ -19,7 +20,13 @@ find_package(catkin REQUIRED COMPONENTS
         nav_msgs
         )
 
-        
+aux_source_directory(src/can_card/can CAN_SRC )
+aux_source_directory(src/can_card/can_SDK CAN_SDK )
+aux_source_directory(src/can_card/error_code ERROR_SRC )
+aux_source_directory(src/can_card/tool TOOL_SRC )
+aux_source_directory(src/can_card/communication_can COMMUNICATION_CAN_SRC )
+aux_source_directory(src/can_card/motor MOTOR_SRC )
+
 #aux_source_directory(src/cifx CIFX_SRC)
 #MESSAGE(WARN "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n${CIFX_SRC}")
 
@@ -28,6 +35,13 @@ include_directories(
   ${catkin_INCLUDE_DIRS}
   #/usr/local/include/cifx
   #${CIFX_SRC}
+  ${CAN_SRC}
+  ${CAN_SDK}
+  ${ERROR_SRC}
+  ${TOOL_SRC}
+  ${COMMUNICATION_CAN_SRC}
+  ${MOTOR_SRC}
+  ${PROTOBUF_INCLUDE_DIRS}
 )
 
 #if(CIFX_HEADER OR CIFX_INC_LIB)
@@ -49,6 +63,20 @@ include_directories(
 # target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES})
 
 # CAN通信
-add_executable(${PROJECT_NAME}_node src/robot_control_node.cpp src/turtle_robot.h)
+add_executable(${PROJECT_NAME}_node src/robot_control_node.cpp src/Agv_card.cpp   
+      ${CAN_SRC}
+      ${CAN_SDK}
+      ${ERROR_SRC}
+      ${TOOL_SRC}
+      ${COMMUNICATION_CAN_SRC}
+      ${MOTOR_SRC})
+
 link_directories("/usr/local/lib")
-target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME}_node 
+      ${catkin_LIBRARIES} 
+      -lpthread 
+      ${PROTOBUF_LIBRARIES}
+      /usr/local/lib/libglog.a
+      /usr/local/lib/libgflags.a
+      /home/youchen/extra_space/agv_200731/src/robot_control/src/can_card/can_SDK/libcontrolcan.so
+      )

src/robot_control/src/Agv_card.cpp

@@ -2,9 +2,8 @@
 
 Agv_card::Agv_card()
 {
-
     //laser_base_link_.setOrigin(tf::Vector3(0.365,0.28,0.0));   //urg 20m
-
+    bConnect_ = false;
     //雷达相对小车坐标系的 tf变换
     laser_base_link_.setOrigin(tf::Vector3(-1.65,0,0));     //R2000
     laser_base_link_.setRotation(tf::Quaternion(0,0,0,1));
@@ -12,15 +11,31 @@ Agv_card::Agv_card()
 
     tf_sub_ = nh_.subscribe<tf2_msgs::TFMessage>("tf", 1, boost::bind(&Agv_card::location_tf_callback, _1, this));
 
-    Error_manager ec = m_cifx.init();
-    printf(ec.to_string().append("\n").c_str());
-    bConnect_=true;
-
+    // Error_manager ec = m_cifx.init();
+    // printf(ec.to_string().append("\n").c_str());
+    // printf("00000");
+    Error_manager ec = Motor_manager::get_instance_references().motor_manager_init();
+    if(ec != SUCCESS)
+    {
+        std::cout<<"motor init: "<<ec.to_string()<<std::endl;
+    }else{
+        ec = Motor_communication::get_instance_references().communication_init();
+        if(ec != SUCCESS)
+        {
+            std::cout<<"can comm init: "<<ec.to_string()<<std::endl;
+        }else{
+            bConnect_=true;
+        }
+    }
+    // printf("11111");
 }
 
 Agv_card::~Agv_card()
 {
-
+    // Motor_manager::get_instance_references().motor_driver_stop(1);
+	// Motor_manager::get_instance_references().motor_driver_stop(2);
+	Motor_communication::get_instance_references().communication_uninit();
+	Motor_manager::get_instance_references().motor_manager_uninit();
 }
 
 void Agv_card::location_tf_callback(const tf2_msgs::TFMessage::ConstPtr &msg, Agv_card *par)
@@ -77,65 +92,41 @@ bool Agv_card::down_speed()
 bool Agv_card::readSpeed()
 {
     if(bConnect_==false) return false;
-    if(!m_cifx.is_initialized()) return false;
-    /*
-     * 读取两轮速度值
-     */
+    // if(!m_cifx.is_initialized()) return false;
+    // /*
+    //  * 读取两轮速度值
+    //  */
+    // ws_load.right_linear_speed = m_cifx.m_driver_data.front_motor_data[0];
+    // ws_load.left_linear_speed = m_cifx.m_driver_data.front_motor_data[1];
+
+    ws_load.right_linear_speed = Motor_manager::get_instance_references().get_actual_velocity(2);
+    ws_load.left_linear_speed = Motor_manager::get_instance_references().get_actual_velocity(1);
 
-    /*ws_load.right_linear_speed=m_card.ReadData(1000);
-    ws_load.left_linear_speed = m_card.ReadData(1001);*/
-    ws_load.right_linear_speed = m_cifx.m_driver_data.front_motor_data[0];
-    ws_load.left_linear_speed = m_cifx.m_driver_data.front_motor_data[1];
     // printf("read data \t\t\t\t %.3f %.3f\n",ws_load.left_linear_speed, ws_load.right_linear_speed);
     return true;
 }
 
 bool Agv_card::writeSpeed(const double &linear_speed, const double &angular_speed)
 {
+    if(bConnect_==false) return false;
     if(!point2Wheels(linear_speed, angular_speed)) return false;
     /*
      * 下发两轮速度值
      */
-
-    /*m_card.WriteData(1002,ws_down.left_linear_speed);
-    m_card.WriteData(1003,ws_down.right_linear_speed);*/
-    // static int my_count = 0;
-    if(m_cifx.is_initialized()){
-        Driver_data t_data;
-        t_data.front_motor_data[1] = ws_down.left_linear_speed;
-        t_data.front_motor_data[0] = ws_down.right_linear_speed;
-        // if(my_count++ < 20*3)
-        // {
-        //     t_data.front_motor_data[1] = 0;
-        //     t_data.front_motor_data[0] = 0;
-        // }else if(my_count < 20*6)
-        // {
-        //     t_data.front_motor_data[1] = 0.2;
-        //     t_data.front_motor_data[0] = 0.2;
-        // }
-        // else if(my_count < 20*9)
-        // {
-        //     t_data.front_motor_data[1] = 0.5;
-        //     t_data.front_motor_data[0] = 0.5;
-        // }
-        // else if(my_count < 20*12)
-        // {
-        //     t_data.front_motor_data[1] = 0.1;
-        //     t_data.front_motor_data[0] = 0.1;
-        // }
-        // else
-        // {
-        //     t_data.front_motor_data[1] = 0;
-        //     t_data.front_motor_data[0] = 0;
-        // }
-        Error_manager ec = m_cifx.write_driver_data(t_data);
-        if(ec != SUCCESS)
-            printf("write failed\n");
-        else
-        {
-            printf("write data \t %.3f %.3f\n",linear_speed, angular_speed);
-        }
-    }
+    // if(m_cifx.is_initialized()){
+    //     Driver_data t_data;
+    //     t_data.front_motor_data[1] = ws_down.left_linear_speed;
+    //     t_data.front_motor_data[0] = ws_down.right_linear_speed;
+    //     Error_manager ec = m_cifx.write_driver_data(t_data);
+    //     if(ec != SUCCESS)
+    //         printf("write failed\n");
+    //     else
+    //     {
+    //         printf("write data \t %.3f %.3f\n",linear_speed, angular_speed);
+    //     }
+    // }
+    Motor_manager::get_instance_references().set_target_velocity(1, ws_down.left_linear_speed);
+	Motor_manager::get_instance_references().set_target_velocity(2, ws_down.right_linear_speed);
     
     return true;
 
@@ -147,11 +138,6 @@ bool Agv_card::point2Wheels(const double linear_speed, const double angular_spee
     double v=linear_speed;
     double w=angular_speed;
 
-    /*if(fabs(w)>0.02&& fabs(v)<1e-4)
-    {
-        w=w>0?0.02:-0.02;
-    }*/
-
     //两轮间距
     double width_robot=0.275*2.0;
     if (fabs(v) < 1e-2)

src/robot_control/src/Agv_card.h

@@ -5,7 +5,10 @@
 #include "robot_base.h"
 #include "tf2_msgs/TFMessage.h"
 //#include "cp1616/CP16Card.h"
-#include "cifx/Odom.h"
+// #include "cifx/Odom.h"
+#include "can_card/dtype.h"
+#include "can_card/motor/motor_communication.h"
+#include "can_card/motor/motor_manager.h"
 #define DB_NUMBER   10
 
 using namespace agv;
@@ -39,7 +42,7 @@ public:
     wheelStatus ws_down;
 
     //CP16Card            m_card;
-    Odom m_cifx;
+    // Odom m_cifx;
     bool exit_;
 };
 

src/robot_control/src/can_card/CMakeLists.txt

@@ -0,0 +1,55 @@
+cmake_minimum_required(VERSION 3.15)
+project(can_test)
+
+set(CMAKE_CXX_STANDARD 11)
+
+FIND_PACKAGE(Protobuf REQUIRED)
+
+
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/can CAN_SRC )
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/can_SDK CAN_SDK )
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/error_code ERROR_SRC )
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/tool TOOL_SRC )
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/communication_can COMMUNICATION_CAN_SRC )
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/motor MOTOR_SRC )
+
+
+
+
+include_directories(
+        /usr/local/include
+        ${PCL_INCLUDE_DIRS}
+        ${OpenCV_INCLUDE_DIRS}
+        ${PROTOBUF_INCLUDE_DIRS}
+
+        error_code
+        tool
+        communication_can
+        motor
+)
+link_directories("/usr/local/lib")
+message(STATUS ${EXECUTABLE_OUTPUT_PATH})
+
+add_executable(can_test main.cpp
+        ${CAN_SRC}
+        ${CAN_SDK}
+        ${ERROR_SRC}
+        ${TOOL_SRC}
+        ${COMMUNICATION_CAN_SRC}
+        ${MOTOR_SRC}
+
+
+        )
+
+
+
+
+target_link_libraries(can_test
+        /home/huli/huli/usb_can_test/can_test/can_SDK/libcontrolcan.so
+
+        /usr/local/lib/libglog.a
+        /usr/local/lib/libgflags.a
+        ${PROTOBUF_LIBRARIES}
+
+        -lpthread
+        )

src/robot_control/src/can_card/can_SDK/controlcan.h

@@ -0,0 +1,269 @@
+#ifndef CONTROLCAN_H
+#define CONTROLCAN_H
+
+////文件版本：v2.02 20190609
+
+
+//接口卡类型定义, 设备类型。
+#define VCI_USBCAN1		3
+#define VCI_USBCAN2		4
+#define VCI_USBCAN2A		4
+
+#define VCI_USBCAN_E_U 		20
+#define VCI_USBCAN_2E_U 	21
+
+//函数调用返回状态值
+#define	STATUS_OK					1
+#define STATUS_ERR					0
+	
+#define USHORT unsigned short int
+#define BYTE unsigned char
+#define CHAR char
+#define UCHAR unsigned char
+#define UINT unsigned int
+#define DWORD unsigned int
+#define PVOID void*
+#define ULONG unsigned int
+#define INT int
+#define UINT32 UINT
+#define LPVOID void*
+#define BOOL BYTE
+#define TRUE 1
+#define FALSE 0
+
+//1.ZLGCAN系列接口卡信息的数据类型。
+//VCI_BOARD_INFO结构体包含USB-CAN系列接口卡的设备信息。结构体将在VCI_ReadBoardInfo函数中被填充。
+typedef  struct  _VCI_BOARD_INFO{
+		USHORT	hw_Version;				//硬件版本号,用16进制表示。比如0x0100表示V1.00。
+		USHORT	fw_Version;				//固件版本号,用16进制表示。比如0x0100表示V1.00。
+		USHORT	dr_Version;				//驱动程序版本号,用16进制表示。比如0x0100表示V1.00。
+		USHORT	in_Version;				//接口库版本号,用16进制表示。比如0x0100表示V1.00。
+		USHORT	irq_Num;				//保留参数
+		BYTE	can_Num;				//表示有几路CAN通道。
+		CHAR	str_Serial_Num[20];		//此板卡的序列号。
+		CHAR	str_hw_Type[40];		//硬件类型,比如“USBCAN V1.00”(注意:包括字符串结束符’\0’)
+		USHORT	Reserved[4];			//系统保留。
+} VCI_BOARD_INFO,*PVCI_BOARD_INFO; 
+
+//2.定义CAN信息帧的数据类型。
+//VCI_CAN_OBJ结构体是CAN帧结构体,即1个结构体表示一个帧的数据结构。在发送函数VCI_Transmit和接收函数VCI_Receive中,被用来传送CAN信息帧。
+typedef  struct  _VCI_CAN_OBJ{
+	UINT	ID;							//帧ID。32位变量,数据格式为靠右对齐。详情请参照:《8. 附件 1 : ID 对齐方式 .pdf》说明文档。
+	UINT	TimeStamp;					//设备接收到某一帧的时间标识。时间标示从CAN卡上电开始计时,计时单位为0.1ms。
+	BYTE	TimeFlag;					//是否使用时间标识,为1时TimeStamp有效,TimeFlag和TimeStamp只在此帧为接收帧时有意义。
+	BYTE	SendType;					//发送帧类型。
+											//	=0时为正常发送(发送失败会自动重发,重发时间为4秒,4秒内没有发出则取消);
+											//	=1时为单次发送(只发送一次,发送失败不会自动重发,总线只产生一帧数据);
+											//	其它值无效。
+											//	(二次开发,建议SendType=1,提高发送的响应速度)
+	BYTE	RemoteFlag;					//是否是远程帧。=0时为为数据帧,=1时为远程帧(数据段空)。
+	BYTE	ExternFlag;					//是否是扩展帧。=0时为标准帧(11位ID),=1时为扩展帧(29位ID)。
+	BYTE	DataLen;					//数据长度 DLC (<=8),即CAN帧Data有几个字节。约束了后面Data[8]中的有效字节。
+	BYTE	Data[8];					//CAN帧的数据。由于CAN规定了最大是8个字节,所以这里预留了8个字节的空间,受
+											//DataLen约束。如DataLen定义为3,即Data[0]、Data[1]、Data[2]是有效的。
+	BYTE	Reserved[3];				//系统保留。
+}VCI_CAN_OBJ,*PVCI_CAN_OBJ;
+
+//3.定义初始化CAN的数据类型
+//VCI_INIT_CONFIG结构体定义了初始化CAN的配置。结构体将在VCI_InitCan函数中被
+//填充,即初始化之前,要先填好这个结构体变量。
+typedef struct _INIT_CONFIG{
+	DWORD	AccCode;	//验收码。SJA1000的帧过滤验收码。对经过屏蔽码过滤为“有关位”进行匹配,全部匹
+							//配成功后,此帧可以被接收。否则不接收。详见VCI_InitCAN。
+	DWORD	AccMask;	//屏蔽码。SJA1000的帧过滤屏蔽码。对接收的CAN帧ID进行过滤,对应位为0的是“有
+							//关位”,对应位为1的是“无关位”。屏蔽码推荐设置为0xFFFFFFFF,即全部接收。
+	DWORD	Reserved;	//保留。
+	UCHAR	Filter;		//滤波方式,允许设置为0-3,详细请参照2.2.3节的滤波模式对照表。
+	UCHAR	Timing0;	//波特率定时器 0(BTR0)。设置值见下表。
+	UCHAR	Timing1;	//波特率定时器 1(BTR1)。设置值见下表。
+	UCHAR	Mode;		//模式。 =0表示正常模式(相当于正常节点), =1表示只听模式(只接收,不影响总线),
+							//=2表示自发自收模式(环回模式)。
+}VCI_INIT_CONFIG,*PVCI_INIT_CONFIG;
+
+///////// new add struct for filter /////////
+typedef struct _VCI_FILTER_RECORD{
+	DWORD ExtFrame;	//是否为扩展帧
+	DWORD Start;
+	DWORD End;
+}VCI_FILTER_RECORD,*PVCI_FILTER_RECORD;
+
+#ifdef __cplusplus
+#define EXTERN_C  extern "C"
+#else
+#define EXTERN_C
+#endif
+
+
+
+
+
+
+
+//参数描述
+//DWORD DeviceType				设备类型。(linux版默认为 VCI_USBCAN2 )
+//DWORD DevType					设备类型。(linux版默认为 VCI_USBCAN2 )
+//DWORD DeviceInd				设备索引, 默认为0
+									// 	比如当只有一个USB-CAN适配器时,索引号为0,这时再插入一个USB-CAN适配器
+									//那么后面插入的这个设备索引号就是1,以此类推。
+//DWORD Reserved				保留参数,通常为 0。
+//DWORD CANInd					CAN通道索引。第几路 CAN。即对应卡的CAN通道号,CAN1为0,CAN2为1。
+
+//PVCI_INIT_CONFIG pInitConfig	初始化参数结构。
+//PVCI_BOARD_INFO pInfo			用来存储设备信息的VCI_BOARD_INFO结构指针。
+
+//return DWORD					返回值=1,表示操作成功; =0表示操作失败; =-1表示USB-CAN设备不存在或USB掉线。
+//return ULONG					返回消息数据的帧数,=-1表示USB-CAN设备不存在或USB掉线。
+
+
+//此函数用以打开设备。注意一个设备只能打开一次。
+EXTERN_C DWORD VCI_OpenDevice(DWORD DeviceType,DWORD DeviceInd,DWORD Reserved);
+//此函数用以关闭设备。注意一个设备只能关闭一次。
+EXTERN_C DWORD VCI_CloseDevice(DWORD DeviceType,DWORD DeviceInd);
+//此函数用以初始化指定的CAN通道。有多个CAN通道时,需要多次调用。
+EXTERN_C DWORD VCI_InitCAN(DWORD DeviceType, DWORD DeviceInd, DWORD CANInd, PVCI_INIT_CONFIG pInitConfig);
+//此函数用以获取设备信息。
+EXTERN_C DWORD VCI_ReadBoardInfo(DWORD DeviceType,DWORD DeviceInd,PVCI_BOARD_INFO pInfo);
+
+EXTERN_C DWORD VCI_SetReference(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,DWORD RefType,PVOID pData);
+
+//此函数用以获取指定CAN通道的接收缓冲区中,接收到但尚未被读取的帧数量。主要
+//用途是配合VCI_Receive使用,即缓冲区有数据,再接收。
+//实际应用中,用户可以忽略该函数,直接循环调用VCI_Receive,可以节约PC系统资源,提高程序效率。
+// 返回尚未被读取的帧数,
+EXTERN_C ULONG VCI_GetReceiveNum(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
+//此函数用以清空指定CAN通道的缓冲区。主要用于需要清除接收缓冲区数据的情况,同时发送
+//缓冲区数据也会一并清除。
+EXTERN_C DWORD VCI_ClearBuffer(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
+
+//此函数用以启动CAN卡的某一个CAN通道。有多个CAN通道时,需要多次调用。
+EXTERN_C DWORD VCI_StartCAN(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
+//此函数用以复位 CAN。主要用与 VCI_StartCAN配合使用,无需再初始化,即可恢复CAN卡
+//的正常状态。比如当CAN卡进入总线关闭状态时,可以调用这个函数。
+EXTERN_C DWORD VCI_ResetCAN(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
+
+//发送函数。返回值为实际发送成功的帧数。
+//PVCI_CAN_OBJ pSend	要发送的帧结构体 VCI_CAN_OBJ数组的首指针。
+//UINT Len				要发送的帧结构体数组的长度(发送的帧数量)。最大为1000,建议设为1,每次发送
+							//单帧,以提高发送效率。
+//return ULONG			返回实际发送的帧数,=-1表示USB-CAN设备不存在或USB掉线。
+EXTERN_C ULONG VCI_Transmit(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_OBJ pSend,UINT Len);
+
+//接收函数。此函数从指定的设备CAN通道的接收缓冲区中读取数据。
+//PVCI_CAN_OBJ pReceive	用来接收的帧结构体VCI_CAN_OBJ数组的首指针。注意:数组的大小一定要比下面的len
+							//参数大,否则会出现内存读写错误。
+//UINT Len				用来接收的帧结构体数组的长度(本次接收的最大帧数,实际返回值小于等于这个值)。
+							//该值为所提供的存储空间大小,适配器中为每个通道设置了2000帧左右的接收缓存区,用户
+							//根据自身系统和工作环境需求,在1到2000之间选取适当的接收数组长度。一般pReceive数
+							//组大小与Len都设置大于2000,如:2500为宜,可有效防止数据溢出导致地址冲突。同时每
+							//隔30ms调用一次VCI_Receive为宜。
+							//(在满足应用的时效性情况下,尽量降低调用VCI_Receive
+							//的频率,只要保证内部缓存不溢出,每次读取并处理更多帧,可以提高运行效率。)
+//INT WaitTime			保留参数。
+//return ULONG			返回实际读取的帧数,=-1表示USB-CAN设备不存在或USB掉线。
+EXTERN_C ULONG VCI_Receive(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_OBJ pReceive,UINT Len,INT WaitTime);
+
+
+//复位USB-CAN适配器,复位后需要重新使用VCI_OpenDevice打开设备。等同于插拔一次USB设备。
+EXTERN_C DWORD  VCI_UsbDeviceReset(DWORD DevType,DWORD DevIndex,DWORD Reserved);
+//当同一台PC上使用多个USB-CAN的时候,可用此函数查找当前的设备,并获取所有设备的序列号。最多支持50个设备。
+//PVCI_BOARD_INFO pInfo	结构体数组首地址,用来存储设备序列号等信息的结构体数组。数组长度建议定义为50,
+							//如:VCI_BOARD_INFO pInfo[50]。
+//return DWORD			返回计算机中已插入的USB-CAN适配器的数量。
+EXTERN_C DWORD  VCI_FindUsbDevice2(PVCI_BOARD_INFO pInfo);
+/*
+主要作用:
+1、软件与硬件绑定:用户二次开发软件时,可以实现软件与硬件唯一绑定。通过调用该函
+数,可以获取到设备的序列号,与用户应用程序预先设定的序列号对比。实现软件与硬件的
+唯一绑定与加密。单台设备使用时,也可以直接用VCI_ReadBoardInfo函数填充的
+VCI_BOARD_INFO结构体中的str_Serial_Num参数用于加密。
+2、多卡同机时,设备索引号与序列号一一匹配:多台设备同时插在一台电脑时,要操作对
+应设备,就需要知道此时对应设备的设备索引号。通过调用该函数,结构体数组pInfo中对
+应的结构体中元素str_Serial_Num中即可保存所有设备的序列号,
+pInfo[0].str_Serial_Num、pInfo[1].str_Serial_Num、pInfo[2].str_Serial_Num设备索
+引号依次为0、1、2、3。
+ */
+#endif
+
+
+/*
+备注:
+关于滤波器的设置,请参照:《9. 附件 2 : CAN 参数设置 .pdf》说明文档。
+Timing0和Timing1用来设置CAN波特率,几种常见的波特率(采样点87.5%,SJW为0)
+设置如下:
+CAN波特率 Timing0(BTR0) Timing1(BTR1)
+10 Kbps 0x31 0x1C
+20 Kbps 0x18 0x1C
+40 Kbps 0x87 0xFF
+50 Kbps 0x09 0x1C
+80 Kbps 0x83 0xFF
+100 Kbps 0x04 0x1C
+125 Kbps 0x03 0x1C
+200 Kbps 0x81 0xFA
+250 Kbps 0x01 0x1C
+400 Kbps 0x80 0xFA
+500 Kbps 0x00 0x1C
+666 Kbps 0x80 0xB6
+800 Kbps 0x00 0x16
+1000 Kbps 0x00 0x14
+33.33 Kbps 0x09 0x6F
+66.66 Kbps 0x04 0x6F
+83.33 Kbps 0x03 0x6F
+注:
+1.配置波特率时,用户只需要按照 SJA1000(16MHz)给的波特率参数进行设置
+即可。
+2.常规波特率直接按照上表的值配置即可。其它非常规波特率,可以使用附带
+的波特率侦测工具进行侦测,并到得相应的波特率参数。或是使用USB_CAN TOOL
+安装目录下的波特率计算工具计算。(参考《6.插件2:波特率侦测工具使用说明书.pdf》)
+3.本适配器使用最新高速CAN收发器,不再支持10K以下波特率。
+
+
+
+ pInitConfig
+初始化参数结构。
+参数成员列表:
+成员
+功能描述
+由AccCode和AccMask可以共同决定哪些报文能
+够被接受,这两个寄存器均采用ID的左对齐方式
+pInitConfig->AccCode
+设置,即AccCode与AccMask的最高位(Bit31)
+与ID值的最高位对齐。
+关于ID的对齐方式,请参照:
+《 附件 1 : ID 对齐方
+式详细说明 》说明文档。
+例如: 若把AccCode的值设为0x24600000(即
+0x123左移21位的结果), AccMask的值设为
+0x00000000,则只有CAN信息帧ID为0x123的报
+文能够被接受(AccMask的值0x00000000表示所
+有 位 均 为 相 关 位 ) 。 若 把 AccCode 的 值 设 为
+pInitConfig->AccMask
+0x24600000,AccMask的值设为0x600000(0x03
+左 移 21 位 的 结 果 ), 则 只 有 CAN 信 息 帧 ID 为
+0x120~0x123的报文都能够被接受(AccMask的
+值 0x600000 表 示 除 了 bit0~bit1 其 他 位
+(bit2~bit10)均为相关位)。
+注:本滤波器设置示例以标准帧为例,高11位
+有效;若为扩展帧,则ID为29位,AccCode和
+ AccMask设置时高29位对扩展帧有效!
+保留
+pInitConfig->Reserved
+滤波方式的设置请参照本节的 滤波模式对照
+pInitConfig->Filter
+表 。
+pInitConfig->Timing0 波特率 T0 设置
+pInitConfig->Timing1 波特率 T1 设置
+工作模式:
+0-正常工作
+pInitConfig->Mode
+1-仅监听模式
+2-自发自收测试模式
+滤波模式对照表:
+值 名称
+说明
+0/1 接收所有类型 滤波器同时对标准帧与扩展帧过滤!
+2 只接收标准帧 滤波器只对标准帧过滤,扩展帧将直接被滤除。
+3 只接收扩展帧 滤波器只对扩展帧过滤,标准帧将直接被滤除。
+ */
+
+
+

src/robot_control/src/can_card/communication_can/communication_can.pb.cc

@@ -0,0 +1,875 @@
+// Generated by the protocol buffer compiler.  DO NOT EDIT!
+// source: communication_can.proto
+
+#include "communication_can.pb.h"
+
+#include <algorithm>
+
+#include <google/protobuf/stubs/common.h>
+#include <google/protobuf/stubs/port.h>
+#include <google/protobuf/stubs/once.h>
+#include <google/protobuf/io/coded_stream.h>
+#include <google/protobuf/wire_format_lite_inl.h>
+#include <google/protobuf/descriptor.h>
+#include <google/protobuf/generated_message_reflection.h>
+#include <google/protobuf/reflection_ops.h>
+#include <google/protobuf/wire_format.h>
+// This is a temporary google only hack
+#ifdef GOOGLE_PROTOBUF_ENFORCE_UNIQUENESS
+#include "third_party/protobuf/version.h"
+#endif
+// @@protoc_insertion_point(includes)
+namespace Communication_can_proto {
+class Communication_can_parameterDefaultTypeInternal {
+ public:
+  ::google::protobuf::internal::ExplicitlyConstructed<Communication_can_parameter>
+      _instance;
+} _Communication_can_parameter_default_instance_;
+class Communication_can_parameter_allDefaultTypeInternal {
+ public:
+  ::google::protobuf::internal::ExplicitlyConstructed<Communication_can_parameter_all>
+      _instance;
+} _Communication_can_parameter_all_default_instance_;
+}  // namespace Communication_can_proto
+namespace protobuf_communication_5fcan_2eproto {
+void InitDefaultsCommunication_can_parameterImpl() {
+  GOOGLE_PROTOBUF_VERIFY_VERSION;
+
+#ifdef GOOGLE_PROTOBUF_ENFORCE_UNIQUENESS
+  ::google::protobuf::internal::InitProtobufDefaultsForceUnique();
+#else
+  ::google::protobuf::internal::InitProtobufDefaults();
+#endif  // GOOGLE_PROTOBUF_ENFORCE_UNIQUENESS
+  {
+    void* ptr = &::Communication_can_proto::_Communication_can_parameter_default_instance_;
+    new (ptr) ::Communication_can_proto::Communication_can_parameter();
+    ::google::protobuf::internal::OnShutdownDestroyMessage(ptr);
+  }
+  ::Communication_can_proto::Communication_can_parameter::InitAsDefaultInstance();
+}
+
+void InitDefaultsCommunication_can_parameter() {
+  static GOOGLE_PROTOBUF_DECLARE_ONCE(once);
+  ::google::protobuf::GoogleOnceInit(&once, &InitDefaultsCommunication_can_parameterImpl);
+}
+
+void InitDefaultsCommunication_can_parameter_allImpl() {
+  GOOGLE_PROTOBUF_VERIFY_VERSION;
+
+#ifdef GOOGLE_PROTOBUF_ENFORCE_UNIQUENESS
+  ::google::protobuf::internal::InitProtobufDefaultsForceUnique();
+#else
+  ::google::protobuf::internal::InitProtobufDefaults();
+#endif  // GOOGLE_PROTOBUF_ENFORCE_UNIQUENESS
+  protobuf_communication_5fcan_2eproto::InitDefaultsCommunication_can_parameter();
+  {
+    void* ptr = &::Communication_can_proto::_Communication_can_parameter_all_default_instance_;
+    new (ptr) ::Communication_can_proto::Communication_can_parameter_all();
+    ::google::protobuf::internal::OnShutdownDestroyMessage(ptr);
+  }
+  ::Communication_can_proto::Communication_can_parameter_all::InitAsDefaultInstance();
+}
+
+void InitDefaultsCommunication_can_parameter_all() {
+  static GOOGLE_PROTOBUF_DECLARE_ONCE(once);
+  ::google::protobuf::GoogleOnceInit(&once, &InitDefaultsCommunication_can_parameter_allImpl);
+}
+
+::google::protobuf::Metadata file_level_metadata[2];
+
+const ::google::protobuf::uint32 TableStruct::offsets[] GOOGLE_PROTOBUF_ATTRIBUTE_SECTION_VARIABLE(protodesc_cold) = {
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::Communication_can_proto::Communication_can_parameter, _has_bits_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::Communication_can_proto::Communication_can_parameter, _internal_metadata_),
+  ~0u,  // no _extensions_
+  ~0u,  // no _oneof_case_
+  ~0u,  // no _weak_field_map_
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::Communication_can_proto::Communication_can_parameter, can_device_id_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::Communication_can_proto::Communication_can_parameter, can_serial_number_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::Communication_can_proto::Communication_can_parameter, can_device_type_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::Communication_can_proto::Communication_can_parameter, can_port_id_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::Communication_can_proto::Communication_can_parameter, can_baud_rate_),
+  1,
+  0,
+  3,
+  4,
+  2,
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::Communication_can_proto::Communication_can_parameter_all, _has_bits_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::Communication_can_proto::Communication_can_parameter_all, _internal_metadata_),
+  ~0u,  // no _extensions_
+  ~0u,  // no _oneof_case_
+  ~0u,  // no _weak_field_map_
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::Communication_can_proto::Communication_can_parameter_all, communication_can_parameters_),
+  ~0u,
+};
+static const ::google::protobuf::internal::MigrationSchema schemas[] GOOGLE_PROTOBUF_ATTRIBUTE_SECTION_VARIABLE(protodesc_cold) = {
+  { 0, 10, sizeof(::Communication_can_proto::Communication_can_parameter)},
+  { 15, 21, sizeof(::Communication_can_proto::Communication_can_parameter_all)},
+};
+
+static ::google::protobuf::Message const * const file_default_instances[] = {
+  reinterpret_cast<const ::google::protobuf::Message*>(&::Communication_can_proto::_Communication_can_parameter_default_instance_),
+  reinterpret_cast<const ::google::protobuf::Message*>(&::Communication_can_proto::_Communication_can_parameter_all_default_instance_),
+};
+
+void protobuf_AssignDescriptors() {
+  AddDescriptors();
+  ::google::protobuf::MessageFactory* factory = NULL;
+  AssignDescriptors(
+      "communication_can.proto", schemas, file_default_instances, TableStruct::offsets, factory,
+      file_level_metadata, NULL, NULL);
+}
+
+void protobuf_AssignDescriptorsOnce() {
+  static GOOGLE_PROTOBUF_DECLARE_ONCE(once);
+  ::google::protobuf::GoogleOnceInit(&once, &protobuf_AssignDescriptors);
+}
+
+void protobuf_RegisterTypes(const ::std::string&) GOOGLE_PROTOBUF_ATTRIBUTE_COLD;
+void protobuf_RegisterTypes(const ::std::string&) {
+  protobuf_AssignDescriptorsOnce();
+  ::google::protobuf::internal::RegisterAllTypes(file_level_metadata, 2);
+}
+
+void AddDescriptorsImpl() {
+  InitDefaults();
+  static const char descriptor[] GOOGLE_PROTOBUF_ATTRIBUTE_SECTION_VARIABLE(protodesc_cold) = {
+      "\n\027communication_can.proto\022\027Communication"
+      "_can_proto\"\240\001\n\033Communication_can_paramet"
+      "er\022\025\n\rcan_device_id\030\001 \002(\005\022\031\n\021can_serial_"
+      "number\030\002 \002(\t\022\032\n\017can_device_type\030\003 \002(\005:\0014"
+      "\022\026\n\013can_port_id\030\004 \002(\005:\0012\022\033\n\rcan_baud_rat"
+      "e\030\005 \002(\005:\0041000\"}\n\037Communication_can_param"
+      "eter_all\022Z\n\034communication_can_parameters"
+      "\030\001 \003(\01324.Communication_can_proto.Communi"
+      "cation_can_parameter"
+  };
+  ::google::protobuf::DescriptorPool::InternalAddGeneratedFile(
+      descriptor, 340);
+  ::google::protobuf::MessageFactory::InternalRegisterGeneratedFile(
+    "communication_can.proto", &protobuf_RegisterTypes);
+}
+
+void AddDescriptors() {
+  static GOOGLE_PROTOBUF_DECLARE_ONCE(once);
+  ::google::protobuf::GoogleOnceInit(&once, &AddDescriptorsImpl);
+}
+// Force AddDescriptors() to be called at dynamic initialization time.
+struct StaticDescriptorInitializer {
+  StaticDescriptorInitializer() {
+    AddDescriptors();
+  }
+} static_descriptor_initializer;
+}  // namespace protobuf_communication_5fcan_2eproto
+namespace Communication_can_proto {
+
+// ===================================================================
+
+void Communication_can_parameter::InitAsDefaultInstance() {
+}
+#if !defined(_MSC_VER) || _MSC_VER >= 1900
+const int Communication_can_parameter::kCanDeviceIdFieldNumber;
+const int Communication_can_parameter::kCanSerialNumberFieldNumber;
+const int Communication_can_parameter::kCanDeviceTypeFieldNumber;
+const int Communication_can_parameter::kCanPortIdFieldNumber;
+const int Communication_can_parameter::kCanBaudRateFieldNumber;
+#endif  // !defined(_MSC_VER) || _MSC_VER >= 1900
+
+Communication_can_parameter::Communication_can_parameter()
+  : ::google::protobuf::Message(), _internal_metadata_(NULL) {
+  if (GOOGLE_PREDICT_TRUE(this != internal_default_instance())) {
+    ::protobuf_communication_5fcan_2eproto::InitDefaultsCommunication_can_parameter();
+  }
+  SharedCtor();
+  // @@protoc_insertion_point(constructor:Communication_can_proto.Communication_can_parameter)
+}
+Communication_can_parameter::Communication_can_parameter(const Communication_can_parameter& from)
+  : ::google::protobuf::Message(),
+      _internal_metadata_(NULL),
+      _has_bits_(from._has_bits_),
+      _cached_size_(0) {
+  _internal_metadata_.MergeFrom(from._internal_metadata_);
+  can_serial_number_.UnsafeSetDefault(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
+  if (from.has_can_serial_number()) {
+    can_serial_number_.AssignWithDefault(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), from.can_serial_number_);
+  }
+  ::memcpy(&can_device_id_, &from.can_device_id_,
+    static_cast<size_t>(reinterpret_cast<char*>(&can_port_id_) -
+    reinterpret_cast<char*>(&can_device_id_)) + sizeof(can_port_id_));
+  // @@protoc_insertion_point(copy_constructor:Communication_can_proto.Communication_can_parameter)
+}
+
+void Communication_can_parameter::SharedCtor() {
+  _cached_size_ = 0;
+  can_serial_number_.UnsafeSetDefault(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
+  can_device_id_ = 0;
+  can_baud_rate_ = 1000;
+  can_device_type_ = 4;
+  can_port_id_ = 2;
+}
+
+Communication_can_parameter::~Communication_can_parameter() {
+  // @@protoc_insertion_point(destructor:Communication_can_proto.Communication_can_parameter)
+  SharedDtor();
+}
+
+void Communication_can_parameter::SharedDtor() {
+  can_serial_number_.DestroyNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
+}
+
+void Communication_can_parameter::SetCachedSize(int size) const {
+  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
+  _cached_size_ = size;
+  GOOGLE_SAFE_CONCURRENT_WRITES_END();
+}
+const ::google::protobuf::Descriptor* Communication_can_parameter::descriptor() {
+  ::protobuf_communication_5fcan_2eproto::protobuf_AssignDescriptorsOnce();
+  return ::protobuf_communication_5fcan_2eproto::file_level_metadata[kIndexInFileMessages].descriptor;
+}
+
+const Communication_can_parameter& Communication_can_parameter::default_instance() {
+  ::protobuf_communication_5fcan_2eproto::InitDefaultsCommunication_can_parameter();
+  return *internal_default_instance();
+}
+
+Communication_can_parameter* Communication_can_parameter::New(::google::protobuf::Arena* arena) const {
+  Communication_can_parameter* n = new Communication_can_parameter;
+  if (arena != NULL) {
+    arena->Own(n);
+  }
+  return n;
+}
+
+void Communication_can_parameter::Clear() {
+// @@protoc_insertion_point(message_clear_start:Communication_can_proto.Communication_can_parameter)
+  ::google::protobuf::uint32 cached_has_bits = 0;
+  // Prevent compiler warnings about cached_has_bits being unused
+  (void) cached_has_bits;
+
+  cached_has_bits = _has_bits_[0];
+  if (cached_has_bits & 0x00000001u) {
+    GOOGLE_DCHECK(!can_serial_number_.IsDefault(&::google::protobuf::internal::GetEmptyStringAlreadyInited()));
+    (*can_serial_number_.UnsafeRawStringPointer())->clear();
+  }
+  if (cached_has_bits & 30u) {
+    can_device_id_ = 0;
+    can_baud_rate_ = 1000;
+    can_device_type_ = 4;
+    can_port_id_ = 2;
+  }
+  _has_bits_.Clear();
+  _internal_metadata_.Clear();
+}
+
+bool Communication_can_parameter::MergePartialFromCodedStream(
+    ::google::protobuf::io::CodedInputStream* input) {
+#define DO_(EXPRESSION) if (!GOOGLE_PREDICT_TRUE(EXPRESSION)) goto failure
+  ::google::protobuf::uint32 tag;
+  // @@protoc_insertion_point(parse_start:Communication_can_proto.Communication_can_parameter)
+  for (;;) {
+    ::std::pair< ::google::protobuf::uint32, bool> p = input->ReadTagWithCutoffNoLastTag(127u);
+    tag = p.first;
+    if (!p.second) goto handle_unusual;
+    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
+      // required int32 can_device_id = 1;
+      case 1: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(8u /* 8 & 0xFF */)) {
+          set_has_can_device_id();
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
+                 input, &can_device_id_)));
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
+      // required string can_serial_number = 2;
+      case 2: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(18u /* 18 & 0xFF */)) {
+          DO_(::google::protobuf::internal::WireFormatLite::ReadString(
+                input, this->mutable_can_serial_number()));
+          ::google::protobuf::internal::WireFormat::VerifyUTF8StringNamedField(
+            this->can_serial_number().data(), static_cast<int>(this->can_serial_number().length()),
+            ::google::protobuf::internal::WireFormat::PARSE,
+            "Communication_can_proto.Communication_can_parameter.can_serial_number");
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
+      // required int32 can_device_type = 3 [default = 4];
+      case 3: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(24u /* 24 & 0xFF */)) {
+          set_has_can_device_type();
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
+                 input, &can_device_type_)));
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
+      // required int32 can_port_id = 4 [default = 2];
+      case 4: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(32u /* 32 & 0xFF */)) {
+          set_has_can_port_id();
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
+                 input, &can_port_id_)));
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
+      // required int32 can_baud_rate = 5 [default = 1000];
+      case 5: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(40u /* 40 & 0xFF */)) {
+          set_has_can_baud_rate();
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
+                 input, &can_baud_rate_)));
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
+      default: {
+      handle_unusual:
+        if (tag == 0) {
+          goto success;
+        }
+        DO_(::google::protobuf::internal::WireFormat::SkipField(
+              input, tag, _internal_metadata_.mutable_unknown_fields()));
+        break;
+      }
+    }
+  }
+success:
+  // @@protoc_insertion_point(parse_success:Communication_can_proto.Communication_can_parameter)
+  return true;
+failure:
+  // @@protoc_insertion_point(parse_failure:Communication_can_proto.Communication_can_parameter)
+  return false;
+#undef DO_
+}
+
+void Communication_can_parameter::SerializeWithCachedSizes(
+    ::google::protobuf::io::CodedOutputStream* output) const {
+  // @@protoc_insertion_point(serialize_start:Communication_can_proto.Communication_can_parameter)
+  ::google::protobuf::uint32 cached_has_bits = 0;
+  (void) cached_has_bits;
+
+  cached_has_bits = _has_bits_[0];
+  // required int32 can_device_id = 1;
+  if (cached_has_bits & 0x00000002u) {
+    ::google::protobuf::internal::WireFormatLite::WriteInt32(1, this->can_device_id(), output);
+  }
+
+  // required string can_serial_number = 2;
+  if (cached_has_bits & 0x00000001u) {
+    ::google::protobuf::internal::WireFormat::VerifyUTF8StringNamedField(
+      this->can_serial_number().data(), static_cast<int>(this->can_serial_number().length()),
+      ::google::protobuf::internal::WireFormat::SERIALIZE,
+      "Communication_can_proto.Communication_can_parameter.can_serial_number");
+    ::google::protobuf::internal::WireFormatLite::WriteStringMaybeAliased(
+      2, this->can_serial_number(), output);
+  }
+
+  // required int32 can_device_type = 3 [default = 4];
+  if (cached_has_bits & 0x00000008u) {
+    ::google::protobuf::internal::WireFormatLite::WriteInt32(3, this->can_device_type(), output);
+  }
+
+  // required int32 can_port_id = 4 [default = 2];
+  if (cached_has_bits & 0x00000010u) {
+    ::google::protobuf::internal::WireFormatLite::WriteInt32(4, this->can_port_id(), output);
+  }
+
+  // required int32 can_baud_rate = 5 [default = 1000];
+  if (cached_has_bits & 0x00000004u) {
+    ::google::protobuf::internal::WireFormatLite::WriteInt32(5, this->can_baud_rate(), output);
+  }
+
+  if (_internal_metadata_.have_unknown_fields()) {
+    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
+        _internal_metadata_.unknown_fields(), output);
+  }
+  // @@protoc_insertion_point(serialize_end:Communication_can_proto.Communication_can_parameter)
+}
+
+::google::protobuf::uint8* Communication_can_parameter::InternalSerializeWithCachedSizesToArray(
+    bool deterministic, ::google::protobuf::uint8* target) const {
+  (void)deterministic; // Unused
+  // @@protoc_insertion_point(serialize_to_array_start:Communication_can_proto.Communication_can_parameter)
+  ::google::protobuf::uint32 cached_has_bits = 0;
+  (void) cached_has_bits;
+
+  cached_has_bits = _has_bits_[0];
+  // required int32 can_device_id = 1;
+  if (cached_has_bits & 0x00000002u) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(1, this->can_device_id(), target);
+  }
+
+  // required string can_serial_number = 2;
+  if (cached_has_bits & 0x00000001u) {
+    ::google::protobuf::internal::WireFormat::VerifyUTF8StringNamedField(
+      this->can_serial_number().data(), static_cast<int>(this->can_serial_number().length()),
+      ::google::protobuf::internal::WireFormat::SERIALIZE,
+      "Communication_can_proto.Communication_can_parameter.can_serial_number");
+    target =
+      ::google::protobuf::internal::WireFormatLite::WriteStringToArray(
+        2, this->can_serial_number(), target);
+  }
+
+  // required int32 can_device_type = 3 [default = 4];
+  if (cached_has_bits & 0x00000008u) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(3, this->can_device_type(), target);
+  }
+
+  // required int32 can_port_id = 4 [default = 2];
+  if (cached_has_bits & 0x00000010u) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(4, this->can_port_id(), target);
+  }
+
+  // required int32 can_baud_rate = 5 [default = 1000];
+  if (cached_has_bits & 0x00000004u) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(5, this->can_baud_rate(), target);
+  }
+
+  if (_internal_metadata_.have_unknown_fields()) {
+    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
+        _internal_metadata_.unknown_fields(), target);
+  }
+  // @@protoc_insertion_point(serialize_to_array_end:Communication_can_proto.Communication_can_parameter)
+  return target;
+}
+
+size_t Communication_can_parameter::RequiredFieldsByteSizeFallback() const {
+// @@protoc_insertion_point(required_fields_byte_size_fallback_start:Communication_can_proto.Communication_can_parameter)
+  size_t total_size = 0;
+
+  if (has_can_serial_number()) {
+    // required string can_serial_number = 2;
+    total_size += 1 +
+      ::google::protobuf::internal::WireFormatLite::StringSize(
+        this->can_serial_number());
+  }
+
+  if (has_can_device_id()) {
+    // required int32 can_device_id = 1;
+    total_size += 1 +
+      ::google::protobuf::internal::WireFormatLite::Int32Size(
+        this->can_device_id());
+  }
+
+  if (has_can_baud_rate()) {
+    // required int32 can_baud_rate = 5 [default = 1000];
+    total_size += 1 +
+      ::google::protobuf::internal::WireFormatLite::Int32Size(
+        this->can_baud_rate());
+  }
+
+  if (has_can_device_type()) {
+    // required int32 can_device_type = 3 [default = 4];
+    total_size += 1 +
+      ::google::protobuf::internal::WireFormatLite::Int32Size(
+        this->can_device_type());
+  }
+
+  if (has_can_port_id()) {
+    // required int32 can_port_id = 4 [default = 2];
+    total_size += 1 +
+      ::google::protobuf::internal::WireFormatLite::Int32Size(
+        this->can_port_id());
+  }
+
+  return total_size;
+}
+size_t Communication_can_parameter::ByteSizeLong() const {
+// @@protoc_insertion_point(message_byte_size_start:Communication_can_proto.Communication_can_parameter)
+  size_t total_size = 0;
+
+  if (_internal_metadata_.have_unknown_fields()) {
+    total_size +=
+      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
+        _internal_metadata_.unknown_fields());
+  }
+  if (((_has_bits_[0] & 0x0000001f) ^ 0x0000001f) == 0) {  // All required fields are present.
+    // required string can_serial_number = 2;
+    total_size += 1 +
+      ::google::protobuf::internal::WireFormatLite::StringSize(
+        this->can_serial_number());
+
+    // required int32 can_device_id = 1;
+    total_size += 1 +
+      ::google::protobuf::internal::WireFormatLite::Int32Size(
+        this->can_device_id());
+
+    // required int32 can_baud_rate = 5 [default = 1000];
+    total_size += 1 +
+      ::google::protobuf::internal::WireFormatLite::Int32Size(
+        this->can_baud_rate());
+
+    // required int32 can_device_type = 3 [default = 4];
+    total_size += 1 +
+      ::google::protobuf::internal::WireFormatLite::Int32Size(
+        this->can_device_type());
+
+    // required int32 can_port_id = 4 [default = 2];
+    total_size += 1 +
+      ::google::protobuf::internal::WireFormatLite::Int32Size(
+        this->can_port_id());
+
+  } else {
+    total_size += RequiredFieldsByteSizeFallback();
+  }
+  int cached_size = ::google::protobuf::internal::ToCachedSize(total_size);
+  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
+  _cached_size_ = cached_size;
+  GOOGLE_SAFE_CONCURRENT_WRITES_END();
+  return total_size;
+}
+
+void Communication_can_parameter::MergeFrom(const ::google::protobuf::Message& from) {
+// @@protoc_insertion_point(generalized_merge_from_start:Communication_can_proto.Communication_can_parameter)
+  GOOGLE_DCHECK_NE(&from, this);
+  const Communication_can_parameter* source =
+      ::google::protobuf::internal::DynamicCastToGenerated<const Communication_can_parameter>(
+          &from);
+  if (source == NULL) {
+  // @@protoc_insertion_point(generalized_merge_from_cast_fail:Communication_can_proto.Communication_can_parameter)
+    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
+  } else {
+  // @@protoc_insertion_point(generalized_merge_from_cast_success:Communication_can_proto.Communication_can_parameter)
+    MergeFrom(*source);
+  }
+}
+
+void Communication_can_parameter::MergeFrom(const Communication_can_parameter& from) {
+// @@protoc_insertion_point(class_specific_merge_from_start:Communication_can_proto.Communication_can_parameter)
+  GOOGLE_DCHECK_NE(&from, this);
+  _internal_metadata_.MergeFrom(from._internal_metadata_);
+  ::google::protobuf::uint32 cached_has_bits = 0;
+  (void) cached_has_bits;
+
+  cached_has_bits = from._has_bits_[0];
+  if (cached_has_bits & 31u) {
+    if (cached_has_bits & 0x00000001u) {
+      set_has_can_serial_number();
+      can_serial_number_.AssignWithDefault(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), from.can_serial_number_);
+    }
+    if (cached_has_bits & 0x00000002u) {
+      can_device_id_ = from.can_device_id_;
+    }
+    if (cached_has_bits & 0x00000004u) {
+      can_baud_rate_ = from.can_baud_rate_;
+    }
+    if (cached_has_bits & 0x00000008u) {
+      can_device_type_ = from.can_device_type_;
+    }
+    if (cached_has_bits & 0x00000010u) {
+      can_port_id_ = from.can_port_id_;
+    }
+    _has_bits_[0] |= cached_has_bits;
+  }
+}
+
+void Communication_can_parameter::CopyFrom(const ::google::protobuf::Message& from) {
+// @@protoc_insertion_point(generalized_copy_from_start:Communication_can_proto.Communication_can_parameter)
+  if (&from == this) return;
+  Clear();
+  MergeFrom(from);
+}
+
+void Communication_can_parameter::CopyFrom(const Communication_can_parameter& from) {
+// @@protoc_insertion_point(class_specific_copy_from_start:Communication_can_proto.Communication_can_parameter)
+  if (&from == this) return;
+  Clear();
+  MergeFrom(from);
+}
+
+bool Communication_can_parameter::IsInitialized() const {
+  if ((_has_bits_[0] & 0x0000001f) != 0x0000001f) return false;
+  return true;
+}
+
+void Communication_can_parameter::Swap(Communication_can_parameter* other) {
+  if (other == this) return;
+  InternalSwap(other);
+}
+void Communication_can_parameter::InternalSwap(Communication_can_parameter* other) {
+  using std::swap;
+  can_serial_number_.Swap(&other->can_serial_number_);
+  swap(can_device_id_, other->can_device_id_);
+  swap(can_baud_rate_, other->can_baud_rate_);
+  swap(can_device_type_, other->can_device_type_);
+  swap(can_port_id_, other->can_port_id_);
+  swap(_has_bits_[0], other->_has_bits_[0]);
+  _internal_metadata_.Swap(&other->_internal_metadata_);
+  swap(_cached_size_, other->_cached_size_);
+}
+
+::google::protobuf::Metadata Communication_can_parameter::GetMetadata() const {
+  protobuf_communication_5fcan_2eproto::protobuf_AssignDescriptorsOnce();
+  return ::protobuf_communication_5fcan_2eproto::file_level_metadata[kIndexInFileMessages];
+}
+
+
+// ===================================================================
+
+void Communication_can_parameter_all::InitAsDefaultInstance() {
+}
+#if !defined(_MSC_VER) || _MSC_VER >= 1900
+const int Communication_can_parameter_all::kCommunicationCanParametersFieldNumber;
+#endif  // !defined(_MSC_VER) || _MSC_VER >= 1900
+
+Communication_can_parameter_all::Communication_can_parameter_all()
+  : ::google::protobuf::Message(), _internal_metadata_(NULL) {
+  if (GOOGLE_PREDICT_TRUE(this != internal_default_instance())) {
+    ::protobuf_communication_5fcan_2eproto::InitDefaultsCommunication_can_parameter_all();
+  }
+  SharedCtor();
+  // @@protoc_insertion_point(constructor:Communication_can_proto.Communication_can_parameter_all)
+}
+Communication_can_parameter_all::Communication_can_parameter_all(const Communication_can_parameter_all& from)
+  : ::google::protobuf::Message(),
+      _internal_metadata_(NULL),
+      _has_bits_(from._has_bits_),
+      _cached_size_(0),
+      communication_can_parameters_(from.communication_can_parameters_) {
+  _internal_metadata_.MergeFrom(from._internal_metadata_);
+  // @@protoc_insertion_point(copy_constructor:Communication_can_proto.Communication_can_parameter_all)
+}
+
+void Communication_can_parameter_all::SharedCtor() {
+  _cached_size_ = 0;
+}
+
+Communication_can_parameter_all::~Communication_can_parameter_all() {
+  // @@protoc_insertion_point(destructor:Communication_can_proto.Communication_can_parameter_all)
+  SharedDtor();
+}
+
+void Communication_can_parameter_all::SharedDtor() {
+}
+
+void Communication_can_parameter_all::SetCachedSize(int size) const {
+  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
+  _cached_size_ = size;
+  GOOGLE_SAFE_CONCURRENT_WRITES_END();
+}
+const ::google::protobuf::Descriptor* Communication_can_parameter_all::descriptor() {
+  ::protobuf_communication_5fcan_2eproto::protobuf_AssignDescriptorsOnce();
+  return ::protobuf_communication_5fcan_2eproto::file_level_metadata[kIndexInFileMessages].descriptor;
+}
+
+const Communication_can_parameter_all& Communication_can_parameter_all::default_instance() {
+  ::protobuf_communication_5fcan_2eproto::InitDefaultsCommunication_can_parameter_all();
+  return *internal_default_instance();
+}
+
+Communication_can_parameter_all* Communication_can_parameter_all::New(::google::protobuf::Arena* arena) const {
+  Communication_can_parameter_all* n = new Communication_can_parameter_all;
+  if (arena != NULL) {
+    arena->Own(n);
+  }
+  return n;
+}
+
+void Communication_can_parameter_all::Clear() {
+// @@protoc_insertion_point(message_clear_start:Communication_can_proto.Communication_can_parameter_all)
+  ::google::protobuf::uint32 cached_has_bits = 0;
+  // Prevent compiler warnings about cached_has_bits being unused
+  (void) cached_has_bits;
+
+  communication_can_parameters_.Clear();
+  _has_bits_.Clear();
+  _internal_metadata_.Clear();
+}
+
+bool Communication_can_parameter_all::MergePartialFromCodedStream(
+    ::google::protobuf::io::CodedInputStream* input) {
+#define DO_(EXPRESSION) if (!GOOGLE_PREDICT_TRUE(EXPRESSION)) goto failure
+  ::google::protobuf::uint32 tag;
+  // @@protoc_insertion_point(parse_start:Communication_can_proto.Communication_can_parameter_all)
+  for (;;) {
+    ::std::pair< ::google::protobuf::uint32, bool> p = input->ReadTagWithCutoffNoLastTag(127u);
+    tag = p.first;
+    if (!p.second) goto handle_unusual;
+    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
+      // repeated .Communication_can_proto.Communication_can_parameter communication_can_parameters = 1;
+      case 1: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(10u /* 10 & 0xFF */)) {
+          DO_(::google::protobuf::internal::WireFormatLite::ReadMessage(input, add_communication_can_parameters()));
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
+      default: {
+      handle_unusual:
+        if (tag == 0) {
+          goto success;
+        }
+        DO_(::google::protobuf::internal::WireFormat::SkipField(
+              input, tag, _internal_metadata_.mutable_unknown_fields()));
+        break;
+      }
+    }
+  }
+success:
+  // @@protoc_insertion_point(parse_success:Communication_can_proto.Communication_can_parameter_all)
+  return true;
+failure:
+  // @@protoc_insertion_point(parse_failure:Communication_can_proto.Communication_can_parameter_all)
+  return false;
+#undef DO_
+}
+
+void Communication_can_parameter_all::SerializeWithCachedSizes(
+    ::google::protobuf::io::CodedOutputStream* output) const {
+  // @@protoc_insertion_point(serialize_start:Communication_can_proto.Communication_can_parameter_all)
+  ::google::protobuf::uint32 cached_has_bits = 0;
+  (void) cached_has_bits;
+
+  // repeated .Communication_can_proto.Communication_can_parameter communication_can_parameters = 1;
+  for (unsigned int i = 0,
+      n = static_cast<unsigned int>(this->communication_can_parameters_size()); i < n; i++) {
+    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
+      1, this->communication_can_parameters(static_cast<int>(i)), output);
+  }
+
+  if (_internal_metadata_.have_unknown_fields()) {
+    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
+        _internal_metadata_.unknown_fields(), output);
+  }
+  // @@protoc_insertion_point(serialize_end:Communication_can_proto.Communication_can_parameter_all)
+}
+
+::google::protobuf::uint8* Communication_can_parameter_all::InternalSerializeWithCachedSizesToArray(
+    bool deterministic, ::google::protobuf::uint8* target) const {
+  (void)deterministic; // Unused
+  // @@protoc_insertion_point(serialize_to_array_start:Communication_can_proto.Communication_can_parameter_all)
+  ::google::protobuf::uint32 cached_has_bits = 0;
+  (void) cached_has_bits;
+
+  // repeated .Communication_can_proto.Communication_can_parameter communication_can_parameters = 1;
+  for (unsigned int i = 0,
+      n = static_cast<unsigned int>(this->communication_can_parameters_size()); i < n; i++) {
+    target = ::google::protobuf::internal::WireFormatLite::
+      InternalWriteMessageToArray(
+        1, this->communication_can_parameters(static_cast<int>(i)), deterministic, target);
+  }
+
+  if (_internal_metadata_.have_unknown_fields()) {
+    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
+        _internal_metadata_.unknown_fields(), target);
+  }
+  // @@protoc_insertion_point(serialize_to_array_end:Communication_can_proto.Communication_can_parameter_all)
+  return target;
+}
+
+size_t Communication_can_parameter_all::ByteSizeLong() const {
+// @@protoc_insertion_point(message_byte_size_start:Communication_can_proto.Communication_can_parameter_all)
+  size_t total_size = 0;
+
+  if (_internal_metadata_.have_unknown_fields()) {
+    total_size +=
+      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
+        _internal_metadata_.unknown_fields());
+  }
+  // repeated .Communication_can_proto.Communication_can_parameter communication_can_parameters = 1;
+  {
+    unsigned int count = static_cast<unsigned int>(this->communication_can_parameters_size());
+    total_size += 1UL * count;
+    for (unsigned int i = 0; i < count; i++) {
+      total_size +=
+        ::google::protobuf::internal::WireFormatLite::MessageSize(
+          this->communication_can_parameters(static_cast<int>(i)));
+    }
+  }
+
+  int cached_size = ::google::protobuf::internal::ToCachedSize(total_size);
+  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
+  _cached_size_ = cached_size;
+  GOOGLE_SAFE_CONCURRENT_WRITES_END();
+  return total_size;
+}
+
+void Communication_can_parameter_all::MergeFrom(const ::google::protobuf::Message& from) {
+// @@protoc_insertion_point(generalized_merge_from_start:Communication_can_proto.Communication_can_parameter_all)
+  GOOGLE_DCHECK_NE(&from, this);
+  const Communication_can_parameter_all* source =
+      ::google::protobuf::internal::DynamicCastToGenerated<const Communication_can_parameter_all>(
+          &from);
+  if (source == NULL) {
+  // @@protoc_insertion_point(generalized_merge_from_cast_fail:Communication_can_proto.Communication_can_parameter_all)
+    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
+  } else {
+  // @@protoc_insertion_point(generalized_merge_from_cast_success:Communication_can_proto.Communication_can_parameter_all)
+    MergeFrom(*source);
+  }
+}
+
+void Communication_can_parameter_all::MergeFrom(const Communication_can_parameter_all& from) {
+// @@protoc_insertion_point(class_specific_merge_from_start:Communication_can_proto.Communication_can_parameter_all)
+  GOOGLE_DCHECK_NE(&from, this);
+  _internal_metadata_.MergeFrom(from._internal_metadata_);
+  ::google::protobuf::uint32 cached_has_bits = 0;
+  (void) cached_has_bits;
+
+  communication_can_parameters_.MergeFrom(from.communication_can_parameters_);
+}
+
+void Communication_can_parameter_all::CopyFrom(const ::google::protobuf::Message& from) {
+// @@protoc_insertion_point(generalized_copy_from_start:Communication_can_proto.Communication_can_parameter_all)
+  if (&from == this) return;
+  Clear();
+  MergeFrom(from);
+}
+
+void Communication_can_parameter_all::CopyFrom(const Communication_can_parameter_all& from) {
+// @@protoc_insertion_point(class_specific_copy_from_start:Communication_can_proto.Communication_can_parameter_all)
+  if (&from == this) return;
+  Clear();
+  MergeFrom(from);
+}
+
+bool Communication_can_parameter_all::IsInitialized() const {
+  if (!::google::protobuf::internal::AllAreInitialized(this->communication_can_parameters())) return false;
+  return true;
+}
+
+void Communication_can_parameter_all::Swap(Communication_can_parameter_all* other) {
+  if (other == this) return;
+  InternalSwap(other);
+}
+void Communication_can_parameter_all::InternalSwap(Communication_can_parameter_all* other) {
+  using std::swap;
+  communication_can_parameters_.InternalSwap(&other->communication_can_parameters_);
+  swap(_has_bits_[0], other->_has_bits_[0]);
+  _internal_metadata_.Swap(&other->_internal_metadata_);
+  swap(_cached_size_, other->_cached_size_);
+}
+
+::google::protobuf::Metadata Communication_can_parameter_all::GetMetadata() const {
+  protobuf_communication_5fcan_2eproto::protobuf_AssignDescriptorsOnce();
+  return ::protobuf_communication_5fcan_2eproto::file_level_metadata[kIndexInFileMessages];
+}
+
+
+// @@protoc_insertion_point(namespace_scope)
+}  // namespace Communication_can_proto
+
+// @@protoc_insertion_point(global_scope)

src/robot_control/src/can_card/communication_can/communication_can.pb.h

@@ -0,0 +1,554 @@
+// Generated by the protocol buffer compiler.  DO NOT EDIT!
+// source: communication_can.proto
+
+#ifndef PROTOBUF_communication_5fcan_2eproto__INCLUDED
+#define PROTOBUF_communication_5fcan_2eproto__INCLUDED
+
+#include <string>
+
+#include <google/protobuf/stubs/common.h>
+
+#if GOOGLE_PROTOBUF_VERSION < 3005000
+#error This file was generated by a newer version of protoc which is
+#error incompatible with your Protocol Buffer headers.  Please update
+#error your headers.
+#endif
+#if 3005000 < GOOGLE_PROTOBUF_MIN_PROTOC_VERSION
+#error This file was generated by an older version of protoc which is
+#error incompatible with your Protocol Buffer headers.  Please
+#error regenerate this file with a newer version of protoc.
+#endif
+
+#include <google/protobuf/io/coded_stream.h>
+#include <google/protobuf/arena.h>
+#include <google/protobuf/arenastring.h>
+#include <google/protobuf/generated_message_table_driven.h>
+#include <google/protobuf/generated_message_util.h>
+#include <google/protobuf/metadata.h>
+#include <google/protobuf/message.h>
+#include <google/protobuf/repeated_field.h>  // IWYU pragma: export
+#include <google/protobuf/extension_set.h>  // IWYU pragma: export
+#include <google/protobuf/unknown_field_set.h>
+// @@protoc_insertion_point(includes)
+
+namespace protobuf_communication_5fcan_2eproto {
+// Internal implementation detail -- do not use these members.
+struct TableStruct {
+  static const ::google::protobuf::internal::ParseTableField entries[];
+  static const ::google::protobuf::internal::AuxillaryParseTableField aux[];
+  static const ::google::protobuf::internal::ParseTable schema[2];
+  static const ::google::protobuf::internal::FieldMetadata field_metadata[];
+  static const ::google::protobuf::internal::SerializationTable serialization_table[];
+  static const ::google::protobuf::uint32 offsets[];
+};
+void AddDescriptors();
+void InitDefaultsCommunication_can_parameterImpl();
+void InitDefaultsCommunication_can_parameter();
+void InitDefaultsCommunication_can_parameter_allImpl();
+void InitDefaultsCommunication_can_parameter_all();
+inline void InitDefaults() {
+  InitDefaultsCommunication_can_parameter();
+  InitDefaultsCommunication_can_parameter_all();
+}
+}  // namespace protobuf_communication_5fcan_2eproto
+namespace Communication_can_proto {
+class Communication_can_parameter;
+class Communication_can_parameterDefaultTypeInternal;
+extern Communication_can_parameterDefaultTypeInternal _Communication_can_parameter_default_instance_;
+class Communication_can_parameter_all;
+class Communication_can_parameter_allDefaultTypeInternal;
+extern Communication_can_parameter_allDefaultTypeInternal _Communication_can_parameter_all_default_instance_;
+}  // namespace Communication_can_proto
+namespace Communication_can_proto {
+
+// ===================================================================
+
+class Communication_can_parameter : public ::google::protobuf::Message /* @@protoc_insertion_point(class_definition:Communication_can_proto.Communication_can_parameter) */ {
+ public:
+  Communication_can_parameter();
+  virtual ~Communication_can_parameter();
+
+  Communication_can_parameter(const Communication_can_parameter& from);
+
+  inline Communication_can_parameter& operator=(const Communication_can_parameter& from) {
+    CopyFrom(from);
+    return *this;
+  }
+  #if LANG_CXX11
+  Communication_can_parameter(Communication_can_parameter&& from) noexcept
+    : Communication_can_parameter() {
+    *this = ::std::move(from);
+  }
+
+  inline Communication_can_parameter& operator=(Communication_can_parameter&& from) noexcept {
+    if (GetArenaNoVirtual() == from.GetArenaNoVirtual()) {
+      if (this != &from) InternalSwap(&from);
+    } else {
+      CopyFrom(from);
+    }
+    return *this;
+  }
+  #endif
+  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
+    return _internal_metadata_.unknown_fields();
+  }
+  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
+    return _internal_metadata_.mutable_unknown_fields();
+  }
+
+  static const ::google::protobuf::Descriptor* descriptor();
+  static const Communication_can_parameter& default_instance();
+
+  static void InitAsDefaultInstance();  // FOR INTERNAL USE ONLY
+  static inline const Communication_can_parameter* internal_default_instance() {
+    return reinterpret_cast<const Communication_can_parameter*>(
+               &_Communication_can_parameter_default_instance_);
+  }
+  static PROTOBUF_CONSTEXPR int const kIndexInFileMessages =
+    0;
+
+  void Swap(Communication_can_parameter* other);
+  friend void swap(Communication_can_parameter& a, Communication_can_parameter& b) {
+    a.Swap(&b);
+  }
+
+  // implements Message ----------------------------------------------
+
+  inline Communication_can_parameter* New() const PROTOBUF_FINAL { return New(NULL); }
+
+  Communication_can_parameter* New(::google::protobuf::Arena* arena) const PROTOBUF_FINAL;
+  void CopyFrom(const ::google::protobuf::Message& from) PROTOBUF_FINAL;
+  void MergeFrom(const ::google::protobuf::Message& from) PROTOBUF_FINAL;
+  void CopyFrom(const Communication_can_parameter& from);
+  void MergeFrom(const Communication_can_parameter& from);
+  void Clear() PROTOBUF_FINAL;
+  bool IsInitialized() const PROTOBUF_FINAL;
+
+  size_t ByteSizeLong() const PROTOBUF_FINAL;
+  bool MergePartialFromCodedStream(
+      ::google::protobuf::io::CodedInputStream* input) PROTOBUF_FINAL;
+  void SerializeWithCachedSizes(
+      ::google::protobuf::io::CodedOutputStream* output) const PROTOBUF_FINAL;
+  ::google::protobuf::uint8* InternalSerializeWithCachedSizesToArray(
+      bool deterministic, ::google::protobuf::uint8* target) const PROTOBUF_FINAL;
+  int GetCachedSize() const PROTOBUF_FINAL { return _cached_size_; }
+  private:
+  void SharedCtor();
+  void SharedDtor();
+  void SetCachedSize(int size) const PROTOBUF_FINAL;
+  void InternalSwap(Communication_can_parameter* other);
+  private:
+  inline ::google::protobuf::Arena* GetArenaNoVirtual() const {
+    return NULL;
+  }
+  inline void* MaybeArenaPtr() const {
+    return NULL;
+  }
+  public:
+
+  ::google::protobuf::Metadata GetMetadata() const PROTOBUF_FINAL;
+
+  // nested types ----------------------------------------------------
+
+  // accessors -------------------------------------------------------
+
+  // required string can_serial_number = 2;
+  bool has_can_serial_number() const;
+  void clear_can_serial_number();
+  static const int kCanSerialNumberFieldNumber = 2;
+  const ::std::string& can_serial_number() const;
+  void set_can_serial_number(const ::std::string& value);
+  #if LANG_CXX11
+  void set_can_serial_number(::std::string&& value);
+  #endif
+  void set_can_serial_number(const char* value);
+  void set_can_serial_number(const char* value, size_t size);
+  ::std::string* mutable_can_serial_number();
+  ::std::string* release_can_serial_number();
+  void set_allocated_can_serial_number(::std::string* can_serial_number);
+
+  // required int32 can_device_id = 1;
+  bool has_can_device_id() const;
+  void clear_can_device_id();
+  static const int kCanDeviceIdFieldNumber = 1;
+  ::google::protobuf::int32 can_device_id() const;
+  void set_can_device_id(::google::protobuf::int32 value);
+
+  // required int32 can_baud_rate = 5 [default = 1000];
+  bool has_can_baud_rate() const;
+  void clear_can_baud_rate();
+  static const int kCanBaudRateFieldNumber = 5;
+  ::google::protobuf::int32 can_baud_rate() const;
+  void set_can_baud_rate(::google::protobuf::int32 value);
+
+  // required int32 can_device_type = 3 [default = 4];
+  bool has_can_device_type() const;
+  void clear_can_device_type();
+  static const int kCanDeviceTypeFieldNumber = 3;
+  ::google::protobuf::int32 can_device_type() const;
+  void set_can_device_type(::google::protobuf::int32 value);
+
+  // required int32 can_port_id = 4 [default = 2];
+  bool has_can_port_id() const;
+  void clear_can_port_id();
+  static const int kCanPortIdFieldNumber = 4;
+  ::google::protobuf::int32 can_port_id() const;
+  void set_can_port_id(::google::protobuf::int32 value);
+
+  // @@protoc_insertion_point(class_scope:Communication_can_proto.Communication_can_parameter)
+ private:
+  void set_has_can_device_id();
+  void clear_has_can_device_id();
+  void set_has_can_serial_number();
+  void clear_has_can_serial_number();
+  void set_has_can_device_type();
+  void clear_has_can_device_type();
+  void set_has_can_port_id();
+  void clear_has_can_port_id();
+  void set_has_can_baud_rate();
+  void clear_has_can_baud_rate();
+
+  // helper for ByteSizeLong()
+  size_t RequiredFieldsByteSizeFallback() const;
+
+  ::google::protobuf::internal::InternalMetadataWithArena _internal_metadata_;
+  ::google::protobuf::internal::HasBits<1> _has_bits_;
+  mutable int _cached_size_;
+  ::google::protobuf::internal::ArenaStringPtr can_serial_number_;
+  ::google::protobuf::int32 can_device_id_;
+  ::google::protobuf::int32 can_baud_rate_;
+  ::google::protobuf::int32 can_device_type_;
+  ::google::protobuf::int32 can_port_id_;
+  friend struct ::protobuf_communication_5fcan_2eproto::TableStruct;
+  friend void ::protobuf_communication_5fcan_2eproto::InitDefaultsCommunication_can_parameterImpl();
+};
+// -------------------------------------------------------------------
+
+class Communication_can_parameter_all : public ::google::protobuf::Message /* @@protoc_insertion_point(class_definition:Communication_can_proto.Communication_can_parameter_all) */ {
+ public:
+  Communication_can_parameter_all();
+  virtual ~Communication_can_parameter_all();
+
+  Communication_can_parameter_all(const Communication_can_parameter_all& from);
+
+  inline Communication_can_parameter_all& operator=(const Communication_can_parameter_all& from) {
+    CopyFrom(from);
+    return *this;
+  }
+  #if LANG_CXX11
+  Communication_can_parameter_all(Communication_can_parameter_all&& from) noexcept
+    : Communication_can_parameter_all() {
+    *this = ::std::move(from);
+  }
+
+  inline Communication_can_parameter_all& operator=(Communication_can_parameter_all&& from) noexcept {
+    if (GetArenaNoVirtual() == from.GetArenaNoVirtual()) {
+      if (this != &from) InternalSwap(&from);
+    } else {
+      CopyFrom(from);
+    }
+    return *this;
+  }
+  #endif
+  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
+    return _internal_metadata_.unknown_fields();
+  }
+  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
+    return _internal_metadata_.mutable_unknown_fields();
+  }
+
+  static const ::google::protobuf::Descriptor* descriptor();
+  static const Communication_can_parameter_all& default_instance();
+
+  static void InitAsDefaultInstance();  // FOR INTERNAL USE ONLY
+  static inline const Communication_can_parameter_all* internal_default_instance() {
+    return reinterpret_cast<const Communication_can_parameter_all*>(
+               &_Communication_can_parameter_all_default_instance_);
+  }
+  static PROTOBUF_CONSTEXPR int const kIndexInFileMessages =
+    1;
+
+  void Swap(Communication_can_parameter_all* other);
+  friend void swap(Communication_can_parameter_all& a, Communication_can_parameter_all& b) {
+    a.Swap(&b);
+  }
+
+  // implements Message ----------------------------------------------
+
+  inline Communication_can_parameter_all* New() const PROTOBUF_FINAL { return New(NULL); }
+
+  Communication_can_parameter_all* New(::google::protobuf::Arena* arena) const PROTOBUF_FINAL;
+  void CopyFrom(const ::google::protobuf::Message& from) PROTOBUF_FINAL;
+  void MergeFrom(const ::google::protobuf::Message& from) PROTOBUF_FINAL;
+  void CopyFrom(const Communication_can_parameter_all& from);
+  void MergeFrom(const Communication_can_parameter_all& from);
+  void Clear() PROTOBUF_FINAL;
+  bool IsInitialized() const PROTOBUF_FINAL;
+
+  size_t ByteSizeLong() const PROTOBUF_FINAL;
+  bool MergePartialFromCodedStream(
+      ::google::protobuf::io::CodedInputStream* input) PROTOBUF_FINAL;
+  void SerializeWithCachedSizes(
+      ::google::protobuf::io::CodedOutputStream* output) const PROTOBUF_FINAL;
+  ::google::protobuf::uint8* InternalSerializeWithCachedSizesToArray(
+      bool deterministic, ::google::protobuf::uint8* target) const PROTOBUF_FINAL;
+  int GetCachedSize() const PROTOBUF_FINAL { return _cached_size_; }
+  private:
+  void SharedCtor();
+  void SharedDtor();
+  void SetCachedSize(int size) const PROTOBUF_FINAL;
+  void InternalSwap(Communication_can_parameter_all* other);
+  private:
+  inline ::google::protobuf::Arena* GetArenaNoVirtual() const {
+    return NULL;
+  }
+  inline void* MaybeArenaPtr() const {
+    return NULL;
+  }
+  public:
+
+  ::google::protobuf::Metadata GetMetadata() const PROTOBUF_FINAL;
+
+  // nested types ----------------------------------------------------
+
+  // accessors -------------------------------------------------------
+
+  // repeated .Communication_can_proto.Communication_can_parameter communication_can_parameters = 1;
+  int communication_can_parameters_size() const;
+  void clear_communication_can_parameters();
+  static const int kCommunicationCanParametersFieldNumber = 1;
+  const ::Communication_can_proto::Communication_can_parameter& communication_can_parameters(int index) const;
+  ::Communication_can_proto::Communication_can_parameter* mutable_communication_can_parameters(int index);
+  ::Communication_can_proto::Communication_can_parameter* add_communication_can_parameters();
+  ::google::protobuf::RepeatedPtrField< ::Communication_can_proto::Communication_can_parameter >*
+      mutable_communication_can_parameters();
+  const ::google::protobuf::RepeatedPtrField< ::Communication_can_proto::Communication_can_parameter >&
+      communication_can_parameters() const;
+
+  // @@protoc_insertion_point(class_scope:Communication_can_proto.Communication_can_parameter_all)
+ private:
+
+  ::google::protobuf::internal::InternalMetadataWithArena _internal_metadata_;
+  ::google::protobuf::internal::HasBits<1> _has_bits_;
+  mutable int _cached_size_;
+  ::google::protobuf::RepeatedPtrField< ::Communication_can_proto::Communication_can_parameter > communication_can_parameters_;
+  friend struct ::protobuf_communication_5fcan_2eproto::TableStruct;
+  friend void ::protobuf_communication_5fcan_2eproto::InitDefaultsCommunication_can_parameter_allImpl();
+};
+// ===================================================================
+
+
+// ===================================================================
+
+#ifdef __GNUC__
+  #pragma GCC diagnostic push
+  #pragma GCC diagnostic ignored "-Wstrict-aliasing"
+#endif  // __GNUC__
+// Communication_can_parameter
+
+// required int32 can_device_id = 1;
+inline bool Communication_can_parameter::has_can_device_id() const {
+  return (_has_bits_[0] & 0x00000002u) != 0;
+}
+inline void Communication_can_parameter::set_has_can_device_id() {
+  _has_bits_[0] |= 0x00000002u;
+}
+inline void Communication_can_parameter::clear_has_can_device_id() {
+  _has_bits_[0] &= ~0x00000002u;
+}
+inline void Communication_can_parameter::clear_can_device_id() {
+  can_device_id_ = 0;
+  clear_has_can_device_id();
+}
+inline ::google::protobuf::int32 Communication_can_parameter::can_device_id() const {
+  // @@protoc_insertion_point(field_get:Communication_can_proto.Communication_can_parameter.can_device_id)
+  return can_device_id_;
+}
+inline void Communication_can_parameter::set_can_device_id(::google::protobuf::int32 value) {
+  set_has_can_device_id();
+  can_device_id_ = value;
+  // @@protoc_insertion_point(field_set:Communication_can_proto.Communication_can_parameter.can_device_id)
+}
+
+// required string can_serial_number = 2;
+inline bool Communication_can_parameter::has_can_serial_number() const {
+  return (_has_bits_[0] & 0x00000001u) != 0;
+}
+inline void Communication_can_parameter::set_has_can_serial_number() {
+  _has_bits_[0] |= 0x00000001u;
+}
+inline void Communication_can_parameter::clear_has_can_serial_number() {
+  _has_bits_[0] &= ~0x00000001u;
+}
+inline void Communication_can_parameter::clear_can_serial_number() {
+  can_serial_number_.ClearToEmptyNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
+  clear_has_can_serial_number();
+}
+inline const ::std::string& Communication_can_parameter::can_serial_number() const {
+  // @@protoc_insertion_point(field_get:Communication_can_proto.Communication_can_parameter.can_serial_number)
+  return can_serial_number_.GetNoArena();
+}
+inline void Communication_can_parameter::set_can_serial_number(const ::std::string& value) {
+  set_has_can_serial_number();
+  can_serial_number_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), value);
+  // @@protoc_insertion_point(field_set:Communication_can_proto.Communication_can_parameter.can_serial_number)
+}
+#if LANG_CXX11
+inline void Communication_can_parameter::set_can_serial_number(::std::string&& value) {
+  set_has_can_serial_number();
+  can_serial_number_.SetNoArena(
+    &::google::protobuf::internal::GetEmptyStringAlreadyInited(), ::std::move(value));
+  // @@protoc_insertion_point(field_set_rvalue:Communication_can_proto.Communication_can_parameter.can_serial_number)
+}
+#endif
+inline void Communication_can_parameter::set_can_serial_number(const char* value) {
+  GOOGLE_DCHECK(value != NULL);
+  set_has_can_serial_number();
+  can_serial_number_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), ::std::string(value));
+  // @@protoc_insertion_point(field_set_char:Communication_can_proto.Communication_can_parameter.can_serial_number)
+}
+inline void Communication_can_parameter::set_can_serial_number(const char* value, size_t size) {
+  set_has_can_serial_number();
+  can_serial_number_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(),
+      ::std::string(reinterpret_cast<const char*>(value), size));
+  // @@protoc_insertion_point(field_set_pointer:Communication_can_proto.Communication_can_parameter.can_serial_number)
+}
+inline ::std::string* Communication_can_parameter::mutable_can_serial_number() {
+  set_has_can_serial_number();
+  // @@protoc_insertion_point(field_mutable:Communication_can_proto.Communication_can_parameter.can_serial_number)
+  return can_serial_number_.MutableNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
+}
+inline ::std::string* Communication_can_parameter::release_can_serial_number() {
+  // @@protoc_insertion_point(field_release:Communication_can_proto.Communication_can_parameter.can_serial_number)
+  clear_has_can_serial_number();
+  return can_serial_number_.ReleaseNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
+}
+inline void Communication_can_parameter::set_allocated_can_serial_number(::std::string* can_serial_number) {
+  if (can_serial_number != NULL) {
+    set_has_can_serial_number();
+  } else {
+    clear_has_can_serial_number();
+  }
+  can_serial_number_.SetAllocatedNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), can_serial_number);
+  // @@protoc_insertion_point(field_set_allocated:Communication_can_proto.Communication_can_parameter.can_serial_number)
+}
+
+// required int32 can_device_type = 3 [default = 4];
+inline bool Communication_can_parameter::has_can_device_type() const {
+  return (_has_bits_[0] & 0x00000008u) != 0;
+}
+inline void Communication_can_parameter::set_has_can_device_type() {
+  _has_bits_[0] |= 0x00000008u;
+}
+inline void Communication_can_parameter::clear_has_can_device_type() {
+  _has_bits_[0] &= ~0x00000008u;
+}
+inline void Communication_can_parameter::clear_can_device_type() {
+  can_device_type_ = 4;
+  clear_has_can_device_type();
+}
+inline ::google::protobuf::int32 Communication_can_parameter::can_device_type() const {
+  // @@protoc_insertion_point(field_get:Communication_can_proto.Communication_can_parameter.can_device_type)
+  return can_device_type_;
+}
+inline void Communication_can_parameter::set_can_device_type(::google::protobuf::int32 value) {
+  set_has_can_device_type();
+  can_device_type_ = value;
+  // @@protoc_insertion_point(field_set:Communication_can_proto.Communication_can_parameter.can_device_type)
+}
+
+// required int32 can_port_id = 4 [default = 2];
+inline bool Communication_can_parameter::has_can_port_id() const {
+  return (_has_bits_[0] & 0x00000010u) != 0;
+}
+inline void Communication_can_parameter::set_has_can_port_id() {
+  _has_bits_[0] |= 0x00000010u;
+}
+inline void Communication_can_parameter::clear_has_can_port_id() {
+  _has_bits_[0] &= ~0x00000010u;
+}
+inline void Communication_can_parameter::clear_can_port_id() {
+  can_port_id_ = 2;
+  clear_has_can_port_id();
+}
+inline ::google::protobuf::int32 Communication_can_parameter::can_port_id() const {
+  // @@protoc_insertion_point(field_get:Communication_can_proto.Communication_can_parameter.can_port_id)
+  return can_port_id_;
+}
+inline void Communication_can_parameter::set_can_port_id(::google::protobuf::int32 value) {
+  set_has_can_port_id();
+  can_port_id_ = value;
+  // @@protoc_insertion_point(field_set:Communication_can_proto.Communication_can_parameter.can_port_id)
+}
+
+// required int32 can_baud_rate = 5 [default = 1000];
+inline bool Communication_can_parameter::has_can_baud_rate() const {
+  return (_has_bits_[0] & 0x00000004u) != 0;
+}
+inline void Communication_can_parameter::set_has_can_baud_rate() {
+  _has_bits_[0] |= 0x00000004u;
+}
+inline void Communication_can_parameter::clear_has_can_baud_rate() {
+  _has_bits_[0] &= ~0x00000004u;
+}
+inline void Communication_can_parameter::clear_can_baud_rate() {
+  can_baud_rate_ = 1000;
+  clear_has_can_baud_rate();
+}
+inline ::google::protobuf::int32 Communication_can_parameter::can_baud_rate() const {
+  // @@protoc_insertion_point(field_get:Communication_can_proto.Communication_can_parameter.can_baud_rate)
+  return can_baud_rate_;
+}
+inline void Communication_can_parameter::set_can_baud_rate(::google::protobuf::int32 value) {
+  set_has_can_baud_rate();
+  can_baud_rate_ = value;
+  // @@protoc_insertion_point(field_set:Communication_can_proto.Communication_can_parameter.can_baud_rate)
+}
+
+// -------------------------------------------------------------------
+
+// Communication_can_parameter_all
+
+// repeated .Communication_can_proto.Communication_can_parameter communication_can_parameters = 1;
+inline int Communication_can_parameter_all::communication_can_parameters_size() const {
+  return communication_can_parameters_.size();
+}
+inline void Communication_can_parameter_all::clear_communication_can_parameters() {
+  communication_can_parameters_.Clear();
+}
+inline const ::Communication_can_proto::Communication_can_parameter& Communication_can_parameter_all::communication_can_parameters(int index) const {
+  // @@protoc_insertion_point(field_get:Communication_can_proto.Communication_can_parameter_all.communication_can_parameters)
+  return communication_can_parameters_.Get(index);
+}
+inline ::Communication_can_proto::Communication_can_parameter* Communication_can_parameter_all::mutable_communication_can_parameters(int index) {
+  // @@protoc_insertion_point(field_mutable:Communication_can_proto.Communication_can_parameter_all.communication_can_parameters)
+  return communication_can_parameters_.Mutable(index);
+}
+inline ::Communication_can_proto::Communication_can_parameter* Communication_can_parameter_all::add_communication_can_parameters() {
+  // @@protoc_insertion_point(field_add:Communication_can_proto.Communication_can_parameter_all.communication_can_parameters)
+  return communication_can_parameters_.Add();
+}
+inline ::google::protobuf::RepeatedPtrField< ::Communication_can_proto::Communication_can_parameter >*
+Communication_can_parameter_all::mutable_communication_can_parameters() {
+  // @@protoc_insertion_point(field_mutable_list:Communication_can_proto.Communication_can_parameter_all.communication_can_parameters)
+  return &communication_can_parameters_;
+}
+inline const ::google::protobuf::RepeatedPtrField< ::Communication_can_proto::Communication_can_parameter >&
+Communication_can_parameter_all::communication_can_parameters() const {
+  // @@protoc_insertion_point(field_list:Communication_can_proto.Communication_can_parameter_all.communication_can_parameters)
+  return communication_can_parameters_;
+}
+
+#ifdef __GNUC__
+  #pragma GCC diagnostic pop
+#endif  // __GNUC__
+// -------------------------------------------------------------------
+
+
+// @@protoc_insertion_point(namespace_scope)
+
+}  // namespace Communication_can_proto
+
+// @@protoc_insertion_point(global_scope)
+
+#endif  // PROTOBUF_communication_5fcan_2eproto__INCLUDED

src/robot_control/src/can_card/communication_can/communication_can.proto

@@ -0,0 +1,16 @@
+syntax = "proto2";
+package Communication_can_proto;
+
+message Communication_can_parameter
+{
+    required int32  can_device_id = 1;                      //can设备ID
+    required string can_serial_number = 2;                  //can设备条形码(20个char)
+    required int32  can_device_type = 3 [default = 4];      //can设备类型, linux版本默认为4 ,  #define VCI_USBCAN2		4
+    required int32  can_port_id = 4 [default = 2] ;         //can设备通道口, 0:开启can0通道口, 1:开启can1通道口, 2:开启双通道口
+    required int32  can_baud_rate = 5 [default = 1000] ;    //can设备的波特率, (默认 1000 Kbps)
+}
+
+message Communication_can_parameter_all
+{
+    repeated Communication_can_parameter        communication_can_parameters=1;
+}

src/robot_control/src/can_card/communication_can/communication_can_base.cpp

@@ -0,0 +1,731 @@
+//
+// Created by huli on 2020/7/25.
+//
+
+#include "communication_can_base.h"
+
+Communication_can_base::Communication_can_base()
+{
+	m_initialization_flag = false;
+	m_communication_statu = COMMUNICATION_UNKNOW;
+
+	mp_receive_data_thread = NULL;
+	mp_analysis_data_thread = NULL;
+	mp_send_data_thread = NULL;
+	mp_encapsulate_data_thread = NULL;
+
+	m_analysis_cycle_time = 1;//默认1000ms,就自动解析(接受list)
+	m_encapsulate_cycle_time = 3;//默认1000ms,就自动发送一次状态信息
+}
+
+Communication_can_base::~Communication_can_base()
+{
+	communication_uninit();
+}
+
+
+//初始化 通信 模块。如下三选一
+Error_manager Communication_can_base::communication_init()
+{
+	LOG(INFO) << " ---Communication_can_base::communication_init() run--- "<< this;
+
+	return  communication_init_from_protobuf(COMMUNICATION_CAN_PARAMETER_PATH);
+}
+
+//初始化 通信 模块。从文件读取
+Error_manager Communication_can_base::communication_init_from_protobuf(std::string prototxt_path)
+{
+	Communication_can_proto::Communication_can_parameter_all t_communication_can_parameter_all;
+	if(!  proto_tool::read_proto_param(prototxt_path,t_communication_can_parameter_all) )
+	{
+		return Error_manager(CAN_READ_PROTOBUF_ERROR,MINOR_ERROR,
+							 "Communication_can_base read_proto_param  failed");
+	}
+
+	return communication_init_from_protobuf(t_communication_can_parameter_all);
+}
+
+//初始化 通信 模块。从protobuf读取
+Error_manager Communication_can_base::communication_init_from_protobuf(Communication_can_proto::Communication_can_parameter_all& communication_can_parameter_all)
+{
+	LOG(INFO) << " ---Communication_can_base::communication_init_from_protobuf() run--- "<< this;
+	Error_manager t_error;
+
+	int can_device_count = communication_can_parameter_all.communication_can_parameters_size();
+
+	//检查can的数量
+	if ( can_device_count <= 0 )
+	{
+		return Error_manager(Error_code::CAN_INIT_ERROR, Error_level::MINOR_ERROR,
+							" Communication_can_base::communication_init_from_protobuf error, can_device_count <= 0 ");
+	}
+	else
+	{
+		for (int i = 0; i < can_device_count; ++i)
+		{
+
+
+			//按照prototxt的参数,创建can设备
+			Can_device t_can_device;
+			t_can_device.can_device_id = communication_can_parameter_all.communication_can_parameters(i).can_device_id();
+			t_can_device.can_handle_id = -1;
+			t_can_device.can_serial_number = communication_can_parameter_all.communication_can_parameters(i).can_serial_number();
+			t_can_device.can_device_type = communication_can_parameter_all.communication_can_parameters(i).can_device_type();
+			t_can_device.can_baud_rate = communication_can_parameter_all.communication_can_parameters(i).can_baud_rate();
+			if ( t_can_device.can_device_type != VCI_USBCAN2 )
+			{
+			    return Error_manager(Error_code::CAN_INIT_ERROR, Error_level::MINOR_ERROR,
+			    					" Communication_can_base::communication_init_from_protobuf error,  can_device_type != VCI_USBCAN2");
+			}
+			//can通道口设置, 如果can_port_id等于CAN_PORT_NUMBER, 则开启所有通道, 否则开启指定的单通道
+			int t_can_port_id = communication_can_parameter_all.communication_can_parameters(i).can_port_id();
+			if ( t_can_port_id == CAN_PORT_NUMBER )
+			{
+				for (int j = 0; j < CAN_PORT_NUMBER; ++j)
+				{
+					t_can_device.can_port_flag[j] = true;
+				}
+			}
+			else
+			{
+				for (int j = 0; j < CAN_PORT_NUMBER; ++j)
+				{
+					if ( j == t_can_port_id )
+					{
+						t_can_device.can_port_flag[j] = true;
+					}
+					else
+					{
+						t_can_device.can_port_flag[j] = false;
+					}
+				}
+			}
+
+			//查重
+			if ( m_can_device_map.find(t_can_device.can_device_id) == m_can_device_map.end() )
+			{
+				//can设备添加进map
+				m_can_device_map[t_can_device.can_device_id] = t_can_device;
+			}
+			else
+			{
+				//把后者忽略掉, 提示一级故障
+				LOG(INFO) << " Communication_can_base::communication_init_from_protobuf can_device_id repeat "<< this;
+				t_error.error_manager_reset(Error_code::CAN_INDEX_REPEAT, Error_level::NEGLIGIBLE_ERROR,
+			    					" Communication_can_base::communication_init_from_protobuf can_device_id repeat ");
+			}
+		}
+	}
+
+	//查找所有连接的USB_CAN卡设备信息
+	VCI_BOARD_INFO p_board_information [50];
+	int t_board_num=VCI_FindUsbDevice2(p_board_information);
+	//双循环比较条形码, 找到对应的控制句柄
+	for (auto iter = m_can_device_map.begin(); iter != m_can_device_map.end(); ++iter) 
+	{
+		int i = 0;
+		for ( ; i < t_board_num; ++i)
+		{
+			if ( iter->second.can_serial_number == p_board_information[i].str_Serial_Num )
+			{
+				iter->second.can_handle_id = i;
+				break;	//找到了, 就提前结束内部的循环,
+			}
+		}
+		//判断是否找到, 就是检查前面的break是否执行
+		if ( i == t_board_num )
+		{
+			char buf[256] = {0};
+			sprintf(buf, " can_device can not find,  can_serial_number =  %s\n", iter->second.can_serial_number.c_str());
+		    LOG(INFO) << " can_device can not find,  can_serial_number =  "<< iter->second.can_serial_number << this;
+		    return Error_manager(Error_code::CAN_INIT_ERROR, Error_level::MINOR_ERROR,
+								 buf);
+		}
+	}
+
+	//启动can卡
+	t_error = can_run();
+	if ( t_error != Error_code::SUCCESS )
+	{
+		return t_error;
+	}
+	//启动通信, run thread
+	t_error = communication_run();
+	if ( t_error != Error_code::SUCCESS )
+	{
+		return t_error;
+	}
+
+	return Error_code::SUCCESS;
+}
+
+
+
+//启动can卡,
+Error_manager Communication_can_base::can_run()
+{
+	for (auto iter = m_can_device_map.begin(); iter != m_can_device_map.end(); ++iter)
+	{
+		//此函数用以打开设备。注意一个设备只能打开一次。
+		if(VCI_OpenDevice(iter->second.can_device_type, iter->second.can_handle_id,0) != 1 )
+		{
+			return Error_manager(Error_code::CAN_RUN_ERROR, Error_level::MINOR_ERROR,
+								 " VCI_OpenDevice error ");
+		}
+		m_initialization_flag = true;
+
+		//初始化参数，严格参数二次开发函数库说明书。
+		VCI_INIT_CONFIG config;
+		config.AccCode=0;
+		config.AccMask=0xFFFFFFFF;
+		config.Filter=1;//接收所有帧
+		switch ( iter->second.can_baud_rate )
+		{
+		    case 10:
+		    {
+				config.Timing0=0x31;
+				config.Timing1=0x1C;
+		        break;
+		    }
+			case 20:
+			{
+				config.Timing0=0x18;
+				config.Timing1=0x1C;
+				break;
+			}
+			case 40:
+			{
+				config.Timing0=0x87;
+				config.Timing1=0xFF;
+				break;
+			}
+			case 50:
+			{
+				config.Timing0=0x09;
+				config.Timing1=0x1C;
+				break;
+			}
+			case 80:
+			{
+				config.Timing0=0x83;
+				config.Timing1=0xFF;
+				break;
+			}
+			case 100:
+			{
+				config.Timing0=0x04;
+				config.Timing1=0x1C;
+				break;
+			}
+			case 125:
+			{
+				config.Timing0=0x03;
+				config.Timing1=0x1C;
+				break;
+			}
+			case 200:
+			{
+				config.Timing0=0x81;
+				config.Timing1=0xFA;
+				break;
+			}
+			case 250:
+			{
+				config.Timing0=0x01;
+				config.Timing1=0x1C;
+				break;
+			}
+			case 400:
+			{
+				config.Timing0=0x80;
+				config.Timing1=0xFA;
+				break;
+			}
+			case 500:
+			{
+				config.Timing0=0x00;
+				config.Timing1=0x1C;
+				break;
+			}
+			case 666:
+			{
+				config.Timing0=0x80;
+				config.Timing1=0xB6;
+				break;
+			}
+			case 800:
+			{
+				config.Timing0=0x00;
+				config.Timing1=0x16;
+				break;
+			}
+			case 1000:
+			{
+				config.Timing0=0x00;
+				config.Timing1=0x14;
+				break;
+			}
+		    default:
+		    {
+				return Error_manager(Error_code::CAN_RUN_ERROR, Error_level::MINOR_ERROR,
+									 " can_baud_rate error ");
+		        break;
+		    }
+		}
+		config.Mode=0;//正常模式
+
+
+		for (int i = 0; i < CAN_PORT_NUMBER; ++i)
+		{
+			if ( iter->second.can_port_flag[i] )
+			{
+				//此函数用以初始化指定的CAN通道。有多个CAN通道时,需要多次调用。
+				if(VCI_InitCAN(iter->second.can_device_type,iter->second.can_handle_id,i,&config) != 1)
+				{
+					VCI_CloseDevice(iter->second.can_device_type,iter->second.can_handle_id);
+					return Error_manager(Error_code::CAN_RUN_ERROR, Error_level::MINOR_ERROR,
+										 " VCI_InitCAN error ");
+				}
+				//此函数用以启动CAN卡的某一个CAN通道。有多个CAN通道时,需要多次调用。
+				if(VCI_StartCAN(iter->second.can_device_type,iter->second.can_handle_id,i) != 1)
+				{
+					VCI_CloseDevice(iter->second.can_device_type,iter->second.can_handle_id);
+					return Error_manager(Error_code::CAN_RUN_ERROR, Error_level::MINOR_ERROR,
+										 " VCI_StartCAN error ");
+				}
+			}
+		}
+	}
+	return Error_code::SUCCESS;
+}
+
+//启动通信, run thread
+Error_manager Communication_can_base::communication_run()
+{
+	m_communication_statu = COMMUNICATION_READY;
+	//启动4个线程。
+	//接受线程默认循环, 内部的nn_recv进行等待,
+	m_receive_condition.reset(false, false, false);
+	mp_receive_data_thread = new std::thread(&Communication_can_base::receive_data_thread, this);
+	//解析线程默认循环, 内部的wait_and_pop进行等待,
+	m_analysis_data_condition.reset(false, true, false);
+	mp_analysis_data_thread = new std::thread(&Communication_can_base::analysis_data_thread, this);
+	//发送线程默认循环, 内部的wait_and_pop进行等待,
+	m_send_data_condition.reset(false, true, false);
+	mp_send_data_thread = new std::thread(&Communication_can_base::send_data_thread, this);
+	//封装线程默认等待, ...., 超时 ms, 超时后主动 封装心跳和状态信息,
+	m_encapsulate_data_condition.reset(false, false, false);
+	mp_encapsulate_data_thread = new std::thread(&Communication_can_base::encapsulate_data_thread, this);
+
+	return Error_code::SUCCESS;
+}
+
+
+//反初始化 通信 模块。
+Error_manager Communication_can_base::communication_uninit()
+{
+	//终止list，防止 wait_and_pop 阻塞线程。
+	m_receive_data_list.termination_list();
+	m_send_data_list.termination_list();
+
+	//杀死4个线程，强制退出
+	if (mp_receive_data_thread)
+	{
+		m_receive_condition.kill_all();
+	}
+	if (mp_analysis_data_thread)
+	{
+		m_analysis_data_condition.kill_all();
+	}
+	if (mp_send_data_thread)
+	{
+		m_send_data_condition.kill_all();
+	}
+	if (mp_encapsulate_data_thread)
+	{
+		m_encapsulate_data_condition.kill_all();
+	}
+	//回收4个线程的资源
+	if (mp_receive_data_thread)
+	{
+		mp_receive_data_thread->join();
+		delete mp_receive_data_thread;
+		mp_receive_data_thread = NULL;
+	}
+	if (mp_analysis_data_thread)
+	{
+		mp_analysis_data_thread->join();
+		delete mp_analysis_data_thread;
+		mp_analysis_data_thread = 0;
+	}
+	if (mp_send_data_thread)
+	{
+		mp_send_data_thread->join();
+		delete mp_send_data_thread;
+		mp_send_data_thread = NULL;
+	}
+	if (mp_encapsulate_data_thread)
+	{
+		mp_encapsulate_data_thread->join();
+		delete mp_encapsulate_data_thread;
+		mp_encapsulate_data_thread = NULL;
+	}
+
+	//清空list
+	m_receive_data_list.clear_and_delete();
+	m_send_data_list.clear_and_delete();
+
+	m_communication_statu = COMMUNICATION_UNKNOW;
+
+	if ( m_initialization_flag )
+	{
+		for (auto iter = m_can_device_map.begin(); iter != m_can_device_map.end(); ++iter)
+		{
+			VCI_CloseDevice(iter->second.can_device_type,iter->second.can_handle_id);//关闭设备。
+		}
+		m_initialization_flag = false;
+	}
+
+
+	return Error_code::SUCCESS;
+}
+
+//封装消息, 需要子类重载
+Error_manager Communication_can_base::encapsulate_msg(Can_message* p_msg)
+{
+	Can_message* tp_msg = new Can_message(*p_msg);
+	bool is_push = m_send_data_list.push(tp_msg);
+	if ( is_push == false )
+	{
+		delete(tp_msg);
+		tp_msg = NULL;
+		return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+							 " Communication_can_base::encapsulate_msg error ");
+	}
+	return Error_code::SUCCESS;
+}
+
+
+void Communication_can_base::set_analysis_cycle_time(unsigned int analysis_cycle_time)
+{
+	m_analysis_cycle_time = analysis_cycle_time;
+}
+void Communication_can_base::set_encapsulate_cycle_time(unsigned int encapsulate_cycle_time)
+{
+	m_encapsulate_cycle_time = encapsulate_cycle_time;
+}
+
+
+
+
+
+
+//mp_receive_data_thread 接受线程执行函数，
+//receive_data_thread 内部线程负责接受消息
+void Communication_can_base::receive_data_thread()
+{
+	LOG(INFO) << " Communication_can_base::receive_data_thread start "<< this;
+
+	//接收缓存, 重复利用, 为了防止频繁的分配和释放
+	VCI_CAN_OBJ tp_receive_buf[3000];//接收缓存，设为3000为佳。
+	int t_receive_size=0;			//实际接受帧数
+
+	//通信接受线程, 负责接受socket消息, 并存入 m_receive_data_list
+	while (m_receive_condition.is_alive())
+	{
+		m_receive_condition.wait_for_ex(std::chrono::microseconds(100));
+		if ( m_receive_condition.is_alive() )
+		{
+			std::this_thread::yield();
+			//遍历所有设备, 接受can消息
+			for (auto iter = m_can_device_map.begin(); iter != m_can_device_map.end(); ++iter)
+			{
+				//遍历所有通道口, 接受can消息
+				for (int i = 0; i < CAN_PORT_NUMBER; ++i)
+				{
+					if ( iter->second.can_port_flag[i] )
+					{
+						{//这个大括号表示只对 recv 和 send 加锁, 不要因为后面的复杂逻辑影响通信效率
+							std::unique_lock<std::mutex> lk(m_mutex);
+							t_receive_size = VCI_Receive(iter->second.can_device_type, iter->second.can_handle_id, i, tp_receive_buf, 3000, 0);
+						}
+						for (int j = 0; j < t_receive_size; ++j)
+						{
+							Can_message * tp_can_message = new Can_message;
+							tp_can_message->can_device_id = iter->second.can_device_id;
+							tp_can_message->can_port_id = i;
+							tp_can_message->can_object = tp_receive_buf[j];
+							if ( check_msg(tp_can_message) == SUCCESS )
+							{
+								bool is_push = m_receive_data_list.push(tp_can_message);
+
+								//push成功之后, tp_can_message 内存的管理权限交给链表, 如果失败就要回收内存
+								if ( is_push )
+								{
+									//唤醒解析线程一次,
+									//m_analysis_data_condition.notify_all(false, true);
+									//注:解析线程是常开的, 使用list里面的wait_and_pop来阻塞等待.
+									//push里面会让 wait_and_pop 通过的
+								}
+								else
+								{
+									//push 失败, 就要回收内存
+									delete(tp_can_message);
+									tp_can_message = NULL;
+									LOG(INFO) << " m_receive_data_list.push(tp_can_message) error "<< this;
+								}
+							}
+							else
+							{
+								//check_msg 失败, 就要回收内存
+								delete(tp_can_message);
+								tp_can_message = NULL;
+							}
+						}
+					}
+				}
+			}
+		}
+	}
+
+	LOG(INFO) << " Communication_can_base::receive_data_thread end "<< this;
+	return;
+}
+
+//检查消息是否有效, 判断这条消息是不是给我的.
+Error_manager Communication_can_base::check_msg(Can_message*  p_msg)
+{
+	return Error_code::SUCCESS;
+
+	//通过 p_msg 判断这条消息是不是给我的.
+	//子类重载时, 增加自己模块的判断逻辑, 以后再写.
+//	if ( p_msg->can_device_id == 0
+//	&& p_msg->can_port_id == 0
+//	&& p_msg->can_object.ID == 0x123)
+//	{
+//		return Error_code::SUCCESS;
+//	}
+//	else
+//	{
+//		//无效的消息,
+//		return Error_code::INVALID_MESSAGE;
+//	}
+}
+
+
+//mp_analysis_data_thread 解析线程执行函数，
+//analysis_data_thread 内部线程负责解析消息
+void Communication_can_base::analysis_data_thread()
+{
+	LOG(INFO) << " Communication_can_base::analysis_data_thread start "<< this;
+
+	Error_manager t_error;
+	//通信解析线程, 负责巡检m_receive_data_list, 并解析和处理消息
+	while (m_analysis_data_condition.is_alive())
+	{
+		m_analysis_data_condition.wait();
+		if ( m_analysis_data_condition.is_alive() )
+		{
+			std::this_thread::yield();
+			//如果解析线程被主动唤醒, 那么就表示 收到新的消息, 那就遍历整个链表
+			Can_message* tp_msg = NULL;
+			//这里 wait_and_pop 会使用链表内部的 m_data_cond 条件变量来控制等待,
+			//接受线程使用push的时候, 会唤解析醒线程并通过等待, 此时 m_analysis_data_condition 是一直通过的.
+			//如果需要退出, 那么就要 m_receive_data_list.termination_list();	和 m_analysis_data_condition.kill_all();
+			bool is_pop = m_receive_data_list.wait_and_pop(tp_msg);
+			if ( is_pop )
+			{
+				if ( tp_msg != NULL )
+				{
+					//检查消息是否可以被处理
+					t_error = check_executer(tp_msg);
+					if ( t_error == SUCCESS)
+					{
+						//处理消息
+						t_error = execute_msg(tp_msg);
+						//执行结果不管, 由执行者进行特殊处理, 通信层不管执行者的错误返回
+					}
+					//check_executer 失败, 也不做处理,
+
+					//处理完, 需要回收内存
+					delete(tp_msg);
+					tp_msg = NULL;
+				}
+			}
+			//没有取出, 那么应该就是 m_termination_flag 结束了
+		}
+	}
+
+	LOG(INFO) << " Communication_can_base::analysis_data_thread end "<< this;
+	return;
+}
+
+
+//检查执行者的状态, 判断能否处理这条消息, 需要子类重载
+Error_manager Communication_can_base::check_executer(Can_message*  p_msg)
+{
+	//检查对应模块的状态, 判断是否可以处理这条消息
+
+	return Error_code::SUCCESS;	
+}
+
+//处理消息
+Error_manager Communication_can_base::execute_msg(Can_message*  p_msg)
+{
+	//先将 p_msg 转化为 对应的格式, 使用对应模块的protobuf来二次解析
+	// 不能一直使用 Can_message*  p_msg, 这个是要销毁的
+	//然后处理这个消息, 就是调用对应模块的 execute 接口函数
+	//执行结果不管, 如果需要答复, 那么对应模块 在自己内部 封装一条消息发送即可.
+	//子类重载, 需要完全重写, 以后再写.
+
+	//注注注注注意了, 本模块只是用来做通信,
+	//在做处理消息的时候, 可能会调用执行者的接口函数,
+	//这里不应该长时间阻塞或者处理复杂的逻辑,
+	//请执行者另开线程来处理任务.
+
+	std::cout << "---------execute_msg---------------" << std::endl;
+	std::cout << "p_msg->can_device_id = " << p_msg->can_device_id << std::endl;
+	std::cout << "p_msg->can_port_id = " << p_msg->can_port_id << std::endl;
+	std::cout << "p_msg->ID = " << p_msg->can_object.ID << std::endl;
+	std::cout << "p_msg->Data = "  ;
+	for (int i = 0; i < p_msg->can_object.DataLen; ++i)
+	{
+		std::cout << " "<< p_msg->can_object.Data[i] ;
+	}
+	std::cout <<  std::endl;
+
+	return Error_code::SUCCESS;
+}
+
+
+
+//mp_send_data_thread 发送线程执行函数，
+//send_data_thread 内部线程负责发送消息
+void Communication_can_base::send_data_thread()
+{
+	LOG(INFO) << " Communication_can_base::send_data_thread start "<< this;
+
+	//通信发送线程, 负责巡检m_send_data_list, 并发送消息
+	while (m_send_data_condition.is_alive())
+	{
+		m_send_data_condition.wait();
+		if ( m_send_data_condition.is_alive() )
+		{
+			std::this_thread::yield();
+
+			Can_message* tp_msg = NULL;
+			//这里 wait_and_pop 会使用链表内部的 m_data_cond 条件变量来控制等待,
+			//封装线程使用push的时候, 会唤醒线程并通过等待, 此时 m_send_data_condition 是一直通过的.
+			//如果需要退出, 那么就要 m_send_data_list.termination_list();	和 m_send_data_condition.kill_all();
+			bool is_pop = m_send_data_list.wait_and_pop(tp_msg);
+			if ( is_pop )
+			{
+				if ( tp_msg != NULL )
+				{
+					if ( m_can_device_map.find(tp_msg->can_device_id) != m_can_device_map.end() )
+					{
+						if ( m_can_device_map[tp_msg->can_device_id].can_port_flag[tp_msg->can_port_id] )
+						{
+							//这个大括号表示只对 recv 和 send 加锁, 不要因为后面的复杂逻辑影响通信效率
+							std::unique_lock<std::mutex> lk(m_mutex);
+
+							VCI_Transmit(m_can_device_map[tp_msg->can_device_id].can_device_type,
+										 m_can_device_map[tp_msg->can_device_id].can_handle_id,
+										 tp_msg->can_port_id, &(tp_msg->can_object), 1);
+						}
+						
+					}
+					
+					delete(tp_msg);
+					tp_msg = NULL;
+				}
+			}
+			//没有取出, 那么应该就是 m_termination_flag 结束了
+
+		}
+	}
+
+	LOG(INFO) << " Communication_can_base::send_data_thread end "<< this;
+	return;
+}
+
+
+
+//mp_encapsulate_data_thread 封装线程执行函数，
+//encapsulate_data_thread 内部线程负责封装消息
+void Communication_can_base::encapsulate_data_thread()
+{
+	LOG(INFO) << " Communication_can_base::encapsulate_data_thread start "<< this;
+
+	//通信封装线程, 负责定时封装消息, 并存入 m_send_data_list
+	while (m_encapsulate_data_condition.is_alive())
+	{
+		bool t_pass_flag = m_encapsulate_data_condition.wait_for_millisecond(m_encapsulate_cycle_time);
+
+		if ( m_encapsulate_data_condition.is_alive() )
+		{
+			std::this_thread::yield();
+			//如果封装线程被主动唤醒, 那么就表示 需要主动发送消息,
+			if ( t_pass_flag )
+			{
+				//主动发送消息,
+			}
+				//如果封装线程超时通过, 那么就定时封装心跳和状态信息
+			else
+			{
+				encapsulate_send_data();
+			}
+		}
+	}
+
+	LOG(INFO) << " Communication_can_base::encapsulate_data_thread end "<< this;
+	return;
+}
+
+
+//定时封装发送消息, 一般为心跳和状态信息, 需要子类重载
+Error_manager Communication_can_base::encapsulate_send_data()
+{
+	//定时发送
+	
+//	return SUCCESS;
+
+
+
+	Can_message* tp_msg = new Can_message;
+	tp_msg->can_device_id = 0;
+	tp_msg->can_port_id = 1;
+	tp_msg->can_object.ID = 0x123;
+	tp_msg->can_object.SendType = 0;
+	tp_msg->can_object.RemoteFlag = 0;
+	tp_msg->can_object.ExternFlag = 0;
+	tp_msg->can_object.DataLen = 8;
+	for(int i = 0; i < 8; i++)
+	{
+		tp_msg->can_object.Data[i] = i+63;
+	}
+
+	std::cout << "---------encapsulate_send_data---------------" << std::endl;
+	std::cout << "p_msg->can_device_id = " << tp_msg->can_device_id << std::endl;
+	std::cout << "p_msg->can_port_id = " << tp_msg->can_port_id << std::endl;
+	std::cout << "p_msg->ID = " << tp_msg->can_object.ID << std::endl;
+	std::cout << "p_msg->Data = "  ;
+	for (int i = 0; i < tp_msg->can_object.DataLen; ++i)
+	{
+		std::cout << " "<< tp_msg->can_object.Data[i] ;
+	}
+	std::cout <<  std::endl;
+
+	bool is_push = m_send_data_list.push(tp_msg);
+	if ( is_push == false )
+	{
+		delete(tp_msg);
+		tp_msg = NULL;
+		return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+							 " Communication_can_base::encapsulate_msg error ");
+	}
+	return Error_code::SUCCESS;
+
+}
+
+

src/robot_control/src/can_card/communication_can/communication_can_base.h

@@ -0,0 +1,157 @@
+
+/*
+ *
+ * Communication_can_base   can通信模块的基类,
+ * 用户从这个基类继承, 初始化之后, 便可以自动进行通信
+ *
+ *
+ *
+ * */
+
+#ifndef CAN_TEST_COMMUNICATION_CAN_BASE_H
+#define CAN_TEST_COMMUNICATION_CAN_BASE_H
+
+#include "../can_SDK/controlcan.h"
+#include "../error_code/error_code.h"
+#include "../tool/thread_safe_list.h"
+#include "../tool/thread_condition.h"
+#include "../tool/proto_tool.h"
+#include "../communication_can/communication_can.pb.h"
+
+#include <glog/logging.h>
+#include <string>
+#include <map>
+#include <mutex>
+
+
+//默认can通信的配置参数路径
+#define COMMUNICATION_CAN_PARAMETER_PATH "/home/youchen/extra_space/agv_200731/src/robot_control/src/can_card/setting/communication_can.prototxt"
+
+//can卡通道数量, 默认2个通道
+#define CAN_PORT_NUMBER 			2
+//can设备信息
+struct Can_device
+{
+	int 		can_device_id;						//can设备ID, 自定义的(从prototxt导入)
+	int			can_handle_id;						//can句柄ID, 系统自动分配, 从0开始, 往后++
+	std::string can_serial_number;					//can设备条形码(20个char)
+	int 		can_device_type;					//can设备类型, linux版本默认为4 ,  #define VCI_USBCAN2		4
+	bool  		can_port_flag[CAN_PORT_NUMBER];		//can设备 2个通道口的使能标记位
+	int 		can_baud_rate;						//can设备的波特率
+};
+
+//can消息
+struct Can_message
+{
+	int 			can_device_id;			//can设备ID
+	int  			can_port_id;			//can通道ID
+	_VCI_CAN_OBJ	can_object;				//can消息结构体, SDK里面的
+};
+
+
+//can通信模块的基类,
+class Communication_can_base
+{
+	//通信状态
+	enum Communication_statu
+	{
+		COMMUNICATION_UNKNOW		=0,	        //通信状态 未知
+		COMMUNICATION_READY			=1,			//通信状态 正常
+
+		COMMUNICATION_FAULT			=3,         //通信状态 错误
+	};
+
+
+public:
+	Communication_can_base();
+	Communication_can_base(const Communication_can_base& other)= delete;
+	Communication_can_base& operator =(const Communication_can_base& other)= delete;
+	~Communication_can_base();
+
+public://API functions
+	//初始化 通信 模块。如下三选一
+	virtual Error_manager communication_init();
+	//初始化 通信 模块。从文件读取
+	virtual Error_manager communication_init_from_protobuf(std::string prototxt_path);
+	//初始化 通信 模块。从protobuf读取
+	virtual Error_manager communication_init_from_protobuf(Communication_can_proto::Communication_can_parameter_all& communication_can_parameter_all);
+
+	//反初始化 通信 模块。
+	virtual Error_manager communication_uninit();
+
+	//封装消息, 需要子类重载
+	virtual Error_manager encapsulate_msg(Can_message* p_msg);
+
+public://get or set member variable
+	void set_analysis_cycle_time(unsigned int analysis_cycle_time);
+	void set_encapsulate_cycle_time(unsigned int encapsulate_cycle_time);
+
+protected:
+	//启动can卡,
+	virtual Error_manager can_run();
+	//启动通信, run thread
+	virtual Error_manager communication_run();
+
+	//mp_receive_data_thread 接受线程执行函数，
+	//receive_data_thread 内部线程负责接受消息
+	void receive_data_thread();
+
+	//检查消息是否有效, 判断这条消息是不是给我的.
+	virtual Error_manager check_msg(Can_message* p_msg);
+
+	//mp_analysis_data_thread 解析线程执行函数，
+	//analysis_data_thread 内部线程负责解析消息
+	void analysis_data_thread();
+
+	//检查执行者的状态, 判断能否处理这条消息, 需要子类重载
+	virtual Error_manager check_executer(Can_message* p_msg);
+
+	//处理消息, 需要子类重载
+	virtual Error_manager execute_msg(Can_message* p_msg);
+
+	//mp_send_data_thread 发送线程执行函数，
+	//send_data_thread 内部线程负责发送消息
+	void send_data_thread();
+
+	//mp_encapsulate_data_thread 封装线程执行函数，
+	//encapsulate_data_thread 内部线程负责封装消息
+	void encapsulate_data_thread();
+
+	//定时封装发送消息, 一般为心跳和状态信息, 需要子类重载
+	virtual Error_manager encapsulate_send_data();
+
+public:
+
+
+
+
+protected://member variable
+	bool								m_initialization_flag;			//是否初始化的标记位
+
+	std::map<int, Can_device>			m_can_device_map;				//can设备的map, 索引为设备ID
+	std::mutex 							m_mutex;						//can设备的锁
+
+	//通信状态
+	Communication_statu 				m_communication_statu;			//通信状态
+
+	//接受模块,
+	Thread_safe_list<Can_message*>		m_receive_data_list; 			//接受的list容器
+	std::thread*						mp_receive_data_thread;    		//接受的线程指针
+	Thread_condition					m_receive_condition;			//接受的条件变量
+	std::thread*						mp_analysis_data_thread;    	//解析的线程指针
+	Thread_condition					m_analysis_data_condition;		//解析的条件变量
+	unsigned int 						m_analysis_cycle_time;			//自动解析的时间周期
+
+	//发送模块,
+	Thread_safe_list<Can_message*>		m_send_data_list;				//发送的list容器
+	std::thread*						mp_send_data_thread;    		//发送的线程指针
+	Thread_condition					m_send_data_condition;			//发送的条件变量
+	std::thread*						mp_encapsulate_data_thread;    	//封装的线程指针
+	Thread_condition					m_encapsulate_data_condition;	//封装的条件变量
+	unsigned int 						m_encapsulate_cycle_time;		//自动封装的时间周期
+
+};
+
+
+
+#endif //CAN_TEST_COMMUNICATION_CAN_BASE_H

src/robot_control/src/can_card/dtype.h

@@ -0,0 +1,40 @@
+//
+// Created by zx on 2020/4/28.
+//
+
+#ifndef AGV_DTYPE_H
+#define AGV_DTYPE_H
+namespace agv
+{
+/**
+ * 位姿数据类型
+ */
+    class Pose
+    {
+    public:
+        Pose();
+        Pose(double x,double y,double theta);
+        double x();
+        double y();
+        double theta();
+
+    protected:
+        double m_x;
+        double m_y;
+        double m_theta;
+    };
+
+
+    /*
+     * agv各项驱动数据
+     */
+#define MOTOR_NUM   2
+    typedef struct AGV_DRIVER_DATE
+    {
+        double front_motor_data[MOTOR_NUM]; //前车电机数据
+        double rear_motor_data[MOTOR_NUM];  //后车电机数据
+    }Driver_data;
+
+}
+
+#endif //AGV_DTYPE_H

src/robot_control/src/can_card/error_code/error_code.cpp

@@ -0,0 +1,537 @@
+
+//Error_code是错误码的底层通用模块，
+//功能：用作故障分析和处理。
+
+//用法：所有的功能接口函数return错误管理类，
+//然后上层判断分析错误码，并进行故障处理。
+
+
+
+#include "error_code.h"
+
+/////////////////////////////////////////////
+//构造函数
+Error_manager::Error_manager()
+{
+    m_error_code = SUCCESS;
+    m_error_level = NORMAL;
+    pm_error_description = 0;
+    m_description_length = 0;
+    return ;
+}
+//拷贝构造
+Error_manager::Error_manager(const Error_manager & error_manager)
+{
+    this->m_error_code = error_manager.m_error_code;
+    this->m_error_level = error_manager.m_error_level;
+    pm_error_description = NULL;
+    m_description_length = 0;
+    reallocate_memory_and_copy_string(error_manager.pm_error_description, error_manager.m_description_length);
+    return ;
+}
+//赋值构造
+Error_manager::Error_manager(Error_code error_code, Error_level error_level,
+    const char* p_error_description, int description_length)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    pm_error_description = NULL;
+    m_description_length = 0;
+    reallocate_memory_and_copy_string(p_error_description, description_length);
+    return ;
+}
+//赋值构造
+Error_manager::Error_manager(Error_code error_code, Error_level error_level , std::string & error_aggregate_string)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    pm_error_description = NULL;
+    m_description_length = 0;
+    reallocate_memory_and_copy_string(error_aggregate_string);
+    return ;
+}
+//析构函数
+Error_manager::~Error_manager()
+{
+    free_description();
+}
+
+//初始化
+void Error_manager::error_manager_init()
+{
+    error_manager_clear_all();
+    return;
+}
+//初始化
+void Error_manager::error_manager_init(Error_code error_code, Error_level error_level, const char* p_error_description, int description_length)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    reallocate_memory_and_copy_string(p_error_description, description_length);
+    return ;
+}
+//初始化
+void Error_manager::error_manager_init(Error_code error_code, Error_level error_level , std::string & error_aggregate_string)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    reallocate_memory_and_copy_string(error_aggregate_string);
+    return ;
+}
+//重置
+void Error_manager::error_manager_reset(Error_code error_code, Error_level error_level, const char* p_error_description, int description_length)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    reallocate_memory_and_copy_string(p_error_description, description_length);
+    return ;
+}
+//重置
+void Error_manager::error_manager_reset(Error_code error_code, Error_level error_level , std::string & error_aggregate_string)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    reallocate_memory_and_copy_string(error_aggregate_string);
+    return ;
+}
+//重置
+void Error_manager::error_manager_reset(const Error_manager & error_manager)
+{
+    this->m_error_code = error_manager.m_error_code;
+    this->m_error_level = error_manager.m_error_level;
+    reallocate_memory_and_copy_string(error_manager.pm_error_description, error_manager.m_description_length);
+    return ;
+}
+//清除所有内容
+void Error_manager::error_manager_clear_all()
+{
+    m_error_code = SUCCESS;
+    m_error_level = NORMAL;
+    free_description();
+}
+
+//重载=
+Error_manager& Error_manager::operator=(const Error_manager & error_manager)
+{
+    error_manager_reset(error_manager);
+}
+//重载=，支持Error_manager和Error_code的直接转化，会清空错误等级和描述
+Error_manager& Error_manager::operator=(Error_code error_code)
+{
+    error_manager_clear_all();
+    set_error_code(error_code);
+}
+//重载==
+bool Error_manager::operator==(const Error_manager & error_manager)
+{
+    is_equal_error_manager(error_manager);
+}
+//重载==，支持Error_manager和Error_code的直接比较
+bool Error_manager::operator==(Error_code error_code)
+{
+    if(m_error_code == error_code)
+    {
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+//重载!=
+bool Error_manager::operator!=(const Error_manager & error_manager)
+{
+    ! is_equal_error_manager(error_manager);
+}
+//重载!=，支持Error_manager和Error_code的直接比较
+bool Error_manager::operator!=(Error_code error_code)
+{
+    if(m_error_code != error_code)
+    {
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+//重载<<，支持cout<<
+std::ostream & operator<<(std::ostream &out, Error_manager &error_manager)
+{
+	out << error_manager.to_string();
+	return out;
+}
+
+//获取错误码
+Error_code Error_manager::get_error_code()
+{
+    return m_error_code;
+}
+//获取错误等级
+Error_level Error_manager::get_error_level()
+{
+    return m_error_level;
+}
+//获取错误描述的指针，（浅拷贝）
+char* Error_manager::get_error_description()
+{
+    return pm_error_description;
+}
+
+int Error_manager::get_description_length()
+{
+	return m_description_length;
+}
+
+//复制错误描述，（深拷贝）
+//output：p_error_description     错误描述的字符串指针，不可以为NULL，必须要有实际的内存
+//output:description_length      错误描述的字符串长度，不可以为0，长度最好足够大，一般256即可。
+void Error_manager::copy_error_description(const char* p_error_description, int description_length)
+{
+    if(p_error_description != NULL && pm_error_description != NULL)
+    {
+        char *pt_source = (char *)p_error_description;
+        char* pt_destination = pm_error_description;
+
+        int t_length_min = m_description_length;
+        if(m_description_length > description_length)
+        {
+            t_length_min = description_length;
+        }
+
+        for(int i=0;i<t_length_min; i++)
+        {
+            *pt_destination = *pt_source;
+            pt_destination++;
+            pt_source++;
+        }
+    }
+
+    return;
+}
+//复制错误描述，（深拷贝）
+//output：error_description_string     错误描述的string
+void Error_manager::copy_error_description(std::string & error_description_string)
+{
+    if( (!error_description_string.empty() ) && pm_error_description != NULL)
+    {
+        error_description_string = pm_error_description;
+    }
+    return;
+}
+
+//设置错误码
+void Error_manager::set_error_code(Error_code error_code)
+{
+    m_error_code = error_code;
+    return;
+}
+//比较错误等级并升级，取高等级的结果
+void Error_manager::set_error_level_up(Error_level error_level)
+{
+    if(m_error_level < error_level)
+    {
+        m_error_level = error_level;
+    }
+    return;
+}
+//比较错误等级并降级，取低等级的结果
+void Error_manager::set_error_level_down(Error_level error_level)
+{
+    if(m_error_level > error_level)
+    {
+        m_error_level = error_level;
+    }
+    return;
+}
+//错误等级，设定到固定值
+void Error_manager::set_error_level_location(Error_level error_level)
+{
+    m_error_level = error_level;
+    return;
+}
+//设置错误描述
+void Error_manager::set_error_description(const char* p_error_description, int description_length)
+{
+    reallocate_memory_and_copy_string(p_error_description, description_length);
+    return ;
+}
+//设置错误描述
+void Error_manager::set_error_description(std::string & error_description_string)
+{
+    reallocate_memory_and_copy_string(error_description_string);
+    return ;
+}
+
+//尾部追加错误描述
+void Error_manager::add_error_description(const char* p_error_description, int description_length)
+{
+    if(p_error_description !=NULL)
+    {
+        char* pt_description_front = pm_error_description;
+        int t_description_front_length = m_description_length;
+        char* pt_description_back = (char *)p_error_description;
+        int t_description_back_length = 0;
+
+        if(description_length == 0)
+        {
+            t_description_back_length = 0;
+            while (*pt_description_back != '\0')
+            {
+                t_description_back_length++;
+                pt_description_back++;
+            }
+            t_description_back_length++;
+            pt_description_back = (char *)p_error_description;
+        }
+        else
+        {
+            t_description_back_length = description_length;
+        }
+
+        int t_description_new_length = t_description_front_length + 5 + t_description_back_length - 1;
+        char* pt_description_new =  (char*) malloc(t_description_new_length );
+
+        sprintf(pt_description_new, "%s ### %s", pt_description_front, pt_description_back);
+        free_description();
+        pm_error_description = pt_description_new;
+        m_description_length = t_description_new_length;
+    }
+    return ;
+}
+//尾部追加错误描述
+void Error_manager::add_error_description(std::string & error_description_string)
+{
+    if( ! error_description_string.empty() )
+    {
+        std::string t_description_string = pm_error_description ;
+        t_description_string += (" ### "+ error_description_string);
+
+        reallocate_memory_and_copy_string(t_description_string);
+    }
+}
+
+//比较错误是否相同，
+// 注：只比较错误码和等级
+bool Error_manager::is_equal_error_manager(const Error_manager & error_manager)
+{
+    if(this->m_error_code == error_manager.m_error_code
+       && this->m_error_level == error_manager.m_error_level)
+    {
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+//比较并覆盖错误，讲低级错误转为字符串存放于描述中，
+//如果错误相同，则保留this的，将输入参数转入描述。
+void Error_manager::compare_and_cover_error(const Error_manager & error_manager)
+{
+    if(this->m_error_code == SUCCESS)
+    {
+        error_manager_reset(error_manager);
+    }
+    else if (error_manager.m_error_code == SUCCESS)
+    {
+		return;
+    }
+    else
+    {
+        Error_manager t_error_manager_new;
+        char* pt_string_inside = NULL;
+        int t_string_inside_length = 0;
+        if(this->m_error_level < error_manager.m_error_level)
+        {
+            t_string_inside_length = ERROR_NAMAGER_TO_STRING_FRONT_LENGTH + this->m_description_length;
+            pt_string_inside = (char*)malloc(t_string_inside_length);
+            translate_error_to_string(pt_string_inside, t_string_inside_length);
+
+            error_manager_reset(error_manager);
+            add_error_description(pt_string_inside,t_string_inside_length);
+        }
+        else
+        {
+            t_string_inside_length = ERROR_NAMAGER_TO_STRING_FRONT_LENGTH + error_manager.m_description_length;
+            pt_string_inside = (char*)malloc(t_string_inside_length);
+			((Error_manager & )error_manager).translate_error_to_string(pt_string_inside, t_string_inside_length);
+
+            add_error_description(pt_string_inside,t_string_inside_length);
+        }
+    }
+}
+//比较并覆盖错误，讲低级错误转为字符串存放于描述中，
+//如果错误相同，则保留this的，将输入参数转入描述。
+void Error_manager::compare_and_cover_error( Error_manager * p_error_manager)
+{
+	if(this->m_error_code == SUCCESS)
+	{
+		error_manager_reset(*p_error_manager);
+	}
+	else if (p_error_manager->m_error_code == SUCCESS)
+	{
+		//
+	}
+	else
+	{
+		Error_manager t_error_manager_new;
+		char* pt_string_inside = NULL;
+		int t_string_inside_length = 0;
+		if(this->m_error_level < p_error_manager->m_error_level)
+		{
+			t_string_inside_length = ERROR_NAMAGER_TO_STRING_FRONT_LENGTH + this->m_description_length;
+			pt_string_inside = (char*)malloc(t_string_inside_length);
+			translate_error_to_string(pt_string_inside, t_string_inside_length);
+
+			error_manager_reset(*p_error_manager);
+			add_error_description(pt_string_inside,t_string_inside_length);
+		}
+		else
+		{
+			t_string_inside_length = ERROR_NAMAGER_TO_STRING_FRONT_LENGTH + p_error_manager->m_description_length;
+			pt_string_inside = (char*)malloc(t_string_inside_length);
+			p_error_manager->translate_error_to_string(pt_string_inside, t_string_inside_length);
+
+			add_error_description(pt_string_inside,t_string_inside_length);
+		}
+	}
+}
+
+//将所有的错误信息，格式化为字符串，用作日志打印。
+//output：p_error_description     错误汇总的字符串指针，不可以为NULL，必须要有实际的内存
+//output:description_length      错误汇总的字符串长度，不可以为0，长度最好足够大，一般256即可。
+void Error_manager::translate_error_to_string(char* p_error_aggregate, int aggregate_length )
+{
+    char t_string_array[ERROR_NAMAGER_TO_STRING_FRONT_LENGTH] = {0};
+    char* pt_index_front = t_string_array;
+    char* pt_index_back = pm_error_description;
+    char* pt_index = p_error_aggregate;
+
+    sprintf(t_string_array, "error_code:0x%08x, error_level:%02d, error_description:", m_error_code , m_error_level );
+
+    int t_length_min = m_description_length + ERROR_NAMAGER_TO_STRING_FRONT_LENGTH -1;
+    if(t_length_min > aggregate_length)
+    {
+        t_length_min = aggregate_length;
+    }
+
+    for(int i=0;i<t_length_min; i++)
+    {
+        if(i < ERROR_NAMAGER_TO_STRING_FRONT_LENGTH -1)
+        {
+            *pt_index = *pt_index_front;
+            pt_index++;
+            pt_index_front++;
+        }
+        else
+        {
+            *pt_index = *pt_index_back;
+            pt_index++;
+            pt_index_back++;
+        }
+    }
+}
+//output：error_description_string     错误汇总的string
+void Error_manager::translate_error_to_string(std::string & error_aggregate_string)
+{
+    char t_string_array[ERROR_NAMAGER_TO_STRING_FRONT_LENGTH] = {0};
+    sprintf(t_string_array, "error_code:0x%08x, error_level:%02d, error_description:", m_error_code , m_error_level );
+
+    error_aggregate_string = t_string_array ;
+    if(pm_error_description != NULL)
+    {
+        error_aggregate_string += pm_error_description;
+    }
+    else
+    {
+        //error_aggregate_string += "NULL";
+    }
+}
+//错误码转字符串的简易版，可支持cout<<
+//return     错误汇总的string
+std::string Error_manager::to_string()
+{
+    std::string t_string;
+    translate_error_to_string(t_string);
+    return t_string;
+}
+
+
+
+
+//释放错误描述的内存，
+void Error_manager::free_description()
+{
+    if(pm_error_description != NULL)
+    {
+        free (pm_error_description);
+        pm_error_description = NULL;
+    }
+    m_description_length = 0;
+}
+
+//重新分配错误描述的内存，并从外部拷贝新的（深拷贝）
+//input：p_error_description     错误描述的字符串指针，可以为NULL，
+//input:description_length      错误描述的字符串长度，如果为0，则从p_error_description里面获取有效的长度
+void Error_manager::reallocate_memory_and_copy_string(const char* p_error_description, int description_length)
+{
+    free_description();
+
+    if(p_error_description != NULL)
+    {
+        char* pt_source = (char *)p_error_description;
+        char* pt_destination = NULL;
+
+        if(description_length == 0)
+        {
+            m_description_length = 0;
+            while (*pt_source != '\0')
+            {
+                m_description_length++;
+                pt_source++;
+            }
+            m_description_length++;
+            pt_source = (char *)p_error_description;
+        }
+        else
+        {
+            m_description_length = description_length;
+        }
+
+        pm_error_description =  (char*) malloc(m_description_length );
+        pt_destination = pm_error_description;
+
+        for(int i=0;i<m_description_length; i++)
+        {
+            *pt_destination = *pt_source;
+            pt_destination++;
+            pt_source++;
+        }
+    }
+
+    return;
+}
+
+
+//重新分配错误描述的内存，并从外部拷贝新的（深拷贝）
+//input：error_aggregate_string     错误描述的string
+void Error_manager::reallocate_memory_and_copy_string(std::string & error_aggregate_string)
+{
+    free_description();
+
+    if( ! error_aggregate_string.empty())
+    {
+        m_description_length = error_aggregate_string.length()+1;
+
+        pm_error_description =  (char*) malloc( m_description_length );
+
+        strcpy(pm_error_description ,   error_aggregate_string.c_str()  );
+    }
+}
+
+
+
+
+

src/robot_control/src/can_card/error_code/error_code.h

@@ -0,0 +1,493 @@
+
+//Error_code是错误码的底层通用模块，
+//功能：用作故障分析和处理。
+
+//用法：所有的功能接口函数return错误管理类，
+//然后上层判断分析错误码，并进行故障处理。
+
+
+
+#ifndef TEST_ERROR_ERROR_CODE_H
+#define TEST_ERROR_ERROR_CODE_H
+
+#include <string>
+#include <string.h>
+#include<iostream>
+
+//错误管理类转化为字符串 的前缀，固定长度为58
+//这个是由显示格式来确定的，如果要修改格式或者 Error_code长度超过8位，Error_level长度超过2位，折需要重新计算
+#define ERROR_NAMAGER_TO_STRING_FRONT_LENGTH   58
+
+//进程加锁的状态，
+enum Lock_status
+{
+    UNLOCK      = 0,
+    LOCK        = 1,
+};
+
+//设备使能状态，
+enum Able_status
+{
+    UNABLE      = 0,
+    ENABLE      = 1,
+};
+
+//数据是否为空
+enum Empty_status
+{
+    NON_EMPTY   = 0,
+    EMPTY       = 1,
+};
+
+
+//错误码的枚举，用来做故障分析
+enum Error_code
+{
+    //成功，没有错误，默认值0
+    SUCCESS                         = 0x00000000,
+
+
+    //基本错误码，
+    ERROR                           = 0x00000001,//错误
+    PARTIAL_SUCCESS                 = 0x00000002,//部分成功
+    WARNING                         = 0x00000003,//警告
+    FAILED                          = 0x00000004,//失败
+
+    NO_DATA                         = 0x00000010,//没有数据，传入参数容器内部没有数据时，
+	INVALID_MESSAGE					= 0x00000011, //无效的消息,
+    RESPONSE_TIMEOUT                = 0x00000012,
+
+    POINTER_IS_NULL                 = 0x00000101,//空指针
+    PARAMETER_ERROR                 = 0x00000102,//参数错误，传入参数不符合规范时，
+    POINTER_MALLOC_FAIL             = 0x00000103,//手动分配内存失败
+
+    CLASS_BASE_FUNCTION_CANNOT_USE  = 0x00000201,//基类函数不允许使用，必须使用子类的
+
+	CONTAINER_IS_TERMINATE			= 0x00000301,//容器被终止
+
+
+
+
+//    错误码的规范，
+//    错误码是int型，32位，十六进制。
+//    例如0x12345678
+//    12表示功能模块，例如:laser雷达模块               	框架制定
+//    34表示文件名称，例如:laser_livox.cpp             框架制定
+//    56表示具体的类，例如:class laser_livox           个人制定
+//    78表示类的函数，例如:laser_livox::start();       个人制定
+//    注：错误码的制定从1开始，不要从0开始，
+//        0用作错误码的基数，用来位运算，来判断错误码的范围。
+
+//    laser扫描模块
+    LASER_ERROR_BASE                = 0x01000000,
+
+//    laser_base基类
+	LASER_BASE_ERROR_BASE			= 0x01010000,
+    LASER_TASK_PARAMETER_ERROR      = 0x01010001,   //雷达基类模块, 任务输入参数错误
+    LASER_CONNECT_FAILED,							//雷达基类模块, 连接失败
+	LASER_START_FAILED,								//雷达基类模块, 开始扫描失败
+	LASER_CHECK_FAILED,								//雷达基类模块, 检查失败
+	LASER_STATUS_BUSY,								//雷达基类模块, 状态正忙
+	LASER_STATUS_ERROR,								//雷达基类模块, 状态错误
+	LASER_TASK_OVER_TIME,							//雷达基类模块, 任务超时
+	LASER_QUEUE_ERROR,								//雷达基类模块, 数据缓存错误
+
+
+//    laser_livox.cpp的错误码
+    LIVOX_ERROR_BASE                = 0x01020000,
+    LIVOX_START_FAILE,								//livox模块，开始扫描失败
+	LIVOX_TASK_TYPE_ERROR,							//livox模块，任务类型错误
+	lIVOX_CANNOT_PUSH_DATA,							//livox模块，不能添加扫描的数据
+	lIVOX_CHECK_FAILED,								//livox模块，检查失败
+	lIVOX_STATUS_BUSY,								//livox模块，状态正忙
+	lIVOX_STATUS_ERROR,								//livox模块，状态错误
+
+	//laser_manager 雷达管理模块
+	LASER_MANAGER_ERROR_BASE						= 0x01030000,
+	LASER_MANAGER_READ_PROTOBUF_ERROR,				//雷达管理模块，读取参数错误
+	LASER_MANAGER_STATUS_BUSY,						//雷达管理模块，状态正忙
+	LASER_MANAGER_STATUS_ERROR,						//雷达管理模块，状态错误
+	LASER_MANAGER_TASK_TYPE_ERROR,					//雷达管理模块，任务类型错误
+	LASER_MANAGER_IS_NOT_READY,						//雷达管理模块，不在准备状态
+	LASER_MANAGER_LASER_INDEX_ERRPR,				//雷达管理模块，雷达索引错误，编号错误。
+	LASER_MANAGER_TASK_OVER_TIME,					//雷达管理模块，任务超时
+	LASER_MANAGER_LASER_INDEX_REPEAT,				//雷达管理模块，需要扫描的雷达索引重复,可忽略的错误,提示作用
+
+//livox_driver 雷达livox驱动模块
+	LIVOX_DRIVER_ERROR_BASE							= 0x01040000,
+	LIVOX_DRIVER_SN_REPEAT,							//livox驱动模块, 雷达广播码重复,可忽略的错误,提示作用
+	LIVOX_DRIVER_SN_ERROR,							//livox驱动模块, 雷达广播码错误
+	LIVOX_SKD_INIT_FAILED,							//livox驱动模块, livox_sdk初始化失败
+	LIVOX_DRIVER_NOT_READY,							//livox驱动模块, livox没有准备好.
+
+
+
+
+     //PLC error code  ...
+    PLC_ERROR_BASE                  = 0x02010000,
+    PLC_UNKNOWN_ERROR,
+    PLC_EMPTY_TASK,
+    PLC_IP_PORT_ERROR,
+    PLC_SLAVE_ID_ERROR,
+    PLC_CONNECTION_FAILED,
+    PLC_READ_FAILED,
+    PLC_WRITE_FAILED,
+    PLC_NOT_ENOUGH_DATA_ERROR,
+
+
+
+    //locate 定位模块,
+	LOCATER_ERROR_BASE                				= 0x03000000,
+
+	//LASER_MANAGER 定位管理模块
+	LOCATER_MANAGER_ERROR_BASE                		= 0x03010000,
+	LOCATER_MANAGER_READ_PROTOBUF_ERROR,				//定位管理模块，读取参数错误
+	LOCATER_MANAGER_STATUS_BUSY,						//定位管理模块，状态正忙
+	LOCATER_MANAGER_STATUS_ERROR,						//定位管理模块，状态错误
+	LOCATER_MANAGER_TASK_TYPE_ERROR,					//定位管理模块，任务类型错误
+	LOCATER_MANAGER_IS_NOT_READY,						//定位管理模块，不在准备状态
+	LOCATER_MANAGER_CLOUD_MAP_ERROR,					//定位管理模块，任务输入点云map的error
+	LOCATE_MANAGER_TASK_OVER_TIME,
+
+
+	//Locater.cpp error from 0x0301000-0x030100FF
+	LOCATER_TASK_INIT_CLOUD_EMPTY ,
+    LOCATER_TASK_ERROR,
+    LOCATER_TASK_INPUT_CLOUD_UNINIT,
+    LOCATER_INPUT_CLOUD_EMPTY,
+    LOCATER_YOLO_UNINIT,
+    LOCATER_POINTSIFT_UNINIT,
+    LOCATER_3DCNN_UNINIT,
+    LOCATER_INPUT_YOLO_CLOUD_EMPTY,
+    LOCATER_Y_OUT_RANGE_BY_PLC,
+    LOCATER_MEASURE_HEIGHT_CLOUD_UNINIT,
+    LOCATER_MEASURE_HEIGHT_CLOUD_EMPTY,
+    LOCATER_INPUT_CLOUD_UNINIT,
+
+
+    //point sift from 0x03010100-0x030101FF
+    LOCATER_SIFT_INIT_FAILED=0x03010100,
+    LOCATER_SIFT_INPUT_CLOUD_UNINIT,
+	LOCATER_SIFT_INPUT_CLOUD_EMPTY,
+	LOCATER_SIFT_GRID_ERROR,
+	LOCATER_SIFT_SELECT_ERROR,
+	LOCATER_SIFT_CLOUD_VERY_LITTLE,
+	LOCATER_SIFT_CREATE_INPUT_DATA_FAILED,
+	LOCATER_SIFT_PREDICT_FAILED,
+	LOCATER_SIFT_PREDICT_NO_WHEEL_POINT,
+	LOCATER_SIFT_PREDICT_NO_CAR_POINT,
+
+    LOCATER_SIFT_FILTE_OBS_FAILED,
+    LOCATER_SIFT_INPUT_BOX_PARAMETER_FAILED,
+
+//    //yolo from 0x03010200-0x030102FF
+//        LOCATER_YOLO_DETECT_FAILED=0x03010200,
+//    LOCATER_YOLO_DETECT_NO_TARGET,
+//    LOCATER_YOLO_PARAMETER_INVALID,
+//    LOCATER_YOLO_INPUT_CLOUD_UNINIT,
+
+    //3dcnn from 0x03010300-0x030103FF
+    LOCATER_3DCNN_INIT_FAILED=0x03010300,
+    LOCATER_3DCNN_INPUT_CLOUD_UNINIT,
+	LOCATER_3DCNN_INPUT_CLOUD_EMPTY,
+	LOCATER_3DCNN_INPUT_CLOUD_MAP_ERROR,
+	LOCATER_3DCNN_PCA_OUT_ERROR,
+	LOCATER_3DCNN_EXTRACT_RECT_ERROR,
+	LOCATER_3DCNN_RECT_SIZE_ERROR,
+
+    LOCATER_3DCNN_PREDICT_FAILED,
+    LOCATER_3DCNN_VERIFY_RECT_FAILED_3,
+    LOCATER_3DCNN_VERIFY_RECT_FAILED_4,
+    LOCATER_3DCNN_KMEANS_FAILED,
+    LOCATER_3DCNN_IIU_FAILED,
+    LOCATER_3DCNN_PCA_OUT_CLOUD_EMPTY,
+
+    //System_manager error from 0x04010000-0x0401FFFF
+    SYSTEM_READ_PARAMETER_ERROR=0x04010100,
+    SYSTEM_PARAMETER_ERROR,
+    SYSTEM_INPUT_TERMINOR_NO_LASERS,
+
+    //terminor_command_executor.cpp from 0x04010200-0x040102FF
+    TERMINOR_NOT_READY=0x04010200,
+    TERMINOR_INPUT_LASER_NULL,
+    TERMINOR_NOT_CONTAINS_LASER,
+    TERMINOR_INPUT_PLC_NULL,
+    TERMINOR_INPUT_LOCATER_NULL,
+    TERMINOR_CREATE_WORKING_THREAD_FAILED,
+    TERMINOR_FORCE_QUIT,
+    TERMINOR_LASER_TIMEOUT,
+    TERMINOR_POST_PLC_TIMEOUT,
+    TERMINOR_CHECK_RESULTS_ERROR,
+
+    ////Hardware limit from 0x05010000 - 0x0501ffff
+    ///railing.cpp from 0x05010100-0x050101ff
+    HARDWARE_LIMIT_LEFT_RAILING=0x05010100,         //左栏杆限制
+    HARDWARE_LIMIT_RAILING_PARAMETER_ERROR,
+    HARDWARE_LIMIT_RAILING_ERROR,
+    HARDWARE_LIMIT_CENTER_X_LEFT,
+    HARDWARE_LIMIT_CENTER_X_RIGHT,
+    HARDWARE_LIMIT_CENTER_Y_TOP,
+    HARDWARE_LIMIT_CENTER_Y_BOTTOM,
+    HARDWARE_LIMIT_HEIGHT_OUT_RANGE,
+    HARDWARE_LIMIT_ANGLE_OUT_RANGE,
+    //termonal_limit from 0x05010200-0x050102ff
+    HARDWARE_LIMIT_TERMINAL_LEFT_ERROR,
+    HARDWARE_LIMIT_TERMINAL_RIGHT_ERROR,
+    HARDWARE_LIMIT_TERMINAL_LR_ERROR,
+
+
+    //wj_lidar error from 0x06010000-0x0601FFFF
+        WJ_LIDAR_CONNECT_FAILED=0x06010000,
+    WJ_LIDAR_UNINITIALIZED,
+    WJ_LIDAR_READ_FAILED,
+    WJ_LIDAR_WRITE_FAILED,
+    WJ_LIDAR_GET_CLOUD_TIMEOUT,
+
+    //wj lidar protocol error from 0x06020000-0x0602FFFF
+        WJ_PROTOCOL_ERROR_BASE=0x06020000,
+    WJ_PROTOCOL_INTEGRITY_ERROR,
+    WJ_PROTOCOL_PARSE_FAILED,
+    WJ_PROTOCOL_EMPTY_PACKAGE,
+    WJ_PROTOCOL_EXCEED_MAX_SIZE,
+
+    //wj region detect error from 0x06030000-0x0603FFFF
+        WJ_REGION_EMPTY_CLOUD=0x06030000,
+    WJ_REGION_RECTANGLE_ANGLE_ERROR,
+    WJ_REGION_RECTANGLE_SIZE_ERROR,
+    WJ_REGION_RECTANGLE_SYMMETRY_ERROR,
+    WJ_REGION_CLUSTER_SIZE_ERROR,
+
+    //wj manager error from 0x06040000-0x0604FFFF
+    WJ_MANAGER_UNINITIALIZED=0x06040000,
+    WJ_MANAGER_LIDAR_DISCONNECTED,
+    WJ_MANAGER_PLC_DISCONNECTED,
+    WJ_MANAGER_EMPTY_CLOUD,
+
+    WJ_LIDAR_TASK_EMPTY_RESULT=0x06050000,
+    WJ_LIDAR_TASK_EMPTY_TASK,
+    WJ_LIDAR_TASK_WRONG_TYPE,
+    WJ_LIDAR_TASK_INVALID_TASK,
+    WJ_LIDAR_TASK_MEASURE_FAILED,
+
+
+
+    //task module, 任务模块  error from 0x10010000-0x1001FFFF
+	TASK_MODULE_ERROR_BASE 							= 0x10010000,
+	TASK_TYPE_IS_UNKNOW,
+	TASK_NO_RECEIVER,
+
+
+	//Communication module, 通信模块
+	COMMUNICATION_BASE_ERROR_BASE					= 0x11010000,
+	COMMUNICATION_READ_PROTOBUF_ERROR,				//模块，读取参数错误
+	COMMUNICATION_BIND_ERROR,
+	COMMUNICATION_CONNECT_ERROR,
+	COMMUNICATION_ANALYSIS_TIME_OUT,									//解析超时,
+	COMMUNICATION_EXCUTER_IS_BUSY,										//处理器正忙, 请稍等
+
+
+	//system module, 系统模块
+	SYSTEM_EXECUTOR_ERROR_BASE						= 0x12010000,		//系统执行模块,
+	SYSTEM_EXECUTOR_PARSE_ERROR,										//系统执行模块, 解析消息错误
+	SYSTEM_EXECUTOR_STATUS_BUSY,										//系统执行模块, 状态正忙
+	SYSTEM_EXECUTOR_STATUS_ERROR,										//系统执行模块, 状态错误
+	SYSTEM_EXECUTOR_CHECK_ERROR,										//系统执行模块, 检查错误
+
+	LOCATER_MSG_TABLE_NOT_EXIST ,
+    LOCATER_MSG_RESPONSE_TYPE_ERROR,
+    LOCATER_MSG_RESPONSE_INFO_ERROR,
+    LOCATER_MSG_REQUEST_INVALID,
+    LOCATER_MSG_RESPONSE_HAS_NO_REQUEST,
+
+
+	//Communication can module, can通信模块
+	CAN_BASE_ERROR_BASE								= 0x15010000,		//can通信模块错误基地址
+	CAN_READ_PROTOBUF_ERROR,											//can通信模块, 读取参数错误
+	CAN_INDEX_REPEAT,
+	CAN_INIT_ERROR,														//can初始化失败
+	CAN_RUN_ERROR,														//can启动失败
+
+	//motor 电机驱动器模块
+	MOTOR_DRIVER_ERROR_BASE							= 0x16010000,		//电机驱动器模块, 错误基地址
+	MOTOR_DRIVER_DEVICE_ERROR,											//电机驱动器模块, 设备故障
+	MOTOR_DRIVER_COMMUNICATION_ERROR,									//电机驱动器模块, 通信故障
+
+	//motor 电机管理模块
+	MOTOR_MANAGER_ERROR_BASE						= 0x16020000,		//电机管理模块, 错误基地址
+	MOTOR_MANAGER_READ_PROTOBUF_ERROR, 									//电机管理模块, 读取prototxt错误
+	MOTOR_MANAGER_INDEX_REPEAT,											//电机管理模块, 设备索引重复,可忽略的错误,提示作用
+	MOTOR_MANAGER_STATUS_ERROR,
+};
+
+//错误等级，用来做故障处理
+enum Error_level
+{
+//    正常，没有错误，默认值0
+    NORMAL                = 0,
+
+
+//    轻微故障,可忽略的故障，NEGLIGIBLE_ERROR
+//    提示作用，不做任何处理，不影响代码的流程，
+//    用作一些不重要的事件，即使出错也不会影响到系统功能，
+//    例如：文件保存错误，等
+    NEGLIGIBLE_ERROR      = 1,
+
+
+//    一般故障，MINOR_ERROR
+//    用作底层功能函数的错误返回，表示该功能函数执行失败，
+//    返回给应用层之后，需要做故障分析和处理，
+//    例如：雷达数据传输失败，应用层就需要进行重新扫描，或者重连，或者重置参数等。
+    MINOR_ERROR           = 2,
+
+
+//    严重故障，MAJOR_ERROR
+//    用作应用层的任务事件的结果，表示该功能模块失败。
+//    通常是底层函数返回一般故障之后，应用层无法处理并解决故障，此时就要进行故障升级，
+//    从一般故障升级为严重故障，然后进行回退流程，回退已经执行的操作，最终回到故障待机状态。
+//    需要外部清除故障，并复位至正常待机状态，才能恢复功能的使用。
+//    例如：雷达扫描任务失败，且无法自动恢复。
+    MAJOR_ERROR           = 3,
+
+
+//    致命故障，CRITICAL_ERROR
+//    系统出现致命错误。导致系统无法正常运行，
+//    此时系统应该紧急停机，执行紧急流程，快速停机。
+//    此时不允许再执行任何函数和任务指令，防止系统故障更加严重。
+//    也不需要做任何错误处理了，快速执行紧急流程。
+//    例如：内存错误，进程挂死，关键设备失控，监控设备报警，等
+    CRITICAL_ERROR        = 4,
+};
+
+
+class Error_manager
+{
+public://外部接口函数
+    //构造函数
+    Error_manager();
+    //拷贝构造
+    Error_manager(const Error_manager & error_manager);
+    //赋值构造
+    Error_manager(Error_code error_code, Error_level error_level = NORMAL,
+                  const char* p_error_description = NULL, int description_length = 0);
+    //赋值构造
+    Error_manager(Error_code error_code, Error_level error_level , std::string & error_aggregate_string);
+    //析构函数
+    ~Error_manager();
+
+    //初始化
+    void error_manager_init();
+    //初始化
+    void error_manager_init(Error_code error_code, Error_level error_level = NORMAL,
+                            const char* p_error_description = NULL, int description_length = 0);
+    //初始化
+    void error_manager_init(Error_code error_code, Error_level error_level , std::string & error_aggregate_string);
+    //重置
+    void error_manager_reset(Error_code error_code, Error_level error_level = NORMAL,
+                             const char* p_error_description = NULL, int description_length = 0);
+    //重置
+    void error_manager_reset(Error_code error_code, Error_level error_level , std::string & error_aggregate_string);
+    //重置
+    void error_manager_reset(const Error_manager & error_manager);
+    //清除所有内容
+    void error_manager_clear_all();
+
+    //重载=
+    Error_manager& operator=(const Error_manager & error_manager);
+    //重载=，支持Error_manager和Error_code的直接转化，会清空错误等级和描述
+    Error_manager& operator=(Error_code error_code);
+    //重载==
+    bool operator==(const Error_manager & error_manager);
+    //重载==，支持Error_manager和Error_code的直接比较
+    bool operator==(Error_code error_code);
+    //重载!=
+    bool operator!=(const Error_manager & error_manager);
+    //重载!=，支持Error_manager和Error_code的直接比较
+    bool operator!=(Error_code error_code);
+	//重载<<，支持cout<<
+	friend std::ostream & operator<<(std::ostream &out, Error_manager &error_manager);
+
+
+    //获取错误码
+    Error_code get_error_code();
+    //获取错误等级
+    Error_level get_error_level();
+    //获取错误描述的指针，（浅拷贝）
+    char* get_error_description();
+
+	int get_description_length();
+
+    //复制错误描述，（深拷贝）
+    //output：p_error_description     错误描述的字符串指针，不可以为NULL，必须要有实际的内存
+    //output:description_length      错误描述的字符串长度，不可以为0，长度最好足够大，一般256即可。
+    void copy_error_description(const char* p_error_description, int description_length);
+    //复制错误描述，（深拷贝）
+    //output：error_description_string     错误描述的string
+    void copy_error_description(std::string & error_description_string);
+
+    //设置错误码
+    void set_error_code(Error_code error_code);
+    //比较错误等级并升级，取高等级的结果
+    void set_error_level_up(Error_level error_level);
+    //比较错误等级并降级，取低等级的结果
+    void set_error_level_down(Error_level error_level);
+    //错误等级，设定到固定值
+    void set_error_level_location(Error_level error_level);
+    //设置错误描述
+    void set_error_description(const char* p_error_description, int description_length = 0);
+    //设置错误描述
+    void set_error_description(std::string & error_description_string);
+
+    //尾部追加错误描述
+    void add_error_description(const char* p_error_description, int description_length = 0);
+    //尾部追加错误描述
+    void add_error_description(std::string & error_description_string);
+
+    //比较错误是否相同，
+    // 注：只比较错误码和等级
+	bool is_equal_error_manager(const Error_manager & error_manager);
+	//比较并覆盖错误，讲低级错误转为字符串存放于描述中，
+	//如果错误相同，则保留this的，将输入参数转入描述。
+	void compare_and_cover_error(const Error_manager & error_manager);
+	//比较并覆盖错误，讲低级错误转为字符串存放于描述中，
+	//如果错误相同，则保留this的，将输入参数转入描述。
+	void compare_and_cover_error( Error_manager * p_error_manager);
+
+	//将所有的错误信息，格式化为字符串，用作日志打印。
+    //output：p_error_description     错误汇总的字符串指针，不可以为NULL，必须要有实际的内存
+    //output:description_length      错误汇总的字符串长度，不可以为0，长度最好足够大，一般256即可。
+    void translate_error_to_string(char* p_error_aggregate, int aggregate_length);
+    //output：error_description_string     错误汇总的string
+    void translate_error_to_string(std::string & error_aggregate_string);
+    //错误码转字符串的简易版，可支持cout<<
+    //return     错误汇总的string
+    std::string to_string();
+
+
+
+protected:
+    Error_code              m_error_code;               //错误码
+    Error_level             m_error_level;              //错误等级
+    char*                   pm_error_description;       //错误描述
+    int                     m_description_length;       //错误描述的字符长度
+
+protected://内部功能函数
+public:
+    //释放错误描述的内存，
+    void free_description();
+
+    //重新分配错误描述的内存，并从外部拷贝新的（深拷贝）
+    //input：p_error_description     错误描述的字符串指针，可以为NULL，
+    //input:description_length      错误描述的字符串长度，如果为0，则从p_error_description里面获取有效的长度
+    void reallocate_memory_and_copy_string(const char* p_error_description, int description_length = 0);
+
+    //重新分配错误描述的内存，并从外部拷贝新的（深拷贝）
+    //input：error_aggregate_string     错误描述的string
+    void reallocate_memory_and_copy_string(std::string & error_aggregate_string);
+};
+
+
+
+
+#endif //TEST_ERROR_ERROR_CODE_H
+
+

src/robot_control/src/can_card/motor/motor_communication.cpp

@@ -0,0 +1,48 @@
+//
+// Created by huli on 2020/7/29.
+//
+
+#include "motor_communication.h"
+#include "../motor/motor_manager.h"
+
+
+Motor_communication::Motor_communication()
+{
+
+}
+
+Motor_communication::~Motor_communication()
+{
+
+}
+
+
+//检查消息是否有效, 主要检查消息类型和接受者, 判断这条消息是不是给我的.
+Error_manager Motor_communication::check_msg(Can_message*  p_msg)
+{
+	return Motor_manager::get_instance_references().check_msg(p_msg);
+}
+
+//检查执行者的状态, 判断能否处理这条消息, 需要子类重载
+Error_manager Motor_communication::check_executer(Can_message*  p_msg)
+{
+	//只需要总管理的总状态成功就行了, 保证通信正常.
+	return Motor_manager::get_instance_references().check_status();
+}
+
+//处理消息, 需要子类重载
+Error_manager Motor_communication::execute_msg(Can_message* p_msg)
+{
+	return Motor_manager::get_instance_references().execute_msg(p_msg);
+}
+
+//定时封装发送消息,
+Error_manager Motor_communication::encapsulate_send_data()
+{
+	return Motor_manager::get_instance_references().encapsulate_send_data();
+}
+
+
+
+
+

src/robot_control/src/can_card/motor/motor_communication.h

@@ -0,0 +1,46 @@
+//
+// Created by huli on 2020/7/29.
+//
+
+#ifndef CAN_TEST_MOTOR_COMMUNICATION_H
+#define CAN_TEST_MOTOR_COMMUNICATION_H
+
+
+#include "../tool/singleton.h"
+#include "../communication_can/communication_can_base.h"
+
+
+class Motor_communication:public Singleton<Motor_communication>, public Communication_can_base
+{
+// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+   friend class Singleton<Motor_communication>;
+private:
+ // 父类的构造函数必须保护，子类的构造函数必须私有。
+   Motor_communication();
+public:
+    //必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+    Motor_communication(const Motor_communication& other) = delete;
+    Motor_communication& operator =(const Motor_communication& other) = delete;
+    ~Motor_communication();
+public://API functions
+	//检查消息是否有效, 主要检查消息类型和接受者, 判断这条消息是不是给我的.
+	virtual Error_manager check_msg(Can_message* p_msg);
+	//检查执行者的状态, 判断能否处理这条消息, 需要子类重载
+	virtual Error_manager check_executer(Can_message* p_msg);
+	//处理消息, 需要子类重载
+	virtual Error_manager execute_msg(Can_message* p_msg);
+
+	//定时封装发送消息,
+	virtual Error_manager encapsulate_send_data();
+public://get or set member variable
+
+
+protected://member variable
+
+
+private:
+
+};
+
+
+#endif //CAN_TEST_MOTOR_COMMUNICATION_H

src/robot_control/src/can_card/motor/motor_driver.cpp

@@ -0,0 +1,290 @@
+//
+// Created by huli on 2020/7/28.
+//
+
+#include "motor_driver.h"
+#include "../motor/motor_communication.h"
+
+Motor_driver::Motor_driver()
+{
+	m_motor_device_id = -1;
+	m_motor_driver_status = MOTOR_DRIVER_UNKNOW;
+	m_can_device_id = -1;
+	m_can_port_id = -1;
+
+	m_can_send_id = 0;
+	m_can_send_extern_flag = false;
+	m_control_byte = CONTROL_STOP;
+	m_target_velocity = 0;
+
+	m_can_receive_id = 0;
+	m_can_receive_extern_flag = false;
+	m_status_byte = STATUS_UNKNOW;
+	m_actual_velocity = 0;
+	m_receive_time = std::chrono::system_clock::now();
+}
+
+Motor_driver::~Motor_driver()
+{
+	motor_driver_uninit();
+}
+
+Error_manager Motor_driver::motor_driver_init(int motor_device_id, int can_device_id, int can_port_id)
+{
+	m_motor_device_id = motor_device_id;
+	m_motor_driver_status = MOTOR_DRIVER_UNKNOW; //初始化之后,电机驱动器的状态不是ready, 必须唤醒通信之后,才能进入待机.
+	m_can_device_id = can_device_id;
+	m_can_port_id = can_port_id;
+
+	//发送id为0, 就是广播消息, 唤醒can总线上面的所有的电机驱动器
+	m_can_send_id = 0;
+	m_can_send_extern_flag = false;
+	m_control_byte = CONTROL_INIT;
+	m_target_velocity = 0;
+
+	m_can_receive_id = MOTOR_DRIVER_CAN_RECEIVE_MESSAGE_BASE + m_motor_device_id;
+	m_can_receive_extern_flag = false;
+	m_status_byte = STATUS_UNKNOW;
+	m_actual_velocity = 0;
+
+	return Error_code::SUCCESS;
+}
+
+Error_manager Motor_driver::motor_driver_uninit()
+{
+	m_motor_driver_status = MOTOR_DRIVER_UNKNOW;
+
+	m_control_byte = CONTROL_STOP;
+	m_target_velocity = 0;
+
+	return Error_code::SUCCESS;
+}
+
+//刷新状态
+Error_manager Motor_driver::update_status()
+{
+	//根据 接受消息和时间, 刷新状态
+	if ( std::chrono::system_clock::now() - m_receive_time > std::chrono::milliseconds(MOTOR_DRIVER_CAN_COMMUNICATION_TIME_OUT_MS) )
+	{
+		m_motor_driver_status = MOTOR_DRIVER_COMMUNICATION_FAULT;
+		return Error_manager(Error_code::MOTOR_DRIVER_COMMUNICATION_ERROR, Error_level::MINOR_ERROR,
+							" Motor_driver::update_status error ");
+	}
+	else if ( m_status_byte == STATUS_UNKNOW || m_status_byte == STATUS_FAULT)
+	{
+		m_motor_driver_status = MOTOR_DRIVER_DEVICE_FAULT;
+		return Error_manager(Error_code::MOTOR_DRIVER_DEVICE_ERROR, Error_level::MINOR_ERROR,
+							 " Motor_driver::update_status error ");
+	}
+	else if ( m_status_byte == STATUS_READY )
+	{
+		m_motor_driver_status = MOTOR_DRIVER_READY;
+		return Error_code::SUCCESS;
+	}
+	else if ( m_status_byte == STATUS_RUN )
+	{
+		m_motor_driver_status = MOTOR_DRIVER_RUN;
+		return Error_code::SUCCESS;
+	}
+	return Error_code::SUCCESS;
+}
+
+//检查状态
+Error_manager Motor_driver::check_status()
+{
+	return update_status();
+}
+
+//检查消息是否有效, 判断这条消息是不是给我的.
+Error_manager Motor_driver::check_msg(Can_message* p_msg)
+{
+	if ( p_msg->can_device_id == m_can_device_id
+		 && p_msg->can_port_id == m_can_port_id
+		 && p_msg->can_object.ID == m_can_receive_id
+		 && p_msg->can_object.ExternFlag == m_can_receive_extern_flag)
+	{
+		return Error_code::SUCCESS;
+	}
+	else
+	{
+		return Error_manager(Error_code::INVALID_MESSAGE, Error_level::NEGLIGIBLE_ERROR,
+							" INVALID_MESSAGE error ");
+	}
+}
+
+//处理消息
+Error_manager Motor_driver::execute_msg(Can_message* p_msg)
+{
+//	Error_manager t_error;
+	if ( p_msg->can_device_id == m_can_device_id
+		 && p_msg->can_port_id == m_can_port_id
+		 && p_msg->can_object.ID == m_can_receive_id
+		 && p_msg->can_object.ExternFlag == m_can_receive_extern_flag)
+	{
+		//按照通信协议,解析can消息, 2~3字节是频率实际值, (前低后高) 需要转化为速度实际值
+		m_status_byte = (Motor_driver::Status_byte)(p_msg->can_object.Data[0]);
+		short t_frequency= (int)(p_msg->can_object.Data[3])<<8 | (int)(p_msg->can_object.Data[2]);
+		//这里需要 频率和速度的转化公式
+		float temp = ((float)t_frequency)/2605.0f;
+		m_actual_velocity = temp;
+		m_receive_time = std::chrono::system_clock::now();
+
+		//这里不用刷新状态, 保证在使用之前刷新状态就可以了
+		//		update_status();
+
+		//处理任务, 电机驱动器的核心控制流程就在这里
+		return execute_task();
+	}
+	else
+	{
+		return Error_manager(Error_code::INVALID_MESSAGE, Error_level::NEGLIGIBLE_ERROR,
+							 " INVALID_MESSAGE error ");
+	}
+
+	return Error_code::SUCCESS;
+}
+
+//处理任务, 电机驱动器的核心控制流程就在这里
+Error_manager Motor_driver::execute_task()
+{
+	Error_manager t_error;
+
+//	std::cout << "----------huli test----------------" << std::endl;
+//	printf("m_can_send_id = 0x%x, m_control_byte = 0x%x, m_target_velocity = 0x%x, \n",(int)m_can_send_id,(int)m_control_byte, (int)m_target_velocity);
+//	printf("m_can_receive_id = 0x%x, m_status_byte = 0x%x, m_actual_velocity = 0x%x, \n",(int)m_can_receive_id,(int)m_status_byte, (int)m_actual_velocity);
+
+	t_error = update_status();
+	if(t_error != MOTOR_DRIVER_COMMUNICATION_ERROR)
+	{
+		switch ( m_status_byte )
+		{
+		    case STATUS_UNKNOW:
+		    {
+		        ;
+		        break;
+		    }
+		    case STATUS_READY:
+		    {
+		        if ( m_control_byte == CONTROL_INIT )
+		        {//从发送唤醒信息改为正常控制
+					m_can_send_id = MOTOR_DRIVER_CAN_SEND_MESSAGE_BASE + m_motor_device_id;;
+					m_can_send_extern_flag = false;
+					m_control_byte = CONTROL_RUN;
+					m_target_velocity = 0;
+		        }
+		        //正常控制
+		        break;
+		    }
+			case STATUS_RUN:
+			{
+				if ( m_control_byte == CONTROL_INIT )
+				{//从发送唤醒信息改为正常控制
+					m_can_send_id = MOTOR_DRIVER_CAN_SEND_MESSAGE_BASE + m_motor_device_id;;
+					m_can_send_extern_flag = false;
+					m_control_byte = CONTROL_RUN;
+					m_target_velocity = 0;
+				}
+				//正常控制
+				break;
+			}
+			case STATUS_FAULT:
+			{
+				if ( m_control_byte == CONTROL_INIT )
+				{//从发送唤醒信息改为正常控制
+					m_can_send_id = MOTOR_DRIVER_CAN_SEND_MESSAGE_BASE + m_motor_device_id;;
+					m_can_send_extern_flag = false;
+					m_control_byte = CONTROL_STOP;
+					m_target_velocity = 0;
+				}
+				else
+				{
+					//故障状态, 速度锁定为0
+					m_target_velocity = 0;
+				}
+				break;
+			}
+		    default:
+		    {
+
+		        break;
+		    }
+		}
+	}
+	return t_error;
+}
+
+//定时封装发送消息,
+Error_manager Motor_driver::encapsulate_send_data()
+{
+	return Error_code::SUCCESS;
+//	return encapsulate_send_data_once();
+}
+
+//发送can消息, 触发式, 调用本函数就发送一条.
+Error_manager Motor_driver::encapsulate_send_data_once()
+{
+	//定时发送
+	Can_message t_msg;
+	t_msg.can_device_id = m_can_device_id;
+	t_msg.can_port_id = m_can_port_id;
+	t_msg.can_object.ID = m_can_send_id;
+	t_msg.can_object.SendType = 0;
+	t_msg.can_object.RemoteFlag = 0;
+	t_msg.can_object.ExternFlag = 0;
+	t_msg.can_object.DataLen = 4;
+
+	//按照通信协议,解析can消息, 2~3字节是频率实际值, (前低后高)
+	t_msg.can_object.Data[0] = m_control_byte;
+	t_msg.can_object.Data[1] = 0;
+	//这里需要 频率和速度的转化公式
+	float temp = m_target_velocity*2605;
+
+	short t_frequency = (short)temp;
+	t_msg.can_object.Data[2] = t_frequency & 0xff;
+	t_msg.can_object.Data[3] = t_frequency >> 8;
+
+	return Motor_communication::get_instance_references().encapsulate_msg(&t_msg);
+}
+
+
+Motor_driver::Motor_driver_status Motor_driver::get_motor_driver_status()
+{
+	update_status();
+	return m_motor_driver_status;
+}
+
+Error_manager Motor_driver::set_target_velocity(float target_velocity)
+{
+	Error_manager t_error = update_status();
+	if (  t_error== SUCCESS )
+	{
+		m_control_byte = CONTROL_RUN;
+	    m_target_velocity = target_velocity;
+		return encapsulate_send_data_once();
+	}
+	else
+	{
+		return t_error;
+	}
+}
+
+float Motor_driver::get_actual_velocity()
+{
+	return m_actual_velocity;
+}
+
+//复位
+Error_manager Motor_driver::motor_driver_reset()
+{
+	m_control_byte = CONTROL_RESET;
+	m_actual_velocity = 0;
+	return encapsulate_send_data_once();
+}
+
+//急停
+Error_manager 		Motor_driver::motor_driver_stop()
+{
+	m_control_byte = CONTROL_STOP;
+	m_actual_velocity = 0;
+	return encapsulate_send_data_once();
+}

src/robot_control/src/can_card/motor/motor_driver.h

@@ -0,0 +1,116 @@
+//
+// Created by huli on 2020/7/28.
+//
+
+#ifndef CAN_TEST_MOTOR_DRIVER_H
+#define CAN_TEST_MOTOR_DRIVER_H
+
+#include <time.h>
+#include <sys/time.h>
+#include <chrono>
+#include <atomic>
+
+#include "../error_code/error_code.h"
+#include "../communication_can/communication_can_base.h"
+
+//电机驱动器 can通信id的基地址, 实际can id为基地址+电机驱动器设备ID
+#define	MOTOR_DRIVER_CAN_SEND_MESSAGE_BASE		0x200
+#define	MOTOR_DRIVER_CAN_RECEIVE_MESSAGE_BASE	0X180
+
+//电机驱动器 can通信 超时时限, 默认5000ms, 如果长时间没有接受接受消息, 那么就报超时故障
+#define	MOTOR_DRIVER_CAN_COMMUNICATION_TIME_OUT_MS		5000
+
+
+//电机驱动器
+class Motor_driver
+{
+	//电机驱动器 状态
+	enum Motor_driver_status
+	{
+		MOTOR_DRIVER_UNKNOW								= 0,    //未知
+		MOTOR_DRIVER_READY    							= 1,    //待机
+		MOTOR_DRIVER_RUN								= 2,	//运转
+
+		MOTOR_DRIVER_DEVICE_FAULT						= 10,	//设备故障
+		MOTOR_DRIVER_COMMUNICATION_FAULT				= 11,	//通信故障
+	};
+
+	//控制字
+	enum Control_byte
+	{//default CONTROL_STOP = 0
+	    CONTROL_STOP               		= 0x00,    //停止, 急停
+		CONTROL_INIT               		= 0x01,    //初始化, 唤醒通信
+		CONTROL_RUN               		= 0x03,    //启动, 运行, (此时速度生效)
+		CONTROL_RESET					= 0x80,		//复位, 清除故障状态, 变为ready
+	};
+	//状态字
+	enum Status_byte
+	{//default  = 0
+		STATUS_UNKNOW              		= 0,    	//
+		STATUS_READY             		= 0x33,    	//
+		STATUS_RUN						= 0x37,		//
+		STATUS_FAULT					= 0x88,    	//设备故障
+	    
+	};
+
+
+public:
+	Motor_driver();
+	Motor_driver(const Motor_driver& other)= default;
+	Motor_driver& operator =(const Motor_driver& other)= default;
+	~Motor_driver();
+public://API functions
+	Error_manager motor_driver_init(int motor_device_id, int can_device_id, int can_port_id);
+	Error_manager motor_driver_uninit();
+
+	//刷新状态
+	Error_manager update_status();
+	//检查状态
+	Error_manager check_status();
+
+	//检查消息是否有效, 判断这条消息是不是给我的.
+	Error_manager check_msg(Can_message* p_msg);
+	//处理消息
+	Error_manager execute_msg(Can_message* p_msg);
+	//处理任务, 电机驱动器的核心控制流程就在这里
+	Error_manager execute_task();
+	//定时封装发送消息, 循环式
+	Error_manager encapsulate_send_data();
+	//发送can消息, 触发式, 调用本函数就发送一条.
+	Error_manager encapsulate_send_data_once();
+
+public://get or set member variable
+	Motor_driver_status get_motor_driver_status();
+	Error_manager		set_target_velocity(float target_velocity);
+	float 				get_actual_velocity();
+	//复位
+	Error_manager 		motor_driver_reset();
+	//急停
+	Error_manager 		motor_driver_stop();
+protected://member variable
+    int 					m_motor_device_id;		//电机驱动器 设备ID, 自定义, 但是要与驱动器的can通信设置节点id相同
+	Motor_driver_status		m_motor_driver_status;	//电机驱动器 状态
+
+    //can设备 匹配通信接口
+	int 					m_can_device_id;		//can设备ID
+	int  					m_can_port_id;			//can通道ID
+
+	//发送消息
+	std::atomic<int>					m_can_send_id;				//发送can消息的id
+	std::atomic<bool>					m_can_send_extern_flag;		//发送can消息的id是否为扩展帧, (默认false 为标准帧)
+	std::atomic<Control_byte>			m_control_byte;				//控制字
+	std::atomic<float>					m_target_velocity;			//目标速度
+
+	//接受消息
+	std::atomic<int>					m_can_receive_id;			//接受can消息的id
+	std::atomic<bool>					m_can_receive_extern_flag;	//接受can消息的id是否为扩展帧, (默认false 为标准帧)
+	std::atomic<Status_byte>			m_status_byte;				//状态字
+	std::atomic<float>					m_actual_velocity;			//实际速度
+	std::chrono::system_clock::time_point		m_receive_time;				//接收消息的时间点
+
+private:
+
+};
+
+
+#endif //CAN_TEST_MOTOR_DRIVER_H

src/robot_control/src/can_card/motor/motor_manager.cpp

@@ -0,0 +1,162 @@
+//
+// Created by huli on 2020/7/28.
+//
+
+#include "motor_manager.h"
+Motor_manager::Motor_manager()
+{
+	m_motor_manager_status = MOTOR_MANAGER_UNKNOW;
+	m_motor_number = 0;
+}
+Motor_manager::~Motor_manager()
+{
+	motor_manager_uninit();
+}
+
+//初始化 电机驱动器 的总管理 模块。如下三选一
+Error_manager Motor_manager::motor_manager_init()
+{
+	return motor_manager_init_from_protobuf(MOTOR_PARAMETER_PATH);
+}
+//初始化 电机驱动器 的总管理模块。从文件读取
+Error_manager Motor_manager::motor_manager_init_from_protobuf(std::string prototxt_path)
+{
+	Motor_manager_proto::Motor_manager_parameter_all t_motor_manager_parameter_all;
+
+	if(!  proto_tool::read_proto_param(prototxt_path,t_motor_manager_parameter_all) )
+	{
+		return Error_manager(MOTOR_MANAGER_READ_PROTOBUF_ERROR,MINOR_ERROR,"Laser_manager read_proto_param  failed");
+	}
+
+	return motor_manager_init_from_protobuf(t_motor_manager_parameter_all);
+}
+//初始化 电机驱动器 的总管理 模块。从protobuf读取
+Error_manager Motor_manager::motor_manager_init_from_protobuf(Motor_manager_proto::Motor_manager_parameter_all& motor_manager_parameter_all)
+{
+	LOG(INFO) << " --- motor_manager_init ---  "<< this;
+	Error_manager t_error;
+
+	int count = motor_manager_parameter_all.motor_manager_parameters_size();
+
+	for (int i = 0; i < count; ++i)
+	{
+
+		int t_motor_device_id = motor_manager_parameter_all.motor_manager_parameters(i).motor_device_id();
+
+		//查重
+		if ( m_motor_driver_map.find(t_motor_device_id) == m_motor_driver_map.end() )
+		{
+			Motor_driver * tp_motor_driver = new Motor_driver;
+			tp_motor_driver->motor_driver_init(t_motor_device_id,
+											   motor_manager_parameter_all.motor_manager_parameters(i).can_device_id(),
+											   motor_manager_parameter_all.motor_manager_parameters(i).can_port_id() );
+			//添加到map
+			m_motor_driver_map[t_motor_device_id] = tp_motor_driver;
+		}
+		else
+		{
+			LOG(INFO) << " Motor_manager::motor_manager_init_from_protobuf  motor_device_id repeat "<< this;
+			//把后者忽略掉, 提示一级故障
+			t_error.error_manager_reset(MOTOR_MANAGER_INDEX_REPEAT, NEGLIGIBLE_ERROR,
+			"Motor_manager:: motor_device_id repeat");
+		}
+	}
+	m_motor_manager_status = MOTOR_MANAGER_READY;
+	m_motor_number = m_motor_driver_map.size();
+	return t_error;
+}
+//反初始化
+Error_manager Motor_manager::motor_manager_uninit()
+{
+	for (auto iter = m_motor_driver_map.begin(); iter != m_motor_driver_map.end();  )
+	{
+		delete(iter->second);
+		iter = m_motor_driver_map.erase(iter);
+		//注：erase 删除当前 iter 之后返回下一个节点，当前的 iter 无效化，
+		//for循环不用 ++iter
+	}
+
+	m_motor_manager_status = MOTOR_MANAGER_UNKNOW;
+	m_motor_number = 0;
+
+	return Error_code::SUCCESS;
+}
+
+//检查总状态
+Error_manager Motor_manager::check_status()
+{
+	if ( m_motor_manager_status == MOTOR_MANAGER_READY )
+	{
+	    return Error_code::SUCCESS;
+	}
+	else
+	{
+	    return Error_manager(Error_code::MOTOR_MANAGER_STATUS_ERROR, Error_level::MINOR_ERROR,
+	    					" Motor_manager::check_status() error ");
+	}
+}
+//检查消息是否有效, 判断这条消息是不是给我的.
+Error_manager Motor_manager::check_msg(Can_message*  p_msg)
+{
+	//只要有一个检查成功, 那么就返回成功
+	for (auto iter = m_motor_driver_map.begin(); iter != m_motor_driver_map.end(); ++iter)
+	{
+		//每个驱动器只会执行自己的消息, 不是自己的不会执行
+		if ( iter->second->check_msg(p_msg) == SUCCESS	)
+		{
+			return Error_code::SUCCESS;
+		}
+	}
+	return Error_manager(Error_code::INVALID_MESSAGE, Error_level::NEGLIGIBLE_ERROR,
+						 " INVALID_MESSAGE error ");
+}
+//处理消息
+Error_manager Motor_manager::execute_msg(Can_message* p_msg)
+{
+	//只要有一个处理成功, 那么就返回成功
+	for (auto iter = m_motor_driver_map.begin(); iter != m_motor_driver_map.end(); ++iter)
+	{
+		//每个驱动器只会执行自己的消息, 不是自己的不会执行
+		if ( iter->second->execute_msg(p_msg) == SUCCESS	)
+		{
+			return Error_code::SUCCESS;
+		}
+	}
+	return Error_manager(Error_code::INVALID_MESSAGE, Error_level::NEGLIGIBLE_ERROR,
+						 " INVALID_MESSAGE error ");
+}
+
+//定时封装发送消息,
+Error_manager Motor_manager::encapsulate_send_data()
+{
+	Error_manager t_error;
+	for (auto iter = m_motor_driver_map.begin(); iter != m_motor_driver_map.end(); ++iter)
+	{
+		//定时封装发送消息,
+		//必须执行完所有的子驱动器设备, 并将错误汇总
+		t_error.compare_and_cover_error(iter->second->encapsulate_send_data());
+	}
+	return t_error;
+}
+
+Error_manager		Motor_manager::set_target_velocity(int motor_driver_id, float target_velocity)
+{
+	// std::cout<<"set vel"<<std::endl;
+	return m_motor_driver_map[motor_driver_id]->set_target_velocity(target_velocity);
+}
+float 				Motor_manager::get_actual_velocity(int motor_driver_id)
+{
+	// std::cout<<"get vel"<<std::endl;
+	return m_motor_driver_map[motor_driver_id]->get_actual_velocity();
+}
+//复位
+Error_manager 		Motor_manager::motor_driver_reset(int motor_driver_id)
+{
+	return m_motor_driver_map[motor_driver_id]->motor_driver_reset();
+}
+
+//急停
+Error_manager 		Motor_manager::motor_driver_stop(int motor_driver_id)
+{
+	return m_motor_driver_map[motor_driver_id]->motor_driver_stop();
+}