add cloud publish

message/measure_message.pb.cc

@@ -198,6 +198,7 @@ void InitDefaultsGround_status_msgImpl() {
   protobuf_message_5fbase_2eproto::InitDefaultsBase_info();
   protobuf_message_5fbase_2eproto::InitDefaultsLocate_information();
   protobuf_message_5fbase_2eproto::InitDefaultsError_manager();
+  protobuf_measure_5fmessage_2eproto::InitDefaultsCloud_coordinate();
   {
     void* ptr = &::message::_Ground_status_msg_default_instance_;
     new (ptr) ::message::Ground_status_msg();
@@ -386,6 +387,7 @@ const ::google::protobuf::uint32 TableStruct::offsets[] GOOGLE_PROTOBUF_ATTRIBUT
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::message::Ground_status_msg, locate_information_realtime_),
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::message::Ground_status_msg, ground_status_),
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::message::Ground_status_msg, error_manager_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::message::Ground_status_msg, cloud_),
   0,
   3,
   4,
@@ -394,6 +396,7 @@ const ::google::protobuf::uint32 TableStruct::offsets[] GOOGLE_PROTOBUF_ATTRIBUT
   1,
   6,
   2,
+  ~0u,
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::message::Cloud_coordinate, _has_bits_),
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::message::Cloud_coordinate, _internal_metadata_),
   ~0u,  // no _extensions_
@@ -451,11 +454,11 @@ static const ::google::protobuf::internal::MigrationSchema schemas[] GOOGLE_PROT
   { 28, 38, sizeof(::message::Measure_response_msg)},
   { 43, 51, sizeof(::message::Ground_detect_request_msg)},
   { 54, 64, sizeof(::message::Ground_detect_response_msg)},
-  { 69, 82, sizeof(::message::Ground_status_msg)},
-  { 90, 98, sizeof(::message::Cloud_coordinate)},
-  { 101, 107, sizeof(::message::Cloud_type)},
-  { 108, 118, sizeof(::message::Locate_sift_request_msg)},
-  { 123, 134, sizeof(::message::Locate_sift_response_msg)},
+  { 69, 83, sizeof(::message::Ground_status_msg)},
+  { 92, 100, sizeof(::message::Cloud_coordinate)},
+  { 103, 109, sizeof(::message::Cloud_type)},
+  { 110, 120, sizeof(::message::Locate_sift_request_msg)},
+  { 125, 136, sizeof(::message::Locate_sift_response_msg)},
 };
 
 static ::google::protobuf::Message const * const file_default_instances[] = {
@@ -517,7 +520,7 @@ void AddDescriptorsImpl() {
       ".Base_info\022\023\n\013command_key\030\002 \002(\t\022\023\n\013termi"
       "nal_id\030\003 \002(\005\0227\n\022locate_information\030\004 \001(\013"
       "2\033.message.Locate_information\022-\n\rerror_m"
-      "anager\030\005 \002(\0132\026.message.Error_manager\"\257\003\n"
+      "anager\030\005 \002(\0132\026.message.Error_manager\"\331\003\n"
       "\021Ground_status_msg\022%\n\tbase_info\030\001 \002(\0132\022."
       "message.Base_info\022\023\n\013terminal_id\030\002 \002(\005\022;"
       "\n\024wanji_manager_status\030\003 \002(\0162\035.message.W"
@@ -528,50 +531,51 @@ void AddDescriptorsImpl() {
       "e_information_realtime\030\006 \002(\0132\033.message.L"
       "ocate_information\022,\n\rground_status\030\007 \002(\016"
       "2\025.message.Ground_statu\022-\n\rerror_manager"
-      "\030\010 \002(\0132\026.message.Error_manager\"3\n\020Cloud_"
-      "coordinate\022\t\n\001x\030\001 \002(\002\022\t\n\001y\030\002 \002(\002\022\t\n\001z\030\003 "
-      "\002(\002\"\032\n\nCloud_type\022\014\n\004type\030\001 \002(\005\"\262\001\n\027Loca"
-      "te_sift_request_msg\022%\n\tbase_info\030\001 \002(\0132\022"
-      ".message.Base_info\022\023\n\013command_key\030\002 \002(\t\022"
-      "\023\n\013terminal_id\030\003 \002(\005\022\020\n\010lidar_id\030\004 \002(\005\0224"
-      "\n\021cloud_coordinates\030\005 \003(\0132\031.message.Clou"
-      "d_coordinate\"\325\001\n\030Locate_sift_response_ms"
-      "g\022%\n\tbase_info\030\001 \002(\0132\022.message.Base_info"
-      "\022\023\n\013command_key\030\002 \002(\t\022\023\n\013terminal_id\030\003 \002"
-      "(\005\022\020\n\010lidar_id\030\004 \002(\005\022\'\n\ncloud_type\030\005 \003(\013"
-      "2\023.message.Cloud_type\022-\n\rerror_manager\030\006"
-      " \002(\0132\026.message.Error_manager*\237\001\n\024Laser_m"
-      "anager_status\022\030\n\024LASER_MANAGER_UNKNOW\020\000\022"
-      "\027\n\023LASER_MANAGER_READY\020\001\022\035\n\031LASER_MANAGE"
-      "R_ISSUED_TASK\020\002\022\034\n\030LASER_MANAGER_WAIT_RE"
-      "PLY\020\003\022\027\n\023LASER_MANAGER_FAULT\020\004*U\n\013Laser_"
-      "statu\022\024\n\020LASER_DISCONNECT\020\000\022\017\n\013LASER_REA"
-      "DY\020\001\022\016\n\nLASER_BUSY\020\002\022\017\n\013LASER_FAULT\020\003*\261\001"
-      "\n\025Locate_manager_status\022\031\n\025LOCATE_MANAGE"
-      "R_UNKNOW\020\000\022\030\n\024LOCATE_MANAGER_READY\020\001\022\027\n\023"
-      "LOCATE_MANAGER_SIFT\020\002\022\026\n\022LOCATE_MANAGER_"
-      "CAR\020\003\022\030\n\024LOCATE_MANAGER_WHEEL\020\004\022\030\n\024LOCAT"
-      "E_MANAGER_FAULT\020\005*\367\001\n\024Wanji_manager_stat"
-      "us\022\031\n\025WANJI_MANAGER_UNKNOWN\020\000\022\027\n\023WANJI_M"
-      "ANAGER_READY\020\001\022\026\n\022WANJI_MANAGER_BUSY\020\002\022\035"
-      "\n\031WANJI_MANAGER_ISSUED_SCAN\020\003\022\033\n\027WANJI_M"
-      "ANAGER_WAIT_SCAN\020\004\022\037\n\033WANJI_MANAGER_ISSU"
-      "ED_DETECT\020\005\022\035\n\031WANJI_MANAGER_WAIT_DETECT"
-      "\020\006\022\027\n\023WANJI_MANAGER_FAULT\020\n*\267\001\n\031Wanji_li"
-      "dar_device_status\022\036\n\032WANJI_LIDAR_DEVICE_"
-      "UNKNOWN\020\000\022\034\n\030WANJI_LIDAR_DEVICE_READY\020\001\022"
-      "!\n\035WANJI_LIDAR_DEVICE_DISCONNECT\020\002\022\033\n\027WA"
-      "NJI_LIDAR_DEVICE_BUSY\020\003\022\034\n\030WANJI_LIDAR_D"
-      "EVICE_FAULT\020\n*{\n\024Region_worker_status\022\031\n"
-      "\025REGION_WORKER_UNKNOWN\020\000\022\027\n\023REGION_WORKE"
-      "R_READY\020\001\022\026\n\022REGION_WORKER_BUSY\020\002\022\027\n\023REG"
-      "ION_WORKER_FAULT\020\n*\201\001\n\014Ground_statu\022\013\n\007N"
-      "othing\020\000\022\t\n\005Noise\020\001\022\017\n\013Car_correct\020\002\022\020\n\014"
-      "Car_left_out\020\003\022\021\n\rCar_right_out\020\004\022\017\n\013Car"
-      "_top_out\020\005\022\022\n\016Car_bottom_out\020\006"
+      "\030\010 \002(\0132\026.message.Error_manager\022(\n\005cloud\030"
+      "\t \003(\0132\031.message.Cloud_coordinate\"3\n\020Clou"
+      "d_coordinate\022\t\n\001x\030\001 \002(\002\022\t\n\001y\030\002 \002(\002\022\t\n\001z\030"
+      "\003 \002(\002\"\032\n\nCloud_type\022\014\n\004type\030\001 \002(\005\"\262\001\n\027Lo"
+      "cate_sift_request_msg\022%\n\tbase_info\030\001 \002(\013"
+      "2\022.message.Base_info\022\023\n\013command_key\030\002 \002("
+      "\t\022\023\n\013terminal_id\030\003 \002(\005\022\020\n\010lidar_id\030\004 \002(\005"
+      "\0224\n\021cloud_coordinates\030\005 \003(\0132\031.message.Cl"
+      "oud_coordinate\"\325\001\n\030Locate_sift_response_"
+      "msg\022%\n\tbase_info\030\001 \002(\0132\022.message.Base_in"
+      "fo\022\023\n\013command_key\030\002 \002(\t\022\023\n\013terminal_id\030\003"
+      " \002(\005\022\020\n\010lidar_id\030\004 \002(\005\022\'\n\ncloud_type\030\005 \003"
+      "(\0132\023.message.Cloud_type\022-\n\rerror_manager"
+      "\030\006 \002(\0132\026.message.Error_manager*\237\001\n\024Laser"
+      "_manager_status\022\030\n\024LASER_MANAGER_UNKNOW\020"
+      "\000\022\027\n\023LASER_MANAGER_READY\020\001\022\035\n\031LASER_MANA"
+      "GER_ISSUED_TASK\020\002\022\034\n\030LASER_MANAGER_WAIT_"
+      "REPLY\020\003\022\027\n\023LASER_MANAGER_FAULT\020\004*U\n\013Lase"
+      "r_statu\022\024\n\020LASER_DISCONNECT\020\000\022\017\n\013LASER_R"
+      "EADY\020\001\022\016\n\nLASER_BUSY\020\002\022\017\n\013LASER_FAULT\020\003*"
+      "\261\001\n\025Locate_manager_status\022\031\n\025LOCATE_MANA"
+      "GER_UNKNOW\020\000\022\030\n\024LOCATE_MANAGER_READY\020\001\022\027"
+      "\n\023LOCATE_MANAGER_SIFT\020\002\022\026\n\022LOCATE_MANAGE"
+      "R_CAR\020\003\022\030\n\024LOCATE_MANAGER_WHEEL\020\004\022\030\n\024LOC"
+      "ATE_MANAGER_FAULT\020\005*\367\001\n\024Wanji_manager_st"
+      "atus\022\031\n\025WANJI_MANAGER_UNKNOWN\020\000\022\027\n\023WANJI"
+      "_MANAGER_READY\020\001\022\026\n\022WANJI_MANAGER_BUSY\020\002"
+      "\022\035\n\031WANJI_MANAGER_ISSUED_SCAN\020\003\022\033\n\027WANJI"
+      "_MANAGER_WAIT_SCAN\020\004\022\037\n\033WANJI_MANAGER_IS"
+      "SUED_DETECT\020\005\022\035\n\031WANJI_MANAGER_WAIT_DETE"
+      "CT\020\006\022\027\n\023WANJI_MANAGER_FAULT\020\n*\267\001\n\031Wanji_"
+      "lidar_device_status\022\036\n\032WANJI_LIDAR_DEVIC"
+      "E_UNKNOWN\020\000\022\034\n\030WANJI_LIDAR_DEVICE_READY\020"
+      "\001\022!\n\035WANJI_LIDAR_DEVICE_DISCONNECT\020\002\022\033\n\027"
+      "WANJI_LIDAR_DEVICE_BUSY\020\003\022\034\n\030WANJI_LIDAR"
+      "_DEVICE_FAULT\020\n*{\n\024Region_worker_status\022"
+      "\031\n\025REGION_WORKER_UNKNOWN\020\000\022\027\n\023REGION_WOR"
+      "KER_READY\020\001\022\026\n\022REGION_WORKER_BUSY\020\002\022\027\n\023R"
+      "EGION_WORKER_FAULT\020\n*\201\001\n\014Ground_statu\022\013\n"
+      "\007Nothing\020\000\022\t\n\005Noise\020\001\022\017\n\013Car_correct\020\002\022\020"
+      "\n\014Car_left_out\020\003\022\021\n\rCar_right_out\020\004\022\017\n\013C"
+      "ar_top_out\020\005\022\022\n\016Car_bottom_out\020\006"
   };
   ::google::protobuf::DescriptorPool::InternalAddGeneratedFile(
-      descriptor, 3030);
+      descriptor, 3072);
   ::google::protobuf::MessageFactory::InternalRegisterGeneratedFile(
     "measure_message.proto", &protobuf_RegisterTypes);
   ::protobuf_message_5fbase_2eproto::AddDescriptors();
@@ -3092,6 +3096,7 @@ const int Ground_status_msg::kRegionWorkerStatusFieldNumber;
 const int Ground_status_msg::kLocateInformationRealtimeFieldNumber;
 const int Ground_status_msg::kGroundStatusFieldNumber;
 const int Ground_status_msg::kErrorManagerFieldNumber;
+const int Ground_status_msg::kCloudFieldNumber;
 #endif  // !defined(_MSC_VER) || _MSC_VER >= 1900
 
 Ground_status_msg::Ground_status_msg()
@@ -3107,7 +3112,8 @@ Ground_status_msg::Ground_status_msg(const Ground_status_msg& from)
       _internal_metadata_(NULL),
       _has_bits_(from._has_bits_),
       _cached_size_(0),
-      wanji_lidar_device_status_(from.wanji_lidar_device_status_) {
+      wanji_lidar_device_status_(from.wanji_lidar_device_status_),
+      cloud_(from.cloud_) {
   _internal_metadata_.MergeFrom(from._internal_metadata_);
   if (from.has_base_info()) {
     base_info_ = new ::message::Base_info(*from.base_info_);
@@ -3178,6 +3184,7 @@ void Ground_status_msg::Clear() {
   (void) cached_has_bits;
 
   wanji_lidar_device_status_.Clear();
+  cloud_.Clear();
   cached_has_bits = _has_bits_[0];
   if (cached_has_bits & 7u) {
     if (cached_has_bits & 0x00000001u) {
@@ -3351,6 +3358,17 @@ bool Ground_status_msg::MergePartialFromCodedStream(
         break;
       }
 
+      // repeated .message.Cloud_coordinate cloud = 9;
+      case 9: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(74u /* 74 & 0xFF */)) {
+          DO_(::google::protobuf::internal::WireFormatLite::ReadMessage(input, add_cloud()));
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
       default: {
       handle_unusual:
         if (tag == 0) {
@@ -3425,6 +3443,13 @@ void Ground_status_msg::SerializeWithCachedSizes(
       8, *this->error_manager_, output);
   }
 
+  // repeated .message.Cloud_coordinate cloud = 9;
+  for (unsigned int i = 0,
+      n = static_cast<unsigned int>(this->cloud_size()); i < n; i++) {
+    ::google::protobuf::internal::WireFormatLite::WriteMessageMaybeToArray(
+      9, this->cloud(static_cast<int>(i)), output);
+  }
+
   if (_internal_metadata_.have_unknown_fields()) {
     ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
         _internal_metadata_.unknown_fields(), output);
@@ -3488,6 +3513,14 @@ void Ground_status_msg::SerializeWithCachedSizes(
         8, *this->error_manager_, deterministic, target);
   }
 
+  // repeated .message.Cloud_coordinate cloud = 9;
+  for (unsigned int i = 0,
+      n = static_cast<unsigned int>(this->cloud_size()); i < n; i++) {
+    target = ::google::protobuf::internal::WireFormatLite::
+      InternalWriteMessageToArray(
+        9, this->cloud(static_cast<int>(i)), deterministic, target);
+  }
+
   if (_internal_metadata_.have_unknown_fields()) {
     target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
         _internal_metadata_.unknown_fields(), target);
@@ -3603,6 +3636,17 @@ size_t Ground_status_msg::ByteSizeLong() const {
     total_size += (1UL * count) + data_size;
   }
 
+  // repeated .message.Cloud_coordinate cloud = 9;
+  {
+    unsigned int count = static_cast<unsigned int>(this->cloud_size());
+    total_size += 1UL * count;
+    for (unsigned int i = 0; i < count; i++) {
+      total_size +=
+        ::google::protobuf::internal::WireFormatLite::MessageSize(
+          this->cloud(static_cast<int>(i)));
+    }
+  }
+
   int cached_size = ::google::protobuf::internal::ToCachedSize(total_size);
   GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
   _cached_size_ = cached_size;
@@ -3633,6 +3677,7 @@ void Ground_status_msg::MergeFrom(const Ground_status_msg& from) {
   (void) cached_has_bits;
 
   wanji_lidar_device_status_.MergeFrom(from.wanji_lidar_device_status_);
+  cloud_.MergeFrom(from.cloud_);
   cached_has_bits = from._has_bits_[0];
   if (cached_has_bits & 127u) {
     if (cached_has_bits & 0x00000001u) {
@@ -3676,6 +3721,7 @@ void Ground_status_msg::CopyFrom(const Ground_status_msg& from) {
 
 bool Ground_status_msg::IsInitialized() const {
   if ((_has_bits_[0] & 0x0000007f) != 0x0000007f) return false;
+  if (!::google::protobuf::internal::AllAreInitialized(this->cloud())) return false;
   if (has_base_info()) {
     if (!this->base_info_->IsInitialized()) return false;
   }
@@ -3692,6 +3738,7 @@ void Ground_status_msg::Swap(Ground_status_msg* other) {
 void Ground_status_msg::InternalSwap(Ground_status_msg* other) {
   using std::swap;
   wanji_lidar_device_status_.InternalSwap(&other->wanji_lidar_device_status_);
+  cloud_.InternalSwap(&other->cloud_);
   swap(base_info_, other->base_info_);
   swap(locate_information_realtime_, other->locate_information_realtime_);
   swap(error_manager_, other->error_manager_);

message/measure_message.pb.h

@@ -1158,6 +1158,18 @@ class Ground_status_msg : public ::google::protobuf::Message /* @@protoc_inserti
   const ::google::protobuf::RepeatedField<int>& wanji_lidar_device_status() const;
   ::google::protobuf::RepeatedField<int>* mutable_wanji_lidar_device_status();
 
+  // repeated .message.Cloud_coordinate cloud = 9;
+  int cloud_size() const;
+  void clear_cloud();
+  static const int kCloudFieldNumber = 9;
+  const ::message::Cloud_coordinate& cloud(int index) const;
+  ::message::Cloud_coordinate* mutable_cloud(int index);
+  ::message::Cloud_coordinate* add_cloud();
+  ::google::protobuf::RepeatedPtrField< ::message::Cloud_coordinate >*
+      mutable_cloud();
+  const ::google::protobuf::RepeatedPtrField< ::message::Cloud_coordinate >&
+      cloud() const;
+
   // required .message.Base_info base_info = 1;
   bool has_base_info() const;
   void clear_base_info();
@@ -1237,6 +1249,7 @@ class Ground_status_msg : public ::google::protobuf::Message /* @@protoc_inserti
   ::google::protobuf::internal::HasBits<1> _has_bits_;
   mutable int _cached_size_;
   ::google::protobuf::RepeatedField<int> wanji_lidar_device_status_;
+  ::google::protobuf::RepeatedPtrField< ::message::Cloud_coordinate > cloud_;
   ::message::Base_info* base_info_;
   ::message::Locate_information* locate_information_realtime_;
   ::message::Error_manager* error_manager_;
@@ -3100,6 +3113,36 @@ inline void Ground_status_msg::set_allocated_error_manager(::message::Error_mana
   // @@protoc_insertion_point(field_set_allocated:message.Ground_status_msg.error_manager)
 }
 
+// repeated .message.Cloud_coordinate cloud = 9;
+inline int Ground_status_msg::cloud_size() const {
+  return cloud_.size();
+}
+inline void Ground_status_msg::clear_cloud() {
+  cloud_.Clear();
+}
+inline const ::message::Cloud_coordinate& Ground_status_msg::cloud(int index) const {
+  // @@protoc_insertion_point(field_get:message.Ground_status_msg.cloud)
+  return cloud_.Get(index);
+}
+inline ::message::Cloud_coordinate* Ground_status_msg::mutable_cloud(int index) {
+  // @@protoc_insertion_point(field_mutable:message.Ground_status_msg.cloud)
+  return cloud_.Mutable(index);
+}
+inline ::message::Cloud_coordinate* Ground_status_msg::add_cloud() {
+  // @@protoc_insertion_point(field_add:message.Ground_status_msg.cloud)
+  return cloud_.Add();
+}
+inline ::google::protobuf::RepeatedPtrField< ::message::Cloud_coordinate >*
+Ground_status_msg::mutable_cloud() {
+  // @@protoc_insertion_point(field_mutable_list:message.Ground_status_msg.cloud)
+  return &cloud_;
+}
+inline const ::google::protobuf::RepeatedPtrField< ::message::Cloud_coordinate >&
+Ground_status_msg::cloud() const {
+  // @@protoc_insertion_point(field_list:message.Ground_status_msg.cloud)
+  return cloud_;
+}
+
 // -------------------------------------------------------------------
 
 // Cloud_coordinate

message/measure_message.proto

@@ -147,6 +147,7 @@ message Ground_status_msg
     required Ground_statu               ground_status=7; // 电子围栏状态
 
     required Error_manager              error_manager = 8;
+    repeated Cloud_coordinate           cloud=9;     //点云坐标
 }
 
 //点云坐标

setting/communication.prototxt

@@ -3,8 +3,8 @@
 communication_parameters
 {
 
-  bind_string:"tcp://192.168.1.39:30010"
-  connect_string_vector:"tcp://192.168.1.39:30000"
+  bind_string:"tcp://192.168.1.150:30010"
+  connect_string_vector:"tcp://192.168.1.150:30000"
  # connect_string_vector:"tcp://192.168.2.166:9002"
 
   # connect_string_vector:"tcp://192.168.2.125:9876"

setting/velodyne_manager.prototxt

@@ -1,7 +1,7 @@
 fence_data_path:"/home/youchen/extra_space/chutian/fence_wj"
 #fence_log_path:"/home/zx/yct/MainStructure/new_electronic_fence/log"
-left_model_path:"/home/youchen/extra_space/chutian/measure/puai_wj_2021/setting/left_model.txt"
-right_model_path:"/home/youchen/extra_space/chutian/measure/puai_wj_2021/setting/right_model.txt"
+left_model_path:"/home/youchen/extra_space/chutian/measure/chutian_velo_ws/src/chutian_velo/setting/left_model.txt"
+right_model_path:"/home/youchen/extra_space/chutian/measure/chutian_velo_ws/src/chutian_velo/setting/right_model.txt"
 distribution_mode:false
 
 # 1 lidar
@@ -14,12 +14,20 @@ velodyne_lidars
     lidar_id:0
     max_range:5.0
     min_range:0.1
-    min_angle:0
+    min_angle:180
     max_angle:360
     rpm:600
     calib
     {
-        y:1.57
+        # r:0.0310211
+        # p:-0.46
+        # y:0.514386
+        # cz:0.0897851
+
+        r:-0.623165
+        p:0.601821
+        y:87.0198
+        cz:0.0
     }
 }
 
@@ -34,41 +42,49 @@ velodyne_lidars
     max_range:5.0
     min_range:0.1
     min_angle:0
-    max_angle:360
+    max_angle:195
     rpm:600
     calib
     {
-        y:1.57
-    }
-}
+        # r:-0.471486
+        # p:-0.599778
+        # y:0.00246781
+        # cz:0.0986272
 
-# 3 lidar
-velodyne_lidars
-{
-    ip:""
-    port:2370
-    model:"VLP16"
-    calibrationFile:"../setting/VLP16db.yaml"
-    lidar_id:2
-    max_range:5.0
-    min_range:0.1
-    min_angle:0
-    max_angle:360
-    rpm:600
-    calib
-    {
-        y:1.57
+        r:0.462994
+        p:1.29624
+        y:96.0048
+        cz:0.0
     }
 }
 
+# # 3 lidar
+# velodyne_lidars
+# {
+#     ip:""
+#     port:2370
+#     model:"VLP16"
+#     calibrationFile:"../setting/VLP16db.yaml"
+#     lidar_id:2
+#     max_range:5.0
+#     min_range:0.1
+#     min_angle:0
+#     max_angle:360
+#     rpm:600
+#     calib
+#     {
+#         y:1.57
+#     }
+# }
+
 # 1 region
 region
 {
-    minx:-3.1
-	maxx:0
-	miny:-2.75
-	maxy:3.08
-	minz:0.01
+    minx:0.5
+	maxx:3.1
+	miny:-2.0
+	maxy:2.75
+	minz:0.02
 	maxz:0.2
     region_id:0
     lidar_exts
@@ -76,8 +92,8 @@ region
         lidar_id:0
         calib
         {
-            cx:0.1
-            cy:0.3
+            cx:0.0
+            cy:0.0
         }
     }
     lidar_exts
@@ -85,38 +101,40 @@ region
         lidar_id:1
         calib
         {
-            cx:0.1
-            cy:0.3
+            cx:-4.021775
+            cy:-0.039429
+            # cx:0.05668
+            # cy:-3.5723
         }
     }
 }
 
-# 2 region
-region
-{
-    minx:-3.1
-	maxx:0
-	miny:-2.75
-	maxy:3.08
-	minz:0.01
-	maxz:0.2
-    region_id:1
-    lidar_exts
-    {
-        lidar_id:1
-        calib
-        {
-            cx:0.1
-            cy:0.3
-        }
-    }
-    lidar_exts
-    {
-        lidar_id:2
-        calib
-        {
-            cx:0.1
-            cy:0.3
-        }
-    }
-}
+# # 2 region
+# region
+# {
+#     minx:-3.1
+# 	maxx:0
+# 	miny:-2.75
+# 	maxy:3.08
+# 	minz:0.01
+# 	maxz:0.2
+#     region_id:1
+#     lidar_exts
+#     {
+#         lidar_id:1
+#         calib
+#         {
+#             cx:0.1
+#             cy:0.3
+#         }
+#     }
+#     lidar_exts
+#     {
+#         lidar_id:2
+#         calib
+#         {
+#             cx:0.1
+#             cy:0.3
+#         }
+#     }
+# }

system/system_executor.cpp

@@ -301,8 +301,19 @@ Error_manager System_executor::encapsulate_send_status()
 		t_multi_status_msg.mutable_error_manager()->set_error_code(t_error.get_error_code());
         t_multi_status_msg.mutable_error_manager()->set_error_level((message::Error_level)t_error.get_error_level());
         t_multi_status_msg.mutable_error_manager()->set_error_description(t_error.get_error_description(), t_error.get_description_length());
-        std::string t_msg = t_multi_status_msg.SerializeAsString();
-        System_communication::get_instance_references().encapsulate_msg(t_msg);
+
+		pcl::PointCloud<pcl::PointXYZ>::Ptr t_region_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+		iter->second->get_region_cloud(t_region_cloud, Ground_region::Region_cloud_type::filtered);
+		for (size_t j = 0; j < t_region_cloud->size(); j++)
+		{
+			message::Cloud_coordinate *tp_cloud = t_multi_status_msg.add_cloud();
+			tp_cloud->set_x(t_region_cloud->points[j].x);
+			tp_cloud->set_y(t_region_cloud->points[j].y);
+			tp_cloud->set_z(t_region_cloud->points[j].z);
+		}
+
+		std::string t_msg = t_multi_status_msg.SerializeAsString();
+		System_communication::get_instance_references().encapsulate_msg(t_msg);
 //        std::cout<<t_multi_status_msg.DebugString()<<std::endl<<std::endl;
 	}
 

tests/lidar_calib_tools.cpp

@@ -2,7 +2,7 @@
  * @Description: lidar calib tools
  * @Author: yct
  * @Date: 2021-09-02 11:02:00
- * @LastEditTime: 2021-09-02 17:31:42
+ * @LastEditTime: 2021-09-07 17:53:53
  * @LastEditors: yct
  */
 
@@ -36,6 +36,10 @@ void find_intrinsic(Eigen::Vector3d ground_norm, Eigen::Vector3d y_axis)
             }
         }
     }
+    if(euler_angles[0]<0)
+    {
+        euler_angles[0] = euler_angles[0] + M_PI;
+    }
     std::cout << "lidar intrinsic y p r: " << euler_angles.transpose()*180.0/M_PI << std::endl;
 }
 
@@ -129,39 +133,49 @@ void find_rotation_from_points(pcl::PointCloud<pcl::PointXYZ>::Ptr front_barrier
 
 int main()
 {
+    std::cout << "------------------------ start calib lidar 0 ----------------------" << std::endl;
     // cloud 0 calib
     pcl::PointCloud<pcl::PointXYZ>::Ptr t_front, t_back;
     t_front = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
-    t_front->push_back(pcl::PointXYZ(3.158999919891,-0.365000009537,0.851999998093));
-    t_front->push_back(pcl::PointXYZ(3.158999919891,-0.363999992609,0.734000027180));
-    t_front->push_back(pcl::PointXYZ(3.154000043869,-0.363000005484,0.616999983788));
-    t_front->push_back(pcl::PointXYZ(3.154000043869,-0.361999988556,0.503000020981));
-    t_front->push_back(pcl::PointXYZ(3.155999898911,-0.361000001431,0.389999985695));
-    t_front->push_back(pcl::PointXYZ(3.140000104904,-0.368000000715,0.165999993682));
-    t_front->push_back(pcl::PointXYZ(3.148000001907,-0.368000000715,0.054999999702));
-    t_front->push_back(pcl::PointXYZ(3.142999887466,-0.363000005484,-0.054999999702));
     t_back = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
-
-    Eigen::Vector3d front_direction, back_direction, y_direction;
-
+    if (!read_pointcloud("/home/youchen/extra_space/chutian/fence_wj/front_pole.txt", t_front))
+    {
+        std::cout << "read front failed." << std::endl;
+        return -1;
+    }
     if(!read_pointcloud("/home/youchen/extra_space/chutian/fence_wj/back_pole.txt", t_back))
     {
-        std::cout << "read failed." << std::endl;
+        std::cout << "read back failed." << std::endl;
         return -1;
     }
 
+    Eigen::Vector3d front_direction, back_direction, y_direction;
+
+
     // front direction: 0.0191082  0.002411  0.999815
     // back direction: 0.0190939 0.0143106  0.999715
-    // 000.00802904 -0.000469358 00000.999968 0000.0897851
+    // 00.0103153 -0.0110542 000.999886 000.101527
     find_rotation_from_points(t_front, t_back, front_direction, back_direction, y_direction);
     // 经过测试，地面法向量不适宜使用杆子获取，还是使用地面环点云拟合获得
     // find_intrinsic((front_direction + back_direction) / 2.0, y_direction);
-    find_intrinsic(Eigen::Vector3d(0.00802904, -0.000469358, 0.999968), y_direction);
-
+    find_intrinsic(Eigen::Vector3d(0.0103153,-0.0110542,0.999886), y_direction);
 
+    std::cout << "------------------------ start calib lidar 1 ----------------------" << std::endl;
     // cloud 1
-    // 000.0104679 -0.00822843 0000.999911 000.0986272
-    find_intrinsic(Eigen::Vector3d(0.0104679, -0.00822843, 0.999911), Eigen::Vector3d::UnitY());
+    if (!read_pointcloud("/home/youchen/extra_space/chutian/fence_wj/front_pole2.txt", t_front))
+    {
+        std::cout << "read front failed." << std::endl;
+        return -1;
+    }
+    if(!read_pointcloud("/home/youchen/extra_space/chutian/fence_wj/back_pole2.txt", t_back))
+    {
+        std::cout << "read back failed." << std::endl;
+        return -1;
+    }
+    find_rotation_from_points(t_front, t_back, front_direction, back_direction, y_direction);
+
+    // -0.00566995 0-0.0233422 0000.999712 000.0949598
+    find_intrinsic(Eigen::Vector3d(-0.00566995,-0.0233422,0.999712), y_direction);
 
     return 0;
 }