measure width debug, center x border debug,

params update

.gitignore

@@ -36,4 +36,7 @@ packages/
 .idea*
 *.dll
 终端/ct_terminal/ct_terminal/Resource*
-管理节点/*
+cmake-build-debug/**
+plc调度节点/build/**
+测量节点/build/**
+.vscode

CMakeLists.txt

@@ -0,0 +1,308 @@
+cmake_minimum_required(VERSION 3.15)
+project(CT_project)
+
+set(CMAKE_CXX_STANDARD 14)
+
+include_directories(plc调度节点/communication)
+include_directories(plc调度节点/dispatch)
+include_directories(plc调度节点/error_code)
+include_directories(plc调度节点/message)
+include_directories(plc调度节点/rabbitmq)
+include_directories(plc调度节点/snap7_communication)
+include_directories(plc调度节点/system)
+include_directories(plc调度节点/task)
+include_directories(plc调度节点/tool)
+include_directories(plc调度节点/tool/TaskQueue)
+include_directories(plc调度节点/tool/TaskQueue/threadpp)
+include_directories(plc调度节点/tool/TaskQueue/threadpp/impl)
+include_directories(测量节点/communication)
+include_directories(测量节点/error_code)
+include_directories(测量节点/message)
+include_directories(测量节点/rabbitmq)
+include_directories(测量节点/system)
+include_directories(测量节点/task)
+include_directories(测量节点/tool)
+include_directories(测量节点/tool/TaskQueue)
+include_directories(测量节点/tool/TaskQueue/threadpp)
+include_directories(测量节点/tool/TaskQueue/threadpp/impl)
+include_directories(测量节点/velodyne_lidar)
+include_directories(测量节点/velodyne_lidar/match3d)
+include_directories(测量节点/velodyne_lidar/match3d/common)
+include_directories(测量节点/velodyne_lidar/velodyne_driver)
+include_directories(测量节点/verify)
+include_directories(测量节点/wanji_lidar)
+
+add_executable(CT_project
+        plc调度节点/communication/communication.pb.cc
+        plc调度节点/communication/communication.pb.h
+        plc调度节点/communication/communication_message.cpp
+        plc调度节点/communication/communication_message.h
+        plc调度节点/communication/communication_socket_base.cpp
+        plc调度节点/communication/communication_socket_base.h
+        plc调度节点/dispatch/database_communication_configuration.pb.cc
+        plc调度节点/dispatch/database_communication_configuration.pb.h
+        plc调度节点/dispatch/database_controller.cpp
+        plc调度节点/dispatch/database_controller.h
+        plc调度节点/dispatch/dispatch_command.cpp
+        plc调度节点/dispatch/dispatch_command.h
+        plc调度节点/dispatch/dispatch_communication.cpp
+        plc调度节点/dispatch/dispatch_communication.h
+        plc调度节点/dispatch/dispatch_coordinates.cpp
+        plc调度节点/dispatch/dispatch_coordinates.h
+        plc调度节点/dispatch/dispatch_ground_lidar.cpp
+        plc调度节点/dispatch/dispatch_ground_lidar.h
+        plc调度节点/dispatch/dispatch_manager.cpp
+        plc调度节点/dispatch/dispatch_manager.h
+        plc调度节点/dispatch/dispatch_parameter.pb.cc
+        plc调度节点/dispatch/dispatch_parameter.pb.h
+        plc调度节点/dispatch/dispatch_plc.cpp
+        plc调度节点/dispatch/dispatch_plc.h
+        plc调度节点/dispatch/dispatch_singlechip.cpp
+        plc调度节点/dispatch/dispatch_singlechip.h
+        plc调度节点/error_code/error_code.cpp
+        plc调度节点/error_code/error_code.h
+        plc调度节点/message/message.pb.cc
+        plc调度节点/message/message.pb.h
+        plc调度节点/message/message_base.pb.cc
+        plc调度节点/message/message_base.pb.h
+        plc调度节点/rabbitmq/rabbitmq.pb.cc
+        plc调度节点/rabbitmq/rabbitmq.pb.h
+        plc调度节点/rabbitmq/rabbitmq_base.cpp
+        plc调度节点/rabbitmq/rabbitmq_base.h
+        plc调度节点/rabbitmq/rabbitmq_message.cpp
+        plc调度节点/rabbitmq/rabbitmq_message.h
+        plc调度节点/rabbitmq/ttt.cpp
+        plc调度节点/rabbitmq/ttt.h
+        plc调度节点/snap7_communication/plc_data.cpp
+        plc调度节点/snap7_communication/plc_data.h
+        plc调度节点/snap7_communication/s7_plc.cpp
+        plc调度节点/snap7_communication/s7_plc.h
+        plc调度节点/snap7_communication/snap7_buf.cpp
+        plc调度节点/snap7_communication/snap7_buf.h
+        plc调度节点/snap7_communication/snap7_communication.pb.cc
+        plc调度节点/snap7_communication/snap7_communication.pb.h
+        plc调度节点/snap7_communication/snap7_communication_base.cpp
+        plc调度节点/snap7_communication/snap7_communication_base.h
+        plc调度节点/system/system_communication.cpp
+        plc调度节点/system/system_communication.h
+        plc调度节点/system/system_executor.cpp
+        plc调度节点/system/system_executor.h
+        plc调度节点/task/task_base.cpp
+        plc调度节点/task/task_base.h
+        plc调度节点/task/task_command_manager.cpp
+        plc调度节点/task/task_command_manager.h
+        plc调度节点/tool/TaskQueue/threadpp/impl/pthread_lock.h
+        plc调度节点/tool/TaskQueue/threadpp/impl/pthread_lock.hpp
+        plc调度节点/tool/TaskQueue/threadpp/impl/pthread_thread.h
+        plc调度节点/tool/TaskQueue/threadpp/impl/pthread_thread.hpp
+        plc调度节点/tool/TaskQueue/threadpp/impl/std_lock.h
+        plc调度节点/tool/TaskQueue/threadpp/impl/std_lock.hpp
+        plc调度节点/tool/TaskQueue/threadpp/impl/std_thread.h
+        plc调度节点/tool/TaskQueue/threadpp/impl/std_thread.hpp
+        plc调度节点/tool/TaskQueue/threadpp/impl/win_lock.h
+        plc调度节点/tool/TaskQueue/threadpp/impl/win_lock.hpp
+        plc调度节点/tool/TaskQueue/threadpp/impl/win_thread.h
+        plc调度节点/tool/TaskQueue/threadpp/impl/win_thread.hpp
+        plc调度节点/tool/TaskQueue/threadpp/recursive_lock.h
+        plc调度节点/tool/TaskQueue/threadpp/threadpp.h
+        plc调度节点/tool/TaskQueue/threadpp/threadpp_assert.h
+        plc调度节点/tool/TaskQueue/BaseTask.cpp
+        plc调度节点/tool/TaskQueue/BaseTask.h
+        plc调度节点/tool/TaskQueue/TaskPool.h
+        plc调度节点/tool/TaskQueue/ThreadTaskQueue.cpp
+        plc调度节点/tool/TaskQueue/ThreadTaskQueue.h
+        plc调度节点/tool/TaskQueue/TQFactory.cpp
+        plc调度节点/tool/TaskQueue/TQFactory.h
+        plc调度节点/tool/TaskQueue/TQInterface.h
+        plc调度节点/tool/binary_buf.cpp
+        plc调度节点/tool/binary_buf.h
+        plc调度节点/tool/common_data.cpp
+        plc调度节点/tool/common_data.h
+        plc调度节点/tool/measure_filter.h
+        plc调度节点/tool/pathcreator.cpp
+        plc调度节点/tool/pathcreator.h
+        plc调度节点/tool/pcl_cloud_with_lock.cpp
+        plc调度节点/tool/pcl_cloud_with_lock.h
+        plc调度节点/tool/point2D_tool.cpp
+        plc调度节点/tool/point2D_tool.h
+        plc调度节点/tool/point3D_tool.cpp
+        plc调度节点/tool/point3D_tool.h
+        plc调度节点/tool/point_tool.cpp
+        plc调度节点/tool/point_tool.h
+        plc调度节点/tool/proto_tool.cpp
+        plc调度节点/tool/proto_tool.h
+        plc调度节点/tool/singleton.cpp
+        plc调度节点/tool/singleton.h
+        plc调度节点/tool/string_convert.cpp
+        plc调度节点/tool/string_convert.h
+        plc调度节点/tool/thread_condition.cpp
+        plc调度节点/tool/thread_condition.h
+        plc调度节点/tool/thread_pool.h
+        plc调度节点/tool/thread_safe_list.cpp
+        plc调度节点/tool/thread_safe_list.h
+        plc调度节点/tool/thread_safe_queue.cpp
+        plc调度节点/tool/thread_safe_queue.h
+        plc调度节点/tool/time_tool.cpp
+        plc调度节点/tool/time_tool.h
+        plc调度节点/main.cpp
+        入口引导提示节点/led资料文档/led_protocol.cpp
+        测量节点/communication/communication.pb.cc
+        测量节点/communication/communication.pb.h
+        测量节点/communication/communication_message.cpp
+        测量节点/communication/communication_message.h
+        测量节点/communication/communication_socket_base.cpp
+        测量节点/communication/communication_socket_base.h
+        测量节点/error_code/error_code.cpp
+        测量节点/error_code/error_code.h
+        测量节点/message/measure_message.pb.cc
+        测量节点/message/measure_message.pb.h
+        测量节点/message/message.pb.cc
+        测量节点/message/message.pb.h
+        测量节点/message/message_base.pb.cc
+        测量节点/message/message_base.pb.h
+        测量节点/rabbitmq/rabbitmq.pb.cc
+        测量节点/rabbitmq/rabbitmq.pb.h
+        测量节点/rabbitmq/rabbitmq_base.cpp
+        测量节点/rabbitmq/rabbitmq_base.h
+        测量节点/rabbitmq/rabbitmq_message.cpp
+        测量节点/rabbitmq/rabbitmq_message.h
+        测量节点/rabbitmq/ttt.cpp
+        测量节点/rabbitmq/ttt.h
+        "测量节点/system/system_communication mq.cpp"
+        测量节点/system/system_communication.cpp
+        测量节点/system/system_communication.h
+        测量节点/system/system_communication_mq.h
+        测量节点/system/system_executor.cpp
+        测量节点/system/system_executor.h
+        测量节点/task/task_base.cpp
+        测量节点/task/task_base.h
+        测量节点/task/task_command_manager.cpp
+        测量节点/task/task_command_manager.h
+        测量节点/tests/lidar_calib_tools.cpp
+        测量节点/tests/vlp16_driver_test.cpp
+        测量节点/tool/TaskQueue/threadpp/impl/pthread_lock.h
+        测量节点/tool/TaskQueue/threadpp/impl/pthread_lock.hpp
+        测量节点/tool/TaskQueue/threadpp/impl/pthread_thread.h
+        测量节点/tool/TaskQueue/threadpp/impl/pthread_thread.hpp
+        测量节点/tool/TaskQueue/threadpp/impl/std_lock.h
+        测量节点/tool/TaskQueue/threadpp/impl/std_lock.hpp
+        测量节点/tool/TaskQueue/threadpp/impl/std_thread.h
+        测量节点/tool/TaskQueue/threadpp/impl/std_thread.hpp
+        测量节点/tool/TaskQueue/threadpp/impl/win_lock.h
+        测量节点/tool/TaskQueue/threadpp/impl/win_lock.hpp
+        测量节点/tool/TaskQueue/threadpp/impl/win_thread.h
+        测量节点/tool/TaskQueue/threadpp/impl/win_thread.hpp
+        测量节点/tool/TaskQueue/threadpp/recursive_lock.h
+        测量节点/tool/TaskQueue/threadpp/threadpp.h
+        测量节点/tool/TaskQueue/threadpp/threadpp_assert.h
+        测量节点/tool/TaskQueue/BaseTask.cpp
+        测量节点/tool/TaskQueue/BaseTask.h
+        测量节点/tool/TaskQueue/TaskPool.h
+        测量节点/tool/TaskQueue/ThreadTaskQueue.cpp
+        测量节点/tool/TaskQueue/ThreadTaskQueue.h
+        测量节点/tool/TaskQueue/TQFactory.cpp
+        测量节点/tool/TaskQueue/TQFactory.h
+        测量节点/tool/TaskQueue/TQInterface.h
+        测量节点/tool/binary_buf.cpp
+        测量节点/tool/binary_buf.h
+        测量节点/tool/common_data.cpp
+        测量节点/tool/common_data.h
+        测量节点/tool/icp_svd_registration.cpp
+        测量节点/tool/icp_svd_registration.hpp
+        测量节点/tool/measure_filter.h
+        测量节点/tool/pathcreator.cpp
+        测量节点/tool/pathcreator.h
+        测量节点/tool/pcl_cloud_with_lock.cpp
+        测量节点/tool/pcl_cloud_with_lock.h
+        测量节点/tool/point2D_tool.cpp
+        测量节点/tool/point2D_tool.h
+        测量节点/tool/point3D_tool.cpp
+        测量节点/tool/point3D_tool.h
+        测量节点/tool/point_tool.cpp
+        测量节点/tool/point_tool.h
+        测量节点/tool/proto_tool.cpp
+        测量节点/tool/proto_tool.h
+        测量节点/tool/region_status_checker.h
+        测量节点/tool/rotate_center_caliber.cpp
+        测量节点/tool/rotate_center_caliber.h
+        测量节点/tool/singleton.cpp
+        测量节点/tool/singleton.h
+        测量节点/tool/thread_condition.cpp
+        测量节点/tool/thread_condition.h
+        测量节点/tool/thread_pool.h
+        测量节点/tool/thread_safe_list.cpp
+        测量节点/tool/thread_safe_list.h
+        测量节点/tool/thread_safe_queue.cpp
+        测量节点/tool/thread_safe_queue.h
+        测量节点/tool/time_tool.cpp
+        测量节点/tool/time_tool.h
+        测量节点/velodyne_lidar/match3d/common/math.h
+        测量节点/velodyne_lidar/match3d/common/port.h
+        测量节点/velodyne_lidar/match3d/detect_ceres3d.cpp
+        测量节点/velodyne_lidar/match3d/detect_ceres3d.h
+        测量节点/velodyne_lidar/match3d/detect_wheel_ceres3d.cpp
+        测量节点/velodyne_lidar/match3d/detect_wheel_ceres3d.h
+        测量节点/velodyne_lidar/match3d/hybrid_grid.hpp
+        测量节点/velodyne_lidar/match3d/interpolated_grid.hpp
+        测量节点/velodyne_lidar/velodyne_driver/angles.h
+        测量节点/velodyne_lidar/velodyne_driver/calibration.cc
+        测量节点/velodyne_lidar/velodyne_driver/calibration.h
+        测量节点/velodyne_lidar/velodyne_driver/input.cc
+        测量节点/velodyne_lidar/velodyne_driver/input.h
+        测量节点/velodyne_lidar/velodyne_driver/rawdata.cc
+        测量节点/velodyne_lidar/velodyne_driver/rawdata.h
+        测量节点/velodyne_lidar/velodyne_driver/velodyne_lidar_device.cpp
+        测量节点/velodyne_lidar/velodyne_driver/velodyne_lidar_device.h
+        测量节点/velodyne_lidar/velodyne_driver/velodyne_lidar_scan_task.cpp
+        测量节点/velodyne_lidar/velodyne_driver/velodyne_lidar_scan_task.h
+        测量节点/velodyne_lidar/car_pose_detector.cpp
+        测量节点/velodyne_lidar/car_pose_detector.h
+        测量节点/velodyne_lidar/chassis_ceres_solver.cpp
+        测量节点/velodyne_lidar/chassis_ceres_solver.h
+        测量节点/velodyne_lidar/ground_region.cpp
+        测量节点/velodyne_lidar/ground_region.h
+        测量节点/velodyne_lidar/velodyne_config.pb.cc
+        测量节点/velodyne_lidar/velodyne_config.pb.h
+        测量节点/velodyne_lidar/velodyne_manager.cpp
+        测量节点/velodyne_lidar/velodyne_manager.h
+        测量节点/velodyne_lidar/velodyne_manager_task.cpp
+        测量节点/velodyne_lidar/velodyne_manager_task.h
+        测量节点/verify/hardware_limit.pb.cc
+        测量节点/verify/hardware_limit.pb.h
+        测量节点/verify/Railing.cpp
+        测量节点/verify/Railing.h
+        测量节点/verify/Terminal_region_limit.cpp
+        测量节点/verify/Terminal_region_limit.h
+        测量节点/verify/Verify_result.cpp
+        测量节点/verify/Verify_result.h
+        测量节点/wanji_lidar/async_client.cpp
+        测量节点/wanji_lidar/async_client.h
+        测量节点/wanji_lidar/detect_wheel_ceres.cpp
+        测量节点/wanji_lidar/detect_wheel_ceres.h
+        测量节点/wanji_lidar/LogFiles.cpp
+        测量节点/wanji_lidar/LogFiles.h
+        测量节点/wanji_lidar/Point2D.cpp
+        测量节点/wanji_lidar/Point2D.h
+        测量节点/wanji_lidar/Point3D.cpp
+        测量节点/wanji_lidar/Point3D.h
+        测量节点/wanji_lidar/region_detect.cpp
+        测量节点/wanji_lidar/region_detect.h
+        测量节点/wanji_lidar/region_worker.cpp
+        测量节点/wanji_lidar/region_worker.h
+        测量节点/wanji_lidar/wanji_716N_device.cpp
+        测量节点/wanji_lidar/wanji_716N_device.h
+        测量节点/wanji_lidar/wanji_lidar_device.cpp
+        测量节点/wanji_lidar/wanji_lidar_device.h
+        测量节点/wanji_lidar/wanji_lidar_scan_task.cpp
+        测量节点/wanji_lidar/wanji_lidar_scan_task.h
+        测量节点/wanji_lidar/wanji_manager.cpp
+        测量节点/wanji_lidar/wanji_manager.h
+        测量节点/wanji_lidar/wanji_manager_task.cpp
+        测量节点/wanji_lidar/wanji_manager_task.h
+        测量节点/wanji_lidar/wj_716_lidar_protocol.cpp
+        测量节点/wanji_lidar/wj_716_lidar_protocol.h
+        测量节点/wanji_lidar/wj_716N_lidar_protocol.cpp
+        测量节点/wanji_lidar/wj_716N_lidar_protocol.h
+        测量节点/wanji_lidar/wj_lidar_conf.pb.cc
+        测量节点/wanji_lidar/wj_lidar_conf.pb.h
+        测量节点/main.cpp)

指令入队节点/node.py

@@ -39,7 +39,7 @@ def command_enqueue_callback(table):
     else:
         cmd=message.pick_table()
         tf.Parse(table,cmd)
-        response_port="pick_response_port"
+        response_port="pick_response_%d_port"%(cmd.terminal_id)
 
     if cmd.statu.execute_statu==message.eNormal:
         #指令入队正常

指令检查节点/CheckEntrance.py

@@ -70,6 +70,7 @@ class EntranceChecker(threading.Thread):
 
     def run(self):
         while self.is_close is False:
+            # print(self.error_str)
             time.sleep(0.05)
             #先判断连接状态
             if self.icpu_statu.timeout():

指令检查节点/node.py

@@ -75,7 +75,7 @@ def check_pick_command(body):
     else:
         print("取车指令 %s ERROR : 唯一码:%s    检查失败!" % (
             datetime.datetime.now(), cmd.primary_key))
-        response_port = "pick_response_port"
+        response_port = "pick_response_%d_port" % cmd.terminal_id
 
     g_rabbitmq.publish( ex_name, response_port,table)
     print("发送反馈端口:%s    反馈表单：%s"%(response_port,table))

测量节点/setting/velodyne_manager.prototxt

@@ -368,8 +368,8 @@ region
             # cx-1.9389+0.04 cy-0.01395+0.03
             cx:-1.89
             cy:0.016
-            cz:0.08
-            p:-1.5
+            cz:0.07
+            p:0.0
         }
     }
 }

测量节点/setting/wanji_manager.prototxt

@@ -495,7 +495,7 @@ regions
     # -1.8683
     plc_offsetx:-1.865
     plc_offsety:-6.11489
-    plc_offset_degree:-89.6
+    plc_offset_degree:-89.5
     plc_border_miny:-7.51
     car_min_width:1.55
     car_max_width:1.92
@@ -559,7 +559,7 @@ regions
     # -1.8683
     plc_offsetx:-1.865
     plc_offsety:-6.11489
-    plc_offset_degree:-89.6
+    plc_offset_degree:-89.5
     plc_border_miny:-7.51
     car_min_width:1.55
     car_max_width:1.92
@@ -610,7 +610,7 @@ regions
 regions
 {
 	minx:-1.42
-	maxx:1.5
+	maxx:1.37
 	miny:-2.6
 	maxy:2.3
 	minz:0.03
@@ -623,7 +623,7 @@ regions
     # -1.8683
     plc_offsetx:-1.865
     plc_offsety:-6.11489
-    plc_offset_degree:-89.6
+    plc_offset_degree:-89.1
     plc_border_miny:-7.51
     car_min_width:1.55
     car_max_width:1.92
@@ -637,6 +637,7 @@ regions
         lidar_id:4
         calib
         {
+            y:2.0 # 20221211 added by yct
             cx:-1.9348
             cy:-2.2882
         }
@@ -687,9 +688,9 @@ regions
     turnplate_cy:0.0
     border_minx:-1.3
     border_maxx:1.3
-    plc_offsetx:0.0
-    plc_offsety:0.0
-    plc_offset_degree:0.0
+    plc_offsetx:1.926
+    plc_offsety:-6.135
+    plc_offset_degree:-89.5
     plc_border_miny:-7.51
     car_min_width:1.55
     car_max_width:1.95
@@ -753,7 +754,7 @@ regions
     # -1.8683
     plc_offsetx:-1.865
     plc_offsety:-6.11489
-    plc_offset_degree:-89.6
+    plc_offset_degree:-89.5
     plc_border_miny:-7.51
     car_min_width:1.55
     car_max_width:1.92
@@ -817,7 +818,7 @@ regions
     # -1.8683
     plc_offsetx:-1.865
     plc_offsety:-6.11489
-    plc_offset_degree:-89.6
+    plc_offset_degree:-89.5
     plc_border_miny:-7.51
     car_min_width:1.55
     car_max_width:1.92

测量节点/system/system_executor.cpp

@@ -545,7 +545,7 @@ Error_manager System_executor::encapsulate_send_mq_status()
 		{
 			std::string t_filename = std::string("vlp16_region_") + std::to_string(iter->first) + "_cloud.txt";
 			save_cloud_txt(t_region_cloud, t_filename);
-			LOG(INFO) << "vlp16 region " << iter->first << " cloud has been saved in " + t_filename;
+			LOG(INFO) << " vlp16 region " << iter->first << " cloud has been saved in " + t_filename;
 		}
 		// for (size_t j = 0; j < t_region_cloud->size(); j++)
 		// {
@@ -667,14 +667,14 @@ Error_manager System_executor::encapsulate_send_mq_status()
 			std::string t_msg = t_multi_measure_info_msg.DebugString();
 			System_communication_mq::get_instance_references().encapsulate_status_msg(t_msg, t_multi_status_msg.id_struct().terminal_id());
 
-            if (t_multi_status_msg.id_struct().terminal_id() == 1)
-            {
-                LOG(WARNING) << "-------------------------------------------------------" <<std::endl;
-                LOG(WARNING) << "t_msg = " << t_msg <<std::endl;
-                LOG(WARNING) << "+++++++++++++++++++++++++++++++++++++++++++++++++++++++" <<std::endl;
-                LOG(WARNING) << "t_multi_status_msg.ground_status() = " << t_ground_status <<std::endl;
-                LOG(WARNING) << "t_multi_measure_info_msg.set_ground_status(0) = " << t_multi_measure_info_msg.ground_status() <<std::endl;
-            }
+            // if (t_multi_status_msg.id_struct().terminal_id() == 1)
+            // {
+            //     LOG(WARNING) << "-------------------------------------------------------" <<std::endl;
+            //     LOG(WARNING) << "t_msg = " << t_msg <<std::endl;
+            //     LOG(WARNING) << "+++++++++++++++++++++++++++++++++++++++++++++++++++++++" <<std::endl;
+            //     LOG(WARNING) << "t_multi_status_msg.ground_status() = " << t_ground_status <<std::endl;
+            //     LOG(WARNING) << "t_multi_measure_info_msg.set_ground_status(0) = " << t_multi_measure_info_msg.ground_status() <<std::endl;
+            // }
 		}
 		if (t_multi_status_msg.id_struct().terminal_id() == DISP_TERM_ID)
 		{
@@ -757,12 +757,12 @@ Error_manager System_executor::encapsulate_send_status()
 			// 获取区域点云填入信息
 			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);
-			if (cloud_count == 5)
-			{
-				std::string t_filename = std::string("region_") + std::to_string(iter->first) + "_cloud.txt";
-				save_cloud_txt(t_region_cloud, t_filename);
-				LOG(INFO) << "region " << iter->first << " cloud has been saved in " + t_filename;
-			}
+			// if (cloud_count == 5)
+			// {
+			// 	std::string t_filename = std::string("region_") + std::to_string(iter->first) + "_cloud.txt";
+			// 	save_cloud_txt(t_region_cloud, t_filename);
+			// 	LOG(INFO) << "region " << iter->first << " cloud has been saved in " + t_filename;
+			// }
 			// for (size_t j = 0; j < t_region_cloud->size(); j++)
 			// {
 			// 	message::Cloud_coordinate *tp_cloud = t_multi_status_msg.add_cloud();
@@ -824,12 +824,12 @@ Error_manager System_executor::encapsulate_send_status()
 			// 获取区域点云填入信息
 			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);
-			if (cloud_count == 5)
-			{
-				std::string t_filename = std::string("region_") + std::to_string(iter->first) + "_cloud.txt";
-				save_cloud_txt(t_region_cloud, t_filename);
-				LOG(INFO) << "region " << iter->first << " cloud has been saved in " + t_filename;
-			}
+			// if (cloud_count == 5)
+			// {
+			// 	std::string t_filename = std::string("region_") + std::to_string(iter->first) + "_cloud.txt";
+			// 	save_cloud_txt(t_region_cloud, t_filename);
+			// 	LOG(INFO) << "region " << iter->first << " cloud has been saved in " + t_filename;
+			// }
 			// for (size_t j = 0; j < t_region_cloud->size(); j++)
 			// {
 			// 	message::Cloud_coordinate *tp_cloud = t_multi_status_msg.add_cloud();
@@ -917,14 +917,16 @@ Error_manager System_executor::encapsulate_send_status()
 		t_error = (*iter).second->get_last_wheel_information(&t_car_wheel_information, std::chrono::system_clock::now());
 
 		// 获取区域点云填入信息
+		// 20221211 changed by yct, nanomsg dont save cloud
 		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);
-		if (cloud_count == 5)
-		{
-			std::string t_filename = std::string("vlp16_region_") + std::to_string(iter->first) + "_cloud.txt";
-			save_cloud_txt(t_region_cloud, t_filename);
-			LOG(INFO) << "vlp16 region " << iter->first << " cloud has been saved in " + t_filename;
-		}
+		// if (cloud_count == 5)
+		// {
+		// 	std::string t_filename = std::string("vlp16_region_") + std::to_string(iter->first) + "_cloud.txt";
+		// 	save_cloud_txt(t_region_cloud, t_filename);
+		// 	LOG(INFO) << "vlp16 region " << iter->first << " cloud has been saved in " + t_filename;
+		// }
+
 		// for (size_t j = 0; j < t_region_cloud->size(); j++)
 		// {
 		// 	message::Cloud_coordinate *tp_cloud = t_multi_status_msg.add_cloud();
@@ -962,12 +964,14 @@ Error_manager System_executor::encapsulate_send_status()
 			// 获取区域点云填入信息
 			pcl::PointCloud<pcl::PointXYZ>::Ptr t_wj_region_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
 			t_wj_region_iter->second->get_region_cloud(t_wj_region_cloud);
-			if (cloud_count == 8)
-			{
-				std::string t_filename = std::string("wj_region_") + std::to_string(t_wj_region_iter->first) + "_cloud.txt";
-				save_cloud_txt(t_wj_region_cloud, t_filename);
-				LOG(INFO) << "wj region " << t_wj_region_iter->first << " cloud has been saved in " + t_filename;
-			}
+
+			// if (cloud_count == 8)
+			// {
+			// 	std::string t_filename = std::string("wj_region_") + std::to_string(t_wj_region_iter->first) + "_cloud.txt";
+			// 	save_cloud_txt(t_wj_region_cloud, t_filename);
+			// 	LOG(INFO) << "wj region " << t_wj_region_iter->first << " cloud has been saved in " + t_filename;
+			// }
+
 			// for (size_t j = 0; j < t_wj_region_cloud->size(); j++)
 			// {
 			// 	message::Cloud_coordinate *tp_cloud = t_multi_status_msg.add_cloud();
@@ -991,7 +995,9 @@ Error_manager System_executor::encapsulate_send_status()
 			}else
 			{
 				LOG_IF(WARNING, iter->second->get_terminal_id() == DISP_TERM_ID)<<
-					"detect mismatch, region "<<iter->first<<", dx dy dang dwb: "<<dx<<", "<<dy<<", "<<dang<<", "<< dwb;
+					"detect mismatch, region "<<iter->first<<", dx dy dang dwb: "<<dx<<", "<<dy<<", "<<dang<<", "<< dwb << std::endl 
+					<< t_car_wheel_information.to_string() << std::endl
+					<< t_wj_car_wheel_information.to_string() << std::endl;
 			}
 
 		}else{

测量节点/system/system_executor.h

@@ -20,7 +20,7 @@
 #include "../wanji_lidar/wanji_manager.h"
 #include "../velodyne_lidar/velodyne_manager.h"
 
-#define DISP_TERM_ID -1
+#define DISP_TERM_ID 2
 // 0wj  1velo	2both
 #define WJ_VELO 2
 

测量节点/velodyne_lidar/ground_region.cpp

@@ -470,12 +470,14 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     Error_manager ec = t_solver.solve_mat(cloud_z_solver, z_solver_x, mid_z, z_solver_width, height, false);
     // 切除大于height高度以外点，并显示width直线
     // 根据z值切原始点云
+    // std::cout<<"before clip: "<<cloud_z_solver->size()<<std::endl;
     pcl::PassThrough<pcl::PointXYZ> pass;
     pass.setInputCloud(cloud_z_solver);
     pass.setFilterFieldName("z");
     pass.setFilterLimits(m_region.minz(), mid_z + height / 2.0);
     pass.setFilterLimitsNegative(false);
     pass.filter(*cloud_z_solver);
+    // std::cout<<"after clip: "<<cloud_z_solver->size()<<std::endl;
     // // 车宽方向画线
     // for (double i = -3.0; i < 3.0; i+=0.02)
     // {
@@ -566,8 +568,8 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     //        center_z, last_result.theta, last_result.front_theta, last_result.wheel_base, last_result.width,
     //        min_mean_loss);
     // std::cout << "\n-------- final z: " << chassis_z << std::endl;
-    // std::cout << "cx: " << last_result.cx << ", cy: " << last_result.cy << ", theta: " << last_result.theta
-    //           << ", front: " << last_result.front_theta << ", wheelbase: " << last_result.wheel_base << ", width: " << last_result.width << std::endl << std::endl;
+    std::cout << "cx: " << last_result.cx << ", cy: " << last_result.cy << ", theta: " << last_result.theta
+              << ", front: " << last_result.front_theta << ", wheelbase: " << last_result.wheel_base << ", width: " << last_result.width << std::endl << std::endl;
 
     // last_result.cx -= x;
     // last_result.cy -= y;
@@ -585,7 +587,10 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     // 车宽精度优化
     {
         pcl::PointCloud<pcl::PointXYZ>::Ptr t_width_extract_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
-        t_width_extract_cloud->operator+=(*mp_cloud_filtered);
+        t_width_extract_cloud->operator+=(m_detector->m_left_front_cloud);
+        t_width_extract_cloud->operator+=(m_detector->m_right_front_cloud);
+        t_width_extract_cloud->operator+=(m_detector->m_left_rear_cloud);
+        t_width_extract_cloud->operator+=(m_detector->m_right_rear_cloud);
 
         //离群点过滤
         sor.setInputCloud(t_width_extract_cloud);
@@ -627,7 +632,7 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     return SUCCESS;
 }
 
-int Ground_region::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double plate_cx, double plate_cy, double theta)
+int Ground_region::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double cx, double cy, double plate_cx, double plate_cy, double theta)
 {
     // LOG(WARNING) << m_range_status_last;
     if(cloud->size() <= 0)
@@ -683,7 +688,7 @@ int Ground_region::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
     //huli 不再使用汽车左右边界来判断，直接使用中心点x值判断左右超界。
     //plc坐标，1 3 5入口x为-1.8左右， 2 4 6入口为1.8左右
     //要求x取值范围，1650～2150左右
-    float t_center_x = (min_p.x + max_p.x) / 2 + m_region.plc_offsetx();
+    float t_center_x = cx + m_region.plc_offsetx();
     if(t_center_x < m_region.border_minx()-0.000)
     {
         m_range_status_last |= Range_status::Range_left;
@@ -874,7 +879,7 @@ int Ground_region::outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
     //else      m_range_status_last no change
 
 
-    m_range_status_last |= outOfRangeDetection(cloud, plate_cx, plate_cy, theta);
+    m_range_status_last |= outOfRangeDetection(cloud, measure_result.car_center_x, measure_result.car_center_y, plate_cx, plate_cy, theta);
     return m_range_status_last;
 #endif
 }

测量节点/velodyne_lidar/ground_region.h

@@ -116,6 +116,7 @@ public:
       case Region_cloud_type::detect_z:
          m_detect_z_cloud_mutex.lock();
          out_cloud->operator+=(*mp_cloud_detect_z);
+         // std::cout<<"get cloud: "<<out_cloud->size()<<std::endl;
          m_detect_z_cloud_mutex.unlock();
          break;
       }
@@ -160,7 +161,7 @@ private:
 
    // 超界判断，旋转单位(deg)，默认无旋转
    // 主要判断左右超界
-   int outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double plate_cx=0.0, double plate_cy=0.0, double theta=0.0);
+   int outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double cx, double cy, double plate_cx=0.0, double plate_cy=0.0, double theta=0.0);
 
    // 超界判断，增加对后轮的判断，对轴距、车宽、角度超界的判断
    int outOfRangeDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Common_data::Car_wheel_information measure_result, double plate_cx=0.0, double plate_cy=0.0, double theta=0.0);