Przeglądaj źródła

雷达初始化连接debug,区域初始化debug,状态信息个数调整

yct 4 lat temu
rodzic
commit
bc539a5de0

+ 4 - 22
CMakeLists.txt

@@ -1,10 +1,10 @@
-project(measure_dj_proj)
+project(measure_wj_proj)
 
 cmake_minimum_required(VERSION 3.5)
 
 set (CMAKE_CXX_STANDARD 11)
 
-set(PCL_DIR "/home/youchen/pcl-1.8/share/pcl-1.8")
+#set(PCL_DIR "/home/youchen/pcl-1.8/share/pcl-1.8")
 find_package(PkgConfig REQUIRED)
 pkg_check_modules(nanomsg REQUIRED nanomsg)
 FIND_PACKAGE(Protobuf REQUIRED)
@@ -20,8 +20,6 @@ include_directories(
         ${OpenCV_INCLUDE_DIRS}
         ${PROTOBUF_INCLUDE_DIRS}
 		${CERES_INCLUDE_DIRS}
-        laser
-        Locate
         communication
         message
         error_code
@@ -35,32 +33,20 @@ message(STATUS ${EXECUTABLE_OUTPUT_PATH})
 
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/error_code ERROR_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/message message_src )
-#aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/lidar_locate locate_src )
-#aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/robot robot_src )
-aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/laser LASER_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/wanji_lidar WANJI_LIDAR_SRC )
 
-#aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/plc PLC_SRC )
-aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/locate LOCATE_SRC )
-#aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/terminor TERMINOR_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/task TASK_MANAGER_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/tool TOOL_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/communication COMMUNICATION_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/system SYSTEM_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/verify VERIFY_SRC )
 
-add_executable(measure_dj
+add_executable(measure_wj
         main.cpp
         ${ERROR_SRC}
-        ${locate_src}
-        ${robot_src}
         ${message_src}
 
-		${LASER_SRC}
 		${WANJI_LIDAR_SRC}
-        ${PLC_SRC}
-        ${TERMINOR_SRC}
-        ${LOCATE_SRC}
         ${TASK_MANAGER_SRC}
         ${TOOL_SRC}
         ${COMMUNICATION_SRC}
@@ -69,7 +55,7 @@ add_executable(measure_dj
 
 		)
 
-target_link_libraries(measure_dj
+target_link_libraries(measure_wj
         /usr/local/lib/libglog.a
         /usr/local/lib/libgflags.a
         /usr/local/lib/liblivox_sdk_static.a
@@ -83,10 +69,6 @@ target_link_libraries(measure_dj
         ${PCL_LIBRARIES}
 		${CERES_LIBRARIES}
 
-        libtensorflow_cc.so
-#        tf_3dcnn_api.so
-        pointSIFT_API.so
-
         -lpthread
         )
 

+ 10 - 4
main.cpp

@@ -4,7 +4,7 @@
 
 #include <iostream>
 #include "./error_code/error_code.h"
-#include "LogFiles.h"
+#include "./wanji_lidar/LogFiles.h"
 #include <glog/logging.h>
 
 #include "./communication/communication_socket_base.h"
@@ -42,7 +42,7 @@ GOOGLE_GLOG_DLL_DECL void shut_down_logging(const char* data, int size)
 
 void test_whole_process()
 {
-	System_executor::get_instance_references().execute_for_measure("key_for_test_only", 5, std::chrono::system_clock::now());
+	System_executor::get_instance_references().execute_for_measure("key_for_test_only", 1, std::chrono::system_clock::now());
 }
 
 int main(int argc,char* argv[])
@@ -76,13 +76,19 @@ int main(int argc,char* argv[])
 
 	// 初始化
 	Wanji_manager::get_instance_references().wanji_manager_init();
-	std::cout << "wanji_manager = " << Wanji_manager::get_instance_references().check_status() << std::endl;
+	std::cout << "wanji_manager = " << Wanji_manager::get_instance_references().check_status().to_string() << std::endl;
 	System_executor::get_instance_references().system_executor_init(4, t_terminal_id);
 	std::cout << "System_executor = " << System_executor::get_instance_references().get_system_executor_status() << std::endl;
 	System_communication::get_instance_references().communication_init();
 
 	// prev_test_pred_task();
-	test_whole_process();
+//	test_whole_process();
+//    usleep(1000*5000);
+    char ch='x' ;
+    while(ch != 'q') {
+        std::cout<<"please input q to quit system."<<std::endl;
+        std::cin >> ch;
+    }
 
 	// 反初始化
 	System_communication::get_instance_references().communication_uninit();

+ 2 - 2
setting/communication.prototxt

@@ -24,8 +24,8 @@ communication_parameters
 
 #   connect_string_vector:"tcp://192.168.2.233:2333"
 
-bind_string:"tcp://192.168.2.141:30008"
-connect_string_vector:"tcp://192.168.2.141:30000"
+bind_string:"tcp://192.168.0.64:30009"
+connect_string_vector:"tcp://192.168.0.64:30000"
 
 
 }

+ 491 - 8
setting/wanji_manager.prototxt

@@ -1,33 +1,516 @@
+fence_data_path:"/home/zx/data/fence_wj"
+#fence_log_path:"/home/zx/yct/MainStructure/new_electronic_fence/log"
 
+#1
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.201"
+        port:2110
+    }
+    transform
+    {
+        m00:-0.100059
+		m01:0.9949814
+		m02:-1.470
+		m10:0.9949814
+		m11:0.1000593
+		m12:-2.285
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
 
+#2
 wj_lidar
 {
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
     net_config
     {
-        ip_address:"192.168.0.208"
+        ip_address:"192.168.2.202"
         port:2110
     }
     transform
     {
+        m00:-0.0123952
+		m01:-0.9999232
+		m02:1.851
+		m10:-0.9999232
+		m11:0.0123952
+		m12:3.48100000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
 
+#3
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.203"
+        port:2110
+    }
+    transform
+    {
+        m00:0.0108588
+		m01:0.9999409
+		m02:5.229
+		m10:0.9999409
+		m11:-0.0108588
+		m12:-2.3500000
     }
     scan_limit
     {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
 
+#4
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.204"
+        port:2110
+    }
+    transform
+    {
+        m00:0.0549574
+		m01:-0.9984888
+		m02:8.717
+		m10:-0.9984888
+		m11:-0.0549574
+		m12:3.54400000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
     }
 }
 
-regions
+#5
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.205"
+        port:2110
+    }
+    transform
+    {
+    	m00:-0.0196446
+		m01:0.9998069
+		m02:12.136
+		m10:0.9998069
+		m11:0.0196446
+		m12:-2.3190000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#6
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.206"
+        port:2110
+    }
+    transform
+    {
+        m00:0.085926
+		m01:-0.9963016
+		m02:15.675
+		m10:-0.9963016
+		m11:-0.0859263
+		m12:3.48400000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#7
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.207"
+        port:2110
+    }
+    transform
+    {
+        m00:-0.0609897
+		m01:0.9981384
+		m02:19.069
+		m10:0.9981384
+		m11:0.0609897
+		m12:-2.3750000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#8
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.208"
+        port:2110
+    }
+    transform
+    {
+        m00:0.0473273
+		m01:-0.9988794
+		m02:-1.398
+		m10:-0.9988794
+		m11:-0.0473273
+		m12:3.35600000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
+
+#9
+wj_lidar
 {
-     minx:-10000
-     maxx:-10000
-     miny:10000
-     maxy:10000
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.209"
+        port:2110
+    }
+    transform
+    {
+        m00:-0.0773542
+		m01:0.9970038
+		m02:1.742
+		m10:0.9970038
+		m11:0.0773542
+		m12:-2.5350000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
 }
 
-fence_data_path:"../data"
-fence_log_path:"../log"
+#10
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.210"
+        port:2110
+    }
+    transform
+    {
+        m00:0.0255674
+		m01:-0.9996730
+		m02:5.233
+		m10:-0.9996730
+		m11:-0.0255674
+		m12:3.44200000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
 
+#11
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.211"
+        port:2110
+    }
+    transform
+    {
+        m00:0.1196470
+		m01:0.9928164
+		m02:8.617
+		m10:0.9928164
+		m11:-0.1196470
+		m12:-2.5510000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:3.2
+    }
+}
 
+#12
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.212"
+        port:2110
+    }
+    transform
+    {
+        m00:0.0158848
+		m01:-0.9998739
+		m02:12.162
+		m10:-0.9998739
+		m11:-0.0158848
+		m12:3.43600000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:4
+    }
+}
 
+#13
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.213"
+        port:2110
+    }
+    transform
+    {
+        m00:-0.0459619
+		m01:0.9989432
+		m02:15.555
+		m10:0.9989432
+		m11:0.0459619
+		m12:-2.5410000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5.5
+        miny:-4
+        maxy:3.25
+    }
+}
+
+#14
+wj_lidar
+{
+    angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"192.168.2.214"
+        port:2110
+    }
+    transform
+    {
+        m00:-0.1357627
+		m01:-0.9907414
+		m02:18.616
+		m10:-0.9907414
+		m11:0.1357627
+		m12:3.48700000
+    }
+    scan_limit
+    {
+        dist_limit:8
+        minx:0.1
+        maxx:5
+        miny:-4
+        maxy:4
+    }
+}
+
+
+#1
+regions
+{
+	minx:-0.7
+	maxx:1.8
+	miny:-2.43
+	maxy:3.3
+}
+
+#2
+regions
+{
+	minx:2.5
+	maxx:5.2
+	miny:-2.4
+	maxy:3.43
+}
 
+#3
+regions
+{
+	minx:5.4
+	maxx:8.7
+	miny:-2.40
+	maxy:3.43
+}
+#4
+regions
+{
+    minx:8.7
+    maxx:12
+    miny:-2.40
+    maxy:3.45
+}
+#5
+regions
+{
+    minx:12.1
+    maxx:15.4
+    miny:-2.40
+    maxy:3.45
+}
+#6
+regions
+{
+    minx:15.8
+    maxx:18.8
+    miny:-2.40
+    maxy:3.45
+}

+ 0 - 253
setting/wj_manager_settings.prototxt

@@ -1,253 +0,0 @@
-fence_data_path:"/home/zx/data/fence_wj"
-#fence_log_path:"/home/zx/yct/MainStructure/new_electronic_fence/log"
-
-
-#2
-wj_lidar
-{
-    angle_min:-2.35619449
-    angle_max:2.35619449
-    angle_increment:0.00582
-    time_increment:0.000062
-    range_min:0
-    range_max:30
-    net_config
-    {
-        ip_address:"192.168.2.202"
-        port:2110
-    }
-    transform
-    {
-        m00:-0.0123952
-		m01:-0.9999232
-		m02:1.851
-		m10:-0.9999232
-		m11:0.0123952
-		m12:3.48100000
-    }
-    scan_limit
-    {
-        dist_limit:8
-        minx:0.1
-        maxx:5.5
-        miny:-4
-        maxy:4
-    }
-}
-
-#3
-wj_lidar
-{
-    angle_min:-2.35619449
-    angle_max:2.35619449
-    angle_increment:0.00582
-    time_increment:0.000062
-    range_min:0
-    range_max:30
-    net_config
-    {
-        ip_address:"192.168.2.203"
-        port:2110
-    }
-    transform
-    {
-        m00:0.0108588
-		m01:0.9999409
-		m02:5.229
-		m10:0.9999409
-		m11:-0.0108588
-		m12:-2.3500000
-    }
-    scan_limit
-    {
-        dist_limit:8
-        minx:0.1
-        maxx:5.5
-        miny:-4
-        maxy:4
-    }
-}
-
-#4
-wj_lidar
-{
-    angle_min:-2.35619449
-    angle_max:2.35619449
-    angle_increment:0.00582
-    time_increment:0.000062
-    range_min:0
-    range_max:30
-    net_config
-    {
-        ip_address:"192.168.2.204"
-        port:2110
-    }
-    transform
-    {
-        m00:0.0549574
-		m01:-0.9984888
-		m02:8.717
-		m10:-0.9984888
-		m11:-0.0549574
-		m12:3.54400000
-    }
-    scan_limit
-    {
-        dist_limit:8
-        minx:0.1
-        maxx:5.5
-        miny:-4
-        maxy:4
-    }
-}
-
-#5
-wj_lidar
-{
-    angle_min:-2.35619449
-    angle_max:2.35619449
-    angle_increment:0.00582
-    time_increment:0.000062
-    range_min:0
-    range_max:30
-    net_config
-    {
-        ip_address:"192.168.2.205"
-        port:2110
-    }
-    transform
-    {
-    	m00:-0.0196446
-		m01:0.9998069
-		m02:12.136
-		m10:0.9998069
-		m11:0.0196446
-		m12:-2.3190000
-    }
-    scan_limit
-    {
-        dist_limit:8
-        minx:0.1
-        maxx:5.5
-        miny:-4
-        maxy:4
-    }
-}
-
-#6
-wj_lidar
-{
-    angle_min:-2.35619449
-    angle_max:2.35619449
-    angle_increment:0.00582
-    time_increment:0.000062
-    range_min:0
-    range_max:30
-    net_config
-    {
-        ip_address:"192.168.2.206"
-        port:2110
-    }
-    transform
-    {
-        m00:0.085926
-		m01:-0.9963016
-		m02:15.675
-		m10:-0.9963016
-		m11:-0.0859263
-		m12:3.48400000
-    }
-    scan_limit
-    {
-        dist_limit:8
-        minx:0.1
-        maxx:5.5
-        miny:-4
-        maxy:4
-    }
-}
-
-#7
-wj_lidar
-{
-    angle_min:-2.35619449
-    angle_max:2.35619449
-    angle_increment:0.00582
-    time_increment:0.000062
-    range_min:0
-    range_max:30
-    net_config
-    {
-        ip_address:"192.168.2.207"
-        port:2110
-    }
-    transform
-    {
-        m00:-0.0609897
-		m01:0.9981384
-		m02:19.069
-		m10:0.9981384
-		m11:0.0609897
-		m12:-2.3750000
-    }
-    scan_limit
-    {
-        dist_limit:8
-        minx:0.1
-        maxx:5.5
-        miny:-4
-        maxy:4
-    }
-}
-
-#1
-regions
-{
-	minx:-0.5
-	maxx:2.4
-	miny:-2.43
-	maxy:3.43
-}
-
-#2
-regions
-{
-	minx:2.5
-	maxx:5.3
-	miny:-2.4
-	maxy:3.43
-}
-
-#3
-regions
-{
-	minx:5.4
-	maxx:8.7
-	miny:-2.40
-	maxy:3.43
-}
-#4
-regions
-{
-    minx:8.7
-    maxx:12
-    miny:-2.40
-    maxy:3.45
-}
-#5
-regions
-{
-    minx:12.1
-    maxx:15.4
-    miny:-2.40
-    maxy:3.45
-}
-#6
-regions
-{
-    minx:15.8
-    maxx:18.8
-    miny:-2.40
-    maxy:3.45
-}
-

+ 14 - 11
system/system_executor.cpp

@@ -227,6 +227,7 @@ Error_manager System_executor::encapsulate_send_status()
 	t_ground_status_msg.mutable_base_info()->set_sender(message::Communicator::eGround_measurer);
 	t_ground_status_msg.mutable_base_info()->set_receiver(message::Communicator::eEmpty);
 
+    // 普爱统一一个万集节点
 	t_ground_status_msg.set_terminal_id(m_terminal_id);
 
 	//万集716
@@ -241,17 +242,21 @@ Error_manager System_executor::encapsulate_send_status()
 	}
 
 	std::map<int, Region_worker*> & t_region_worker_map = Wanji_manager::get_instance_references().get_region_worker_map();
+	int region_index = 0;
 	for (auto iter = t_region_worker_map.begin(); iter != t_region_worker_map.end(); ++iter)
 	{
+        message::Ground_status_msg t_multi_status_msg;
+        t_multi_status_msg.CopyFrom(t_ground_status_msg);
+        t_multi_status_msg.set_terminal_id(region_index++);
 		Region_worker::Region_worker_status t_region_worker_status = (*iter).second->get_status();
-		t_ground_status_msg.add_region_worker_status((message::Region_worker_status)t_region_worker_status);
+        t_multi_status_msg.add_region_worker_status((message::Region_worker_status)t_region_worker_status);
 
 		//万集雷达的自动定位信息
 		Common_data::Car_wheel_information	t_car_wheel_information;
 		t_error = (*iter).second->get_last_wheel_information(&t_car_wheel_information, std::chrono::system_clock::now());
 
 		//无论定位结果如何, 都要填充数据, 即使全部写0, 必须保证通信格式正确.
-		message::Locate_information* tp_locate_information = t_ground_status_msg.add_locate_information_realtime();
+		message::Locate_information* tp_locate_information = t_multi_status_msg.add_locate_information_realtime();
 		tp_locate_information->set_locate_x(t_car_wheel_information.center_x);
 		tp_locate_information->set_locate_y(t_car_wheel_information.center_y);
 		tp_locate_information->set_locate_angle(t_car_wheel_information.car_angle);
@@ -262,20 +267,18 @@ Error_manager System_executor::encapsulate_send_status()
 		tp_locate_information->set_locate_wheel_width(t_car_wheel_information.wheel_width);
 		tp_locate_information->set_locate_front_theta(t_car_wheel_information.front_theta);
 		tp_locate_information->set_locate_correct(t_car_wheel_information.correctness);
-	}
-
 
-	t_ground_status_msg.mutable_error_manager()->set_error_code(t_error.get_error_code());
-	t_ground_status_msg.mutable_error_manager()->set_error_level((message::Error_level)t_error.get_error_level());
-	t_ground_status_msg.mutable_error_manager()->set_error_description(t_error.get_error_description(), t_error.get_description_length());
+        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);
+	}
 
 //	std::cout << " huli test :::: " << " t_ground_status_msg.DebugString() = "  << std::endl;
 //	std::cout << t_ground_status_msg.DebugString() << std::endl;
 
-
-	std::string t_msg = t_ground_status_msg.SerializeAsString();
-	System_communication::get_instance_references().encapsulate_msg(t_msg);
-
+//    LOG(WARNING) << t_ground_status_msg.DebugString();
 //	std::cout << " huli test :::: " << " t_ground_status_msg.DebugString() = " << t_ground_status_msg.DebugString() << std::endl;
 
 	return Error_code::SUCCESS;

+ 5 - 7
wanji_lidar/region_detect.cpp

@@ -34,7 +34,6 @@ Error_manager Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr p_
 	// 1.参数合法性检验
 	if (p_cloud->size() <= 0)
 		return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
-	std::cout << " huli test :::: " << " 222 = " << 222 << std::endl;
 	// 2.直通滤波, 筛选xy
 	pcl::PassThrough<pcl::PointXYZ> pass;
 	pass.setInputCloud(p_cloud);
@@ -42,7 +41,6 @@ Error_manager Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr p_
 	pass.setFilterLimits(m_region_box.x_min, m_region_box.x_max); //将x轴的0到1范围内
 	pass.setFilterLimitsNegative(false);                                //保留(true就是删除,false就是保留而删除此区间外的)
 	pass.filter(*p_cloud);                                            //输出到结果指针
-	std::cout << " huli test :::: " << " 333 = " << 333 << std::endl;
 	if (p_cloud->size() <= 0)
 		return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
 
@@ -51,10 +49,10 @@ Error_manager Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr p_
 	pass.setFilterLimits(m_region_box.y_min, m_region_box.y_max); //将x轴的0到1范围内
 	pass.setFilterLimitsNegative(false);                                //保留(true就是删除,false就是保留而删除此区间外的)
 	pass.filter(*p_cloud);                                            //输出到结果指针
-	std::cout << " huli test :::: " << " 444 = " << 444 << std::endl;
+//    std::cout << p_cloud->size()<<","<<m_region_box.x_min<<","<<m_region_box.x_max<<","<<m_region_box.y_min<<","<<m_region_box.y_max<<std::endl;
 	if (p_cloud->size() <= 0)
 		return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
-	std::cout << " huli test :::: " << " 445 = " << 445 << std::endl;
+
 	// 3.离群点过滤
 	pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
 	sor.setInputCloud(p_cloud);
@@ -62,7 +60,7 @@ Error_manager Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr p_
 	sor.setStddevMulThresh(3.0); //标准差倍数
 	sor.setNegative(false);      //保留未滤波点(内点)
 	sor.filter(*p_cloud);      //保存滤波结果到cloud_filter
-	std::cout << " huli test :::: " << " 555 = " << 555 << std::endl;
+
 	if (p_cloud->size() <= 0)
 		return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
 	else
@@ -527,13 +525,13 @@ Error_manager Region_detector::detect_ex(std::mutex* p_cloud_mutex, pcl::PointCl
 	result = preprocess(p_cloud_filtered);
 	if (result != SUCCESS)
 		return result;
-	std::cout << " huli test :::: " << " 111 = " << 111 << std::endl;
+//	std::cout << " huli test :::: " << " 111 = " << 111 << std::endl;
 	// 2.聚类
 	std::vector<pcl::PointCloud<pcl::PointXYZ>> seg_clouds;
 	result = clustering_only(p_cloud_filtered, seg_clouds, print);
 	if (result != SUCCESS)
 		return result;
-	std::cout << " huli test :::: " << " 112 = " << 112 << std::endl;
+//	std::cout << " huli test :::: " << " 112 = " << 112 << std::endl;
 	detect_wheel_ceres::Detect_result detect_result;
 	if(!m_detector_ceres.detect(seg_clouds, detect_result))
 	{

+ 1 - 1
wanji_lidar/region_detect.h

@@ -63,7 +63,7 @@ public:
     Common_data::Car_wheel_information& car_wheel_information, bool print=true);
     //通过ceres检测中心,旋转,轴距,宽度,  新算法, 可以测量前轮旋转
     Error_manager detect_ex(std::mutex* p_cloud_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
-    Common_data::Car_wheel_information& car_wheel_information, bool print=true);
+    Common_data::Car_wheel_information& car_wheel_information, bool print=false);
 
 protected:
     // 预处理,直通滤波限制点云范围

+ 1 - 1
wanji_lidar/region_worker.cpp

@@ -203,7 +203,7 @@ void Region_worker::auto_detect_thread_fun()
 		{
 //			std::cout << " huli test :::: " << "  = " << "---------------------------------------------------------------------------------" << std::endl;
 //
-//			std::cout << " huli test :::: " << " mp_cloud_collection->size() = " << mp_cloud_collection->size() << std::endl;
+//			LOG(INFO) << " huli test :::: " << " mp_cloud_collection->size() = " << mp_cloud_collection->size();
 //
 //			auto start   = std::chrono::system_clock::now();
 

+ 1 - 1
wanji_lidar/wanji_lidar_device.cpp

@@ -57,7 +57,7 @@ Error_manager Wanji_lidar_device::init(wj::wjLidarParams params)
 	std::string	t_ip = params.net_config().ip_address();
 	int t_port = params.net_config().port();
 
-	t_error = m_async_client.init("192.168.0.208", 2110, &m_communication_data_queue);
+	t_error = m_async_client.init(t_ip, t_port, &m_communication_data_queue);
 	if ( t_error != SUCCESS )
 	{
 		return t_error;

+ 2 - 1
wanji_lidar/wanji_manager.cpp

@@ -56,6 +56,7 @@ Error_manager Wanji_manager::wanji_manager_init_from_protobuf(wj::wjManagerParam
 	{
 		Wanji_lidar_device* p_wanji_lidar_device(new Wanji_lidar_device);
 		t_error = p_wanji_lidar_device->init(wanji_parameters.wj_lidar(i));
+//		LOG(WARNING) << wanji_parameters.wj_lidar(i).DebugString();
 		if ( t_error != Error_code::SUCCESS )
 		{
 			delete(p_wanji_lidar_device);
@@ -73,7 +74,7 @@ Error_manager Wanji_manager::wanji_manager_init_from_protobuf(wj::wjManagerParam
 		t_point2d_box.x_min = wanji_parameters.regions(i).minx();
 		t_point2d_box.x_max = wanji_parameters.regions(i).maxx();
 		t_point2d_box.y_min = wanji_parameters.regions(i).miny();
-		t_point2d_box.y_min = wanji_parameters.regions(i).maxy();
+		t_point2d_box.y_max = wanji_parameters.regions(i).maxy();
 		t_error = p_region_worker->init(t_point2d_box, &m_cloud_collection_mutex, mp_cloud_collection);
 		if ( t_error != Error_code::SUCCESS )
 		{