فهرست منبع

add plc angle offset, config A1A2 plc offsets

yct 4 سال پیش
والد
کامیت
bd2a75ca32

+ 16 - 8
setting/velodyne_manager.prototxt

@@ -97,10 +97,10 @@ velodyne_lidars
     rpm:600
     calib
     {
-        r:-0.1517
-        p:1.37069
+        r:-0.1073
+        p:0.65674
         y:86.3796
-        cz:0.0461
+        cz:0.04
     }
 }
 
@@ -166,6 +166,7 @@ region
     border_maxx:1.3
     plc_offsetx:1.913
     plc_offsety:-6.078
+    plc_offset_degree:-89.5
     plc_border_miny:-7.51
     car_min_width:1.55
     car_max_width:1.95
@@ -216,6 +217,7 @@ region
     border_maxx:1.3
     plc_offsetx:-1.8783
     plc_offsety:-6.11489
+    plc_offset_degree:-89.5
     plc_border_miny:-7.51
     car_min_width:1.55
     car_max_width:1.95
@@ -263,6 +265,7 @@ region
 #     border_maxx:1.3
 #     plc_offsetx:0.0
 #     plc_offsety:0.0
+#     plc_offset_degree:0.0
 #     plc_border_miny:-7.51
 #     car_min_width:1.55
 #     car_max_width:1.95
@@ -293,6 +296,7 @@ region
 # }
 
 # 1 region
+# plcx + -0.06
 region
 {
     minx:-1.5
@@ -306,8 +310,9 @@ region
     turnplate_cy:0.0
     border_minx:-1.3
     border_maxx:1.3
-    plc_offsetx:0.0
-    plc_offsety:0.0
+    plc_offsetx:1.93
+    plc_offsety:-6.1368
+    plc_offset_degree:-89.5
     plc_border_miny:-7.51
     car_min_width:1.55
     car_max_width:1.95
@@ -338,9 +343,11 @@ region
 }
 
 # 0 region
+# center -0.03, 0.4653  --->  -1.89, -5.6415
+# pcly + -0.03
 region
 {
-    minx:-1.1
+    minx:-1.5
 	maxx:1.5
 	miny:-2.6
 	maxy:2.3
@@ -351,8 +358,9 @@ region
     turnplate_cy:0.0
     border_minx:-1.3
     border_maxx:1.3
-    plc_offsetx:0.0
-    plc_offsety:0.0
+    plc_offsetx:-1.86
+    plc_offsety:-6.1368
+    plc_offset_degree:-90
     plc_border_miny:-7.51
     car_min_width:1.55
     car_max_width:1.95

+ 1 - 1
system/system_executor.h

@@ -10,7 +10,7 @@
 #include "../error_code/error_code.h"
 #include "../communication/communication_message.h"
 
-#define DISP_TERM_ID 0
+#define DISP_TERM_ID 1
 
 class System_executor:public Singleton<System_executor>
 {

+ 5 - 4
tests/lidar_calib_tools.cpp

@@ -188,18 +188,19 @@ int main()
     // lidar 1 normal [09:52:53] 	- normal: (0.000297,-0.001893,0.999998) z -0.0686
     // 91.3879 0.108841 -0.0143847
 
-    // lidar 2 normal [10:43:07] 	- normal: (0.001131,-0.024040,0.999710) z -0.0461051
+    // lidar 2 normal [16:45:14] 	- normal: (0.000484,-0.022007,0.999758) z -0.0461051
     // 86.3796  1.37069 -0.15165
+    // 86.3802   1.25674 -0.107284
 
     pcl::PointCloud<pcl::PointXYZ>::Ptr t_front, t_back;
     t_front = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
     t_back = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
-    if (!read_pointcloud("/home/youchen/extra_space/chutian/measure/puai_wj_2021/setting/calib/2front_pole.txt", t_front))
+    if (!read_pointcloud("/home/zx/yct/chutian_measure_2021/setting/calib/2front_pole.txt", t_front))
     {
         std::cout << "read front failed." << std::endl;
         return -1;
     }
-    if(!read_pointcloud("/home/youchen/extra_space/chutian/measure/puai_wj_2021/setting/calib/2back_pole.txt", t_back))
+    if(!read_pointcloud("/home/zx/yct/chutian_measure_2021/setting/calib/2back_pole.txt", t_back))
     {
         std::cout << "read back failed." << std::endl;
         return -1;
@@ -210,7 +211,7 @@ int main()
     // 经过测试,地面法向量不适宜使用杆子获取,还是使用地面环点云拟合获得
     // find_intrinsic((front_direction + back_direction) / 2.0, y_direction);
     // find_intrinsic(Eigen::Vector3d(-0.000392,-0.011935,0.999929), y_direction);
-    find_intrinsic(Eigen::Vector3d(0.001131,-0.024040,0.999710), y_direction);
+    find_intrinsic(Eigen::Vector3d(0.000484,-0.022007,0.999758), y_direction);
 
     return 0;
 

+ 1 - 0
velodyne_lidar/ground_region.cpp

@@ -761,6 +761,7 @@ void Ground_region::thread_measure_func()
                 m_car_wheel_information.car_center_y += m_region.plc_offsety();
                 m_car_wheel_information.uniform_car_x += m_region.plc_offsetx();
                 m_car_wheel_information.uniform_car_y += m_region.plc_offsety();
+                m_car_wheel_information.car_angle += m_region.plc_offset_degree();
 
                 t_wheel_info_stamped.wheel_data = m_car_wheel_information;
                 t_wheel_info_stamped.measure_time = m_detect_update_time;

+ 125 - 85
velodyne_lidar/velodyne_config.pb.cc

@@ -234,6 +234,7 @@ const ::google::protobuf::uint32 TableStruct::offsets[] GOOGLE_PROTOBUF_ATTRIBUT
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::velodyne::Region, border_maxx_),
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::velodyne::Region, plc_offsetx_),
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::velodyne::Region, plc_offsety_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::velodyne::Region, plc_offset_degree_),
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::velodyne::Region, plc_border_miny_),
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::velodyne::Region, car_min_width_),
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::velodyne::Region, car_max_width_),
@@ -262,13 +263,14 @@ const ::google::protobuf::uint32 TableStruct::offsets[] GOOGLE_PROTOBUF_ATTRIBUT
   17,
   18,
   19,
+  20,
 };
 static const ::google::protobuf::internal::MigrationSchema schemas[] GOOGLE_PROTOBUF_ATTRIBUTE_SECTION_VARIABLE(protodesc_cold) = {
   { 0, 12, sizeof(::velodyne::velodyneManagerParams)},
   { 19, 35, sizeof(::velodyne::velodyneLidarParams)},
   { 46, 57, sizeof(::velodyne::CalibParameter)},
   { 63, 70, sizeof(::velodyne::lidarExtrinsic)},
-  { 72, 98, sizeof(::velodyne::Region)},
+  { 72, 99, sizeof(::velodyne::Region)},
 };
 
 static ::google::protobuf::Message const * const file_default_instances[] = {
@@ -318,7 +320,7 @@ void AddDescriptorsImpl() {
       "\002:\0010\022\014\n\001p\030\002 \001(\002:\0010\022\014\n\001y\030\003 \001(\002:\0010\022\r\n\002cx\030\004"
       " \001(\002:\0010\022\r\n\002cy\030\005 \001(\002:\0010\022\r\n\002cz\030\006 \001(\002:\0010\"K\n"
       "\016lidarExtrinsic\022\020\n\010lidar_id\030\001 \002(\005\022\'\n\005cal"
-      "ib\030\002 \001(\0132\030.velodyne.CalibParameter\"\361\003\n\006R"
+      "ib\030\002 \001(\0132\030.velodyne.CalibParameter\"\214\004\n\006R"
       "egion\022\014\n\004minx\030\001 \002(\002\022\014\n\004maxx\030\002 \002(\002\022\014\n\004min"
       "y\030\003 \002(\002\022\014\n\004maxy\030\004 \002(\002\022\014\n\004minz\030\005 \002(\002\022\014\n\004m"
       "axz\030\006 \002(\002\022\021\n\tregion_id\030\007 \002(\005\022,\n\nlidar_ex"
@@ -326,15 +328,16 @@ void AddDescriptorsImpl() {
       "rnplate_cx\030\t \002(\002\022\024\n\014turnplate_cy\030\n \002(\002\022\023"
       "\n\013border_minx\030\013 \002(\002\022\023\n\013border_maxx\030\014 \002(\002"
       "\022\023\n\013plc_offsetx\030\r \002(\002\022\023\n\013plc_offsety\030\016 \002"
-      "(\002\022\027\n\017plc_border_miny\030\017 \002(\002\022\025\n\rcar_min_w"
-      "idth\030\020 \002(\002\022\025\n\rcar_max_width\030\021 \002(\002\022\031\n\021car"
-      "_min_wheelbase\030\022 \002(\002\022\031\n\021car_max_wheelbas"
-      "e\030\023 \002(\002\022,\n$turnplate_angle_limit_anti_cl"
-      "ockwise\030\024 \002(\002\022\'\n\037turnplate_angle_limit_c"
-      "lockwise\030\025 \002(\002"
+      "(\002\022\031\n\021plc_offset_degree\030\017 \002(\002\022\027\n\017plc_bor"
+      "der_miny\030\020 \002(\002\022\025\n\rcar_min_width\030\021 \002(\002\022\025\n"
+      "\rcar_max_width\030\022 \002(\002\022\031\n\021car_min_wheelbas"
+      "e\030\023 \002(\002\022\031\n\021car_max_wheelbase\030\024 \002(\002\022,\n$tu"
+      "rnplate_angle_limit_anti_clockwise\030\025 \002(\002"
+      "\022\'\n\037turnplate_angle_limit_clockwise\030\026 \002("
+      "\002"
   };
   ::google::protobuf::DescriptorPool::InternalAddGeneratedFile(
-      descriptor, 1254);
+      descriptor, 1281);
   ::google::protobuf::MessageFactory::InternalRegisterGeneratedFile(
     "velodyne_config.proto", &protobuf_RegisterTypes);
 }
@@ -2373,6 +2376,7 @@ const int Region::kBorderMinxFieldNumber;
 const int Region::kBorderMaxxFieldNumber;
 const int Region::kPlcOffsetxFieldNumber;
 const int Region::kPlcOffsetyFieldNumber;
+const int Region::kPlcOffsetDegreeFieldNumber;
 const int Region::kPlcBorderMinyFieldNumber;
 const int Region::kCarMinWidthFieldNumber;
 const int Region::kCarMaxWidthFieldNumber;
@@ -2444,13 +2448,13 @@ void Region::Clear() {
   }
   if (cached_has_bits & 65280u) {
     ::memset(&turnplate_cy_, 0, static_cast<size_t>(
-        reinterpret_cast<char*>(&car_max_width_) -
-        reinterpret_cast<char*>(&turnplate_cy_)) + sizeof(car_max_width_));
+        reinterpret_cast<char*>(&car_min_width_) -
+        reinterpret_cast<char*>(&turnplate_cy_)) + sizeof(car_min_width_));
   }
-  if (cached_has_bits & 983040u) {
-    ::memset(&car_min_wheelbase_, 0, static_cast<size_t>(
+  if (cached_has_bits & 2031616u) {
+    ::memset(&car_max_width_, 0, static_cast<size_t>(
         reinterpret_cast<char*>(&turnplate_angle_limit_clockwise_) -
-        reinterpret_cast<char*>(&car_min_wheelbase_)) + sizeof(turnplate_angle_limit_clockwise_));
+        reinterpret_cast<char*>(&car_max_width_)) + sizeof(turnplate_angle_limit_clockwise_));
   }
   _has_bits_.Clear();
   _internal_metadata_.Clear();
@@ -2660,94 +2664,108 @@ bool Region::MergePartialFromCodedStream(
         break;
       }
 
-      // required float plc_border_miny = 15;
+      // required float plc_offset_degree = 15;
       case 15: {
         if (static_cast< ::google::protobuf::uint8>(tag) ==
             static_cast< ::google::protobuf::uint8>(125u /* 125 & 0xFF */)) {
-          set_has_plc_border_miny();
+          set_has_plc_offset_degree();
           DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
                    float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &plc_border_miny_)));
+                 input, &plc_offset_degree_)));
         } else {
           goto handle_unusual;
         }
         break;
       }
 
-      // required float car_min_width = 16;
+      // required float plc_border_miny = 16;
       case 16: {
         if (static_cast< ::google::protobuf::uint8>(tag) ==
             static_cast< ::google::protobuf::uint8>(133u /* 133 & 0xFF */)) {
-          set_has_car_min_width();
+          set_has_plc_border_miny();
           DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
                    float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &car_min_width_)));
+                 input, &plc_border_miny_)));
         } else {
           goto handle_unusual;
         }
         break;
       }
 
-      // required float car_max_width = 17;
+      // required float car_min_width = 17;
       case 17: {
         if (static_cast< ::google::protobuf::uint8>(tag) ==
             static_cast< ::google::protobuf::uint8>(141u /* 141 & 0xFF */)) {
-          set_has_car_max_width();
+          set_has_car_min_width();
           DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
                    float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &car_max_width_)));
+                 input, &car_min_width_)));
         } else {
           goto handle_unusual;
         }
         break;
       }
 
-      // required float car_min_wheelbase = 18;
+      // required float car_max_width = 18;
       case 18: {
         if (static_cast< ::google::protobuf::uint8>(tag) ==
             static_cast< ::google::protobuf::uint8>(149u /* 149 & 0xFF */)) {
-          set_has_car_min_wheelbase();
+          set_has_car_max_width();
           DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
                    float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &car_min_wheelbase_)));
+                 input, &car_max_width_)));
         } else {
           goto handle_unusual;
         }
         break;
       }
 
-      // required float car_max_wheelbase = 19;
+      // required float car_min_wheelbase = 19;
       case 19: {
         if (static_cast< ::google::protobuf::uint8>(tag) ==
             static_cast< ::google::protobuf::uint8>(157u /* 157 & 0xFF */)) {
-          set_has_car_max_wheelbase();
+          set_has_car_min_wheelbase();
           DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
                    float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &car_max_wheelbase_)));
+                 input, &car_min_wheelbase_)));
         } else {
           goto handle_unusual;
         }
         break;
       }
 
-      // required float turnplate_angle_limit_anti_clockwise = 20;
+      // required float car_max_wheelbase = 20;
       case 20: {
         if (static_cast< ::google::protobuf::uint8>(tag) ==
             static_cast< ::google::protobuf::uint8>(165u /* 165 & 0xFF */)) {
-          set_has_turnplate_angle_limit_anti_clockwise();
+          set_has_car_max_wheelbase();
           DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
                    float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
-                 input, &turnplate_angle_limit_anti_clockwise_)));
+                 input, &car_max_wheelbase_)));
         } else {
           goto handle_unusual;
         }
         break;
       }
 
-      // required float turnplate_angle_limit_clockwise = 21;
+      // required float turnplate_angle_limit_anti_clockwise = 21;
       case 21: {
         if (static_cast< ::google::protobuf::uint8>(tag) ==
             static_cast< ::google::protobuf::uint8>(173u /* 173 & 0xFF */)) {
+          set_has_turnplate_angle_limit_anti_clockwise();
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
+                 input, &turnplate_angle_limit_anti_clockwise_)));
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
+      // required float turnplate_angle_limit_clockwise = 22;
+      case 22: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(181u /* 181 & 0xFF */)) {
           set_has_turnplate_angle_limit_clockwise();
           DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
                    float, ::google::protobuf::internal::WireFormatLite::TYPE_FLOAT>(
@@ -2859,39 +2877,44 @@ void Region::SerializeWithCachedSizes(
     ::google::protobuf::internal::WireFormatLite::WriteFloat(14, this->plc_offsety(), output);
   }
 
-  // required float plc_border_miny = 15;
+  // required float plc_offset_degree = 15;
   if (cached_has_bits & 0x00002000u) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(15, this->plc_border_miny(), output);
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(15, this->plc_offset_degree(), output);
   }
 
-  // required float car_min_width = 16;
+  // required float plc_border_miny = 16;
   if (cached_has_bits & 0x00004000u) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(16, this->car_min_width(), output);
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(16, this->plc_border_miny(), output);
   }
 
-  // required float car_max_width = 17;
+  // required float car_min_width = 17;
   if (cached_has_bits & 0x00008000u) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(17, this->car_max_width(), output);
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(17, this->car_min_width(), output);
   }
 
-  // required float car_min_wheelbase = 18;
+  // required float car_max_width = 18;
   if (cached_has_bits & 0x00010000u) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(18, this->car_min_wheelbase(), output);
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(18, this->car_max_width(), output);
   }
 
-  // required float car_max_wheelbase = 19;
+  // required float car_min_wheelbase = 19;
   if (cached_has_bits & 0x00020000u) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(19, this->car_max_wheelbase(), output);
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(19, this->car_min_wheelbase(), output);
   }
 
-  // required float turnplate_angle_limit_anti_clockwise = 20;
+  // required float car_max_wheelbase = 20;
   if (cached_has_bits & 0x00040000u) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(20, this->turnplate_angle_limit_anti_clockwise(), output);
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(20, this->car_max_wheelbase(), output);
   }
 
-  // required float turnplate_angle_limit_clockwise = 21;
+  // required float turnplate_angle_limit_anti_clockwise = 21;
   if (cached_has_bits & 0x00080000u) {
-    ::google::protobuf::internal::WireFormatLite::WriteFloat(21, this->turnplate_angle_limit_clockwise(), output);
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(21, this->turnplate_angle_limit_anti_clockwise(), output);
+  }
+
+  // required float turnplate_angle_limit_clockwise = 22;
+  if (cached_has_bits & 0x00100000u) {
+    ::google::protobuf::internal::WireFormatLite::WriteFloat(22, this->turnplate_angle_limit_clockwise(), output);
   }
 
   if (_internal_metadata_.have_unknown_fields()) {
@@ -2982,39 +3005,44 @@ void Region::SerializeWithCachedSizes(
     target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(14, this->plc_offsety(), target);
   }
 
-  // required float plc_border_miny = 15;
+  // required float plc_offset_degree = 15;
   if (cached_has_bits & 0x00002000u) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(15, this->plc_border_miny(), target);
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(15, this->plc_offset_degree(), target);
   }
 
-  // required float car_min_width = 16;
+  // required float plc_border_miny = 16;
   if (cached_has_bits & 0x00004000u) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(16, this->car_min_width(), target);
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(16, this->plc_border_miny(), target);
   }
 
-  // required float car_max_width = 17;
+  // required float car_min_width = 17;
   if (cached_has_bits & 0x00008000u) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(17, this->car_max_width(), target);
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(17, this->car_min_width(), target);
   }
 
-  // required float car_min_wheelbase = 18;
+  // required float car_max_width = 18;
   if (cached_has_bits & 0x00010000u) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(18, this->car_min_wheelbase(), target);
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(18, this->car_max_width(), target);
   }
 
-  // required float car_max_wheelbase = 19;
+  // required float car_min_wheelbase = 19;
   if (cached_has_bits & 0x00020000u) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(19, this->car_max_wheelbase(), target);
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(19, this->car_min_wheelbase(), target);
   }
 
-  // required float turnplate_angle_limit_anti_clockwise = 20;
+  // required float car_max_wheelbase = 20;
   if (cached_has_bits & 0x00040000u) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(20, this->turnplate_angle_limit_anti_clockwise(), target);
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(20, this->car_max_wheelbase(), target);
   }
 
-  // required float turnplate_angle_limit_clockwise = 21;
+  // required float turnplate_angle_limit_anti_clockwise = 21;
   if (cached_has_bits & 0x00080000u) {
-    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(21, this->turnplate_angle_limit_clockwise(), target);
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(21, this->turnplate_angle_limit_anti_clockwise(), target);
+  }
+
+  // required float turnplate_angle_limit_clockwise = 22;
+  if (cached_has_bits & 0x00100000u) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteFloatToArray(22, this->turnplate_angle_limit_clockwise(), target);
   }
 
   if (_internal_metadata_.have_unknown_fields()) {
@@ -3096,38 +3124,43 @@ size_t Region::RequiredFieldsByteSizeFallback() const {
     total_size += 1 + 4;
   }
 
-  if (has_plc_border_miny()) {
-    // required float plc_border_miny = 15;
+  if (has_plc_offset_degree()) {
+    // required float plc_offset_degree = 15;
     total_size += 1 + 4;
   }
 
+  if (has_plc_border_miny()) {
+    // required float plc_border_miny = 16;
+    total_size += 2 + 4;
+  }
+
   if (has_car_min_width()) {
-    // required float car_min_width = 16;
+    // required float car_min_width = 17;
     total_size += 2 + 4;
   }
 
   if (has_car_max_width()) {
-    // required float car_max_width = 17;
+    // required float car_max_width = 18;
     total_size += 2 + 4;
   }
 
   if (has_car_min_wheelbase()) {
-    // required float car_min_wheelbase = 18;
+    // required float car_min_wheelbase = 19;
     total_size += 2 + 4;
   }
 
   if (has_car_max_wheelbase()) {
-    // required float car_max_wheelbase = 19;
+    // required float car_max_wheelbase = 20;
     total_size += 2 + 4;
   }
 
   if (has_turnplate_angle_limit_anti_clockwise()) {
-    // required float turnplate_angle_limit_anti_clockwise = 20;
+    // required float turnplate_angle_limit_anti_clockwise = 21;
     total_size += 2 + 4;
   }
 
   if (has_turnplate_angle_limit_clockwise()) {
-    // required float turnplate_angle_limit_clockwise = 21;
+    // required float turnplate_angle_limit_clockwise = 22;
     total_size += 2 + 4;
   }
 
@@ -3142,7 +3175,7 @@ size_t Region::ByteSizeLong() const {
       ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
         _internal_metadata_.unknown_fields());
   }
-  if (((_has_bits_[0] & 0x000fffff) ^ 0x000fffff) == 0) {  // All required fields are present.
+  if (((_has_bits_[0] & 0x001fffff) ^ 0x001fffff) == 0) {  // All required fields are present.
     // required float minx = 1;
     total_size += 1 + 4;
 
@@ -3184,25 +3217,28 @@ size_t Region::ByteSizeLong() const {
     // required float plc_offsety = 14;
     total_size += 1 + 4;
 
-    // required float plc_border_miny = 15;
+    // required float plc_offset_degree = 15;
     total_size += 1 + 4;
 
-    // required float car_min_width = 16;
+    // required float plc_border_miny = 16;
+    total_size += 2 + 4;
+
+    // required float car_min_width = 17;
     total_size += 2 + 4;
 
-    // required float car_max_width = 17;
+    // required float car_max_width = 18;
     total_size += 2 + 4;
 
-    // required float car_min_wheelbase = 18;
+    // required float car_min_wheelbase = 19;
     total_size += 2 + 4;
 
-    // required float car_max_wheelbase = 19;
+    // required float car_max_wheelbase = 20;
     total_size += 2 + 4;
 
-    // required float turnplate_angle_limit_anti_clockwise = 20;
+    // required float turnplate_angle_limit_anti_clockwise = 21;
     total_size += 2 + 4;
 
-    // required float turnplate_angle_limit_clockwise = 21;
+    // required float turnplate_angle_limit_clockwise = 22;
     total_size += 2 + 4;
 
   } else {
@@ -3292,27 +3328,30 @@ void Region::MergeFrom(const Region& from) {
       plc_offsety_ = from.plc_offsety_;
     }
     if (cached_has_bits & 0x00002000u) {
-      plc_border_miny_ = from.plc_border_miny_;
+      plc_offset_degree_ = from.plc_offset_degree_;
     }
     if (cached_has_bits & 0x00004000u) {
-      car_min_width_ = from.car_min_width_;
+      plc_border_miny_ = from.plc_border_miny_;
     }
     if (cached_has_bits & 0x00008000u) {
-      car_max_width_ = from.car_max_width_;
+      car_min_width_ = from.car_min_width_;
     }
     _has_bits_[0] |= cached_has_bits;
   }
-  if (cached_has_bits & 983040u) {
+  if (cached_has_bits & 2031616u) {
     if (cached_has_bits & 0x00010000u) {
-      car_min_wheelbase_ = from.car_min_wheelbase_;
+      car_max_width_ = from.car_max_width_;
     }
     if (cached_has_bits & 0x00020000u) {
-      car_max_wheelbase_ = from.car_max_wheelbase_;
+      car_min_wheelbase_ = from.car_min_wheelbase_;
     }
     if (cached_has_bits & 0x00040000u) {
-      turnplate_angle_limit_anti_clockwise_ = from.turnplate_angle_limit_anti_clockwise_;
+      car_max_wheelbase_ = from.car_max_wheelbase_;
     }
     if (cached_has_bits & 0x00080000u) {
+      turnplate_angle_limit_anti_clockwise_ = from.turnplate_angle_limit_anti_clockwise_;
+    }
+    if (cached_has_bits & 0x00100000u) {
       turnplate_angle_limit_clockwise_ = from.turnplate_angle_limit_clockwise_;
     }
     _has_bits_[0] |= cached_has_bits;
@@ -3334,7 +3373,7 @@ void Region::CopyFrom(const Region& from) {
 }
 
 bool Region::IsInitialized() const {
-  if ((_has_bits_[0] & 0x000fffff) != 0x000fffff) return false;
+  if ((_has_bits_[0] & 0x001fffff) != 0x001fffff) return false;
   if (!::google::protobuf::internal::AllAreInitialized(this->lidar_exts())) return false;
   return true;
 }
@@ -3359,6 +3398,7 @@ void Region::InternalSwap(Region* other) {
   swap(border_maxx_, other->border_maxx_);
   swap(plc_offsetx_, other->plc_offsetx_);
   swap(plc_offsety_, other->plc_offsety_);
+  swap(plc_offset_degree_, other->plc_offset_degree_);
   swap(plc_border_miny_, other->plc_border_miny_);
   swap(car_min_width_, other->car_min_width_);
   swap(car_max_width_, other->car_max_width_);

+ 76 - 42
velodyne_lidar/velodyne_config.pb.h

@@ -1026,52 +1026,59 @@ class Region : public ::google::protobuf::Message /* @@protoc_insertion_point(cl
   float plc_offsety() const;
   void set_plc_offsety(float value);
 
-  // required float plc_border_miny = 15;
+  // required float plc_offset_degree = 15;
+  bool has_plc_offset_degree() const;
+  void clear_plc_offset_degree();
+  static const int kPlcOffsetDegreeFieldNumber = 15;
+  float plc_offset_degree() const;
+  void set_plc_offset_degree(float value);
+
+  // required float plc_border_miny = 16;
   bool has_plc_border_miny() const;
   void clear_plc_border_miny();
-  static const int kPlcBorderMinyFieldNumber = 15;
+  static const int kPlcBorderMinyFieldNumber = 16;
   float plc_border_miny() const;
   void set_plc_border_miny(float value);
 
-  // required float car_min_width = 16;
+  // required float car_min_width = 17;
   bool has_car_min_width() const;
   void clear_car_min_width();
-  static const int kCarMinWidthFieldNumber = 16;
+  static const int kCarMinWidthFieldNumber = 17;
   float car_min_width() const;
   void set_car_min_width(float value);
 
-  // required float car_max_width = 17;
+  // required float car_max_width = 18;
   bool has_car_max_width() const;
   void clear_car_max_width();
-  static const int kCarMaxWidthFieldNumber = 17;
+  static const int kCarMaxWidthFieldNumber = 18;
   float car_max_width() const;
   void set_car_max_width(float value);
 
-  // required float car_min_wheelbase = 18;
+  // required float car_min_wheelbase = 19;
   bool has_car_min_wheelbase() const;
   void clear_car_min_wheelbase();
-  static const int kCarMinWheelbaseFieldNumber = 18;
+  static const int kCarMinWheelbaseFieldNumber = 19;
   float car_min_wheelbase() const;
   void set_car_min_wheelbase(float value);
 
-  // required float car_max_wheelbase = 19;
+  // required float car_max_wheelbase = 20;
   bool has_car_max_wheelbase() const;
   void clear_car_max_wheelbase();
-  static const int kCarMaxWheelbaseFieldNumber = 19;
+  static const int kCarMaxWheelbaseFieldNumber = 20;
   float car_max_wheelbase() const;
   void set_car_max_wheelbase(float value);
 
-  // required float turnplate_angle_limit_anti_clockwise = 20;
+  // required float turnplate_angle_limit_anti_clockwise = 21;
   bool has_turnplate_angle_limit_anti_clockwise() const;
   void clear_turnplate_angle_limit_anti_clockwise();
-  static const int kTurnplateAngleLimitAntiClockwiseFieldNumber = 20;
+  static const int kTurnplateAngleLimitAntiClockwiseFieldNumber = 21;
   float turnplate_angle_limit_anti_clockwise() const;
   void set_turnplate_angle_limit_anti_clockwise(float value);
 
-  // required float turnplate_angle_limit_clockwise = 21;
+  // required float turnplate_angle_limit_clockwise = 22;
   bool has_turnplate_angle_limit_clockwise() const;
   void clear_turnplate_angle_limit_clockwise();
-  static const int kTurnplateAngleLimitClockwiseFieldNumber = 21;
+  static const int kTurnplateAngleLimitClockwiseFieldNumber = 22;
   float turnplate_angle_limit_clockwise() const;
   void set_turnplate_angle_limit_clockwise(float value);
 
@@ -1103,6 +1110,8 @@ class Region : public ::google::protobuf::Message /* @@protoc_insertion_point(cl
   void clear_has_plc_offsetx();
   void set_has_plc_offsety();
   void clear_has_plc_offsety();
+  void set_has_plc_offset_degree();
+  void clear_has_plc_offset_degree();
   void set_has_plc_border_miny();
   void clear_has_plc_border_miny();
   void set_has_car_min_width();
@@ -1138,6 +1147,7 @@ class Region : public ::google::protobuf::Message /* @@protoc_insertion_point(cl
   float border_maxx_;
   float plc_offsetx_;
   float plc_offsety_;
+  float plc_offset_degree_;
   float plc_border_miny_;
   float car_min_width_;
   float car_max_width_;
@@ -2514,16 +2524,40 @@ inline void Region::set_plc_offsety(float value) {
   // @@protoc_insertion_point(field_set:velodyne.Region.plc_offsety)
 }
 
-// required float plc_border_miny = 15;
-inline bool Region::has_plc_border_miny() const {
+// required float plc_offset_degree = 15;
+inline bool Region::has_plc_offset_degree() const {
   return (_has_bits_[0] & 0x00002000u) != 0;
 }
-inline void Region::set_has_plc_border_miny() {
+inline void Region::set_has_plc_offset_degree() {
   _has_bits_[0] |= 0x00002000u;
 }
-inline void Region::clear_has_plc_border_miny() {
+inline void Region::clear_has_plc_offset_degree() {
   _has_bits_[0] &= ~0x00002000u;
 }
+inline void Region::clear_plc_offset_degree() {
+  plc_offset_degree_ = 0;
+  clear_has_plc_offset_degree();
+}
+inline float Region::plc_offset_degree() const {
+  // @@protoc_insertion_point(field_get:velodyne.Region.plc_offset_degree)
+  return plc_offset_degree_;
+}
+inline void Region::set_plc_offset_degree(float value) {
+  set_has_plc_offset_degree();
+  plc_offset_degree_ = value;
+  // @@protoc_insertion_point(field_set:velodyne.Region.plc_offset_degree)
+}
+
+// required float plc_border_miny = 16;
+inline bool Region::has_plc_border_miny() const {
+  return (_has_bits_[0] & 0x00004000u) != 0;
+}
+inline void Region::set_has_plc_border_miny() {
+  _has_bits_[0] |= 0x00004000u;
+}
+inline void Region::clear_has_plc_border_miny() {
+  _has_bits_[0] &= ~0x00004000u;
+}
 inline void Region::clear_plc_border_miny() {
   plc_border_miny_ = 0;
   clear_has_plc_border_miny();
@@ -2538,15 +2572,15 @@ inline void Region::set_plc_border_miny(float value) {
   // @@protoc_insertion_point(field_set:velodyne.Region.plc_border_miny)
 }
 
-// required float car_min_width = 16;
+// required float car_min_width = 17;
 inline bool Region::has_car_min_width() const {
-  return (_has_bits_[0] & 0x00004000u) != 0;
+  return (_has_bits_[0] & 0x00008000u) != 0;
 }
 inline void Region::set_has_car_min_width() {
-  _has_bits_[0] |= 0x00004000u;
+  _has_bits_[0] |= 0x00008000u;
 }
 inline void Region::clear_has_car_min_width() {
-  _has_bits_[0] &= ~0x00004000u;
+  _has_bits_[0] &= ~0x00008000u;
 }
 inline void Region::clear_car_min_width() {
   car_min_width_ = 0;
@@ -2562,15 +2596,15 @@ inline void Region::set_car_min_width(float value) {
   // @@protoc_insertion_point(field_set:velodyne.Region.car_min_width)
 }
 
-// required float car_max_width = 17;
+// required float car_max_width = 18;
 inline bool Region::has_car_max_width() const {
-  return (_has_bits_[0] & 0x00008000u) != 0;
+  return (_has_bits_[0] & 0x00010000u) != 0;
 }
 inline void Region::set_has_car_max_width() {
-  _has_bits_[0] |= 0x00008000u;
+  _has_bits_[0] |= 0x00010000u;
 }
 inline void Region::clear_has_car_max_width() {
-  _has_bits_[0] &= ~0x00008000u;
+  _has_bits_[0] &= ~0x00010000u;
 }
 inline void Region::clear_car_max_width() {
   car_max_width_ = 0;
@@ -2586,15 +2620,15 @@ inline void Region::set_car_max_width(float value) {
   // @@protoc_insertion_point(field_set:velodyne.Region.car_max_width)
 }
 
-// required float car_min_wheelbase = 18;
+// required float car_min_wheelbase = 19;
 inline bool Region::has_car_min_wheelbase() const {
-  return (_has_bits_[0] & 0x00010000u) != 0;
+  return (_has_bits_[0] & 0x00020000u) != 0;
 }
 inline void Region::set_has_car_min_wheelbase() {
-  _has_bits_[0] |= 0x00010000u;
+  _has_bits_[0] |= 0x00020000u;
 }
 inline void Region::clear_has_car_min_wheelbase() {
-  _has_bits_[0] &= ~0x00010000u;
+  _has_bits_[0] &= ~0x00020000u;
 }
 inline void Region::clear_car_min_wheelbase() {
   car_min_wheelbase_ = 0;
@@ -2610,15 +2644,15 @@ inline void Region::set_car_min_wheelbase(float value) {
   // @@protoc_insertion_point(field_set:velodyne.Region.car_min_wheelbase)
 }
 
-// required float car_max_wheelbase = 19;
+// required float car_max_wheelbase = 20;
 inline bool Region::has_car_max_wheelbase() const {
-  return (_has_bits_[0] & 0x00020000u) != 0;
+  return (_has_bits_[0] & 0x00040000u) != 0;
 }
 inline void Region::set_has_car_max_wheelbase() {
-  _has_bits_[0] |= 0x00020000u;
+  _has_bits_[0] |= 0x00040000u;
 }
 inline void Region::clear_has_car_max_wheelbase() {
-  _has_bits_[0] &= ~0x00020000u;
+  _has_bits_[0] &= ~0x00040000u;
 }
 inline void Region::clear_car_max_wheelbase() {
   car_max_wheelbase_ = 0;
@@ -2634,15 +2668,15 @@ inline void Region::set_car_max_wheelbase(float value) {
   // @@protoc_insertion_point(field_set:velodyne.Region.car_max_wheelbase)
 }
 
-// required float turnplate_angle_limit_anti_clockwise = 20;
+// required float turnplate_angle_limit_anti_clockwise = 21;
 inline bool Region::has_turnplate_angle_limit_anti_clockwise() const {
-  return (_has_bits_[0] & 0x00040000u) != 0;
+  return (_has_bits_[0] & 0x00080000u) != 0;
 }
 inline void Region::set_has_turnplate_angle_limit_anti_clockwise() {
-  _has_bits_[0] |= 0x00040000u;
+  _has_bits_[0] |= 0x00080000u;
 }
 inline void Region::clear_has_turnplate_angle_limit_anti_clockwise() {
-  _has_bits_[0] &= ~0x00040000u;
+  _has_bits_[0] &= ~0x00080000u;
 }
 inline void Region::clear_turnplate_angle_limit_anti_clockwise() {
   turnplate_angle_limit_anti_clockwise_ = 0;
@@ -2658,15 +2692,15 @@ inline void Region::set_turnplate_angle_limit_anti_clockwise(float value) {
   // @@protoc_insertion_point(field_set:velodyne.Region.turnplate_angle_limit_anti_clockwise)
 }
 
-// required float turnplate_angle_limit_clockwise = 21;
+// required float turnplate_angle_limit_clockwise = 22;
 inline bool Region::has_turnplate_angle_limit_clockwise() const {
-  return (_has_bits_[0] & 0x00080000u) != 0;
+  return (_has_bits_[0] & 0x00100000u) != 0;
 }
 inline void Region::set_has_turnplate_angle_limit_clockwise() {
-  _has_bits_[0] |= 0x00080000u;
+  _has_bits_[0] |= 0x00100000u;
 }
 inline void Region::clear_has_turnplate_angle_limit_clockwise() {
-  _has_bits_[0] &= ~0x00080000u;
+  _has_bits_[0] &= ~0x00100000u;
 }
 inline void Region::clear_turnplate_angle_limit_clockwise() {
   turnplate_angle_limit_clockwise_ = 0;

+ 8 - 7
velodyne_lidar/velodyne_config.proto

@@ -59,11 +59,12 @@ message Region
     required float border_maxx=12; // 最大边界x,右超界提示
     required float plc_offsetx=13; // plc偏移x
     required float plc_offsety=14; // plc偏移y
-    required float plc_border_miny=15;// plc后夹持y方向极限值
-    required float car_min_width=16; // 最小车宽
-    required float car_max_width=17; // 最大车宽
-    required float car_min_wheelbase=18; // 最小轴距
-    required float car_max_wheelbase=19; // 最大轴距
-    required float turnplate_angle_limit_anti_clockwise=20; // 转盘逆时针角度极限
-    required float turnplate_angle_limit_clockwise=21; // 转盘顺时针角度极限
+    required float plc_offset_degree=15; // plc偏移角度
+    required float plc_border_miny=16;// plc后夹持y方向极限值
+    required float car_min_width=17; // 最小车宽
+    required float car_max_width=18; // 最大车宽
+    required float car_min_wheelbase=19; // 最小轴距
+    required float car_max_wheelbase=20; // 最大轴距
+    required float turnplate_angle_limit_anti_clockwise=21; // 转盘逆时针角度极限
+    required float turnplate_angle_limit_clockwise=22; // 转盘顺时针角度极限
 }