2021.8.6 fix bug of faulty initialization of lidar device from wrong param, which caused timing problem.

velodyne_lidar/ground_region.cpp

@@ -434,13 +434,15 @@ Error_manager Ground_region::get_last_wheel_information(Common_data::Car_wheel_i
 	    					"  POINTER IS NULL ");
 	}
 	//获取指令时间之后的信息, 如果没有就会报错, 不会等待
+//    LOG(WARNING) << std::chrono::duration_cast<std::chrono::milliseconds>(command_time-m_detect_update_time).count()/1000.0 <<", "
+//                <<std::chrono::duration_cast<std::chrono::milliseconds>(command_time-m_cloud_collection_time).count()/1000.0;
 	if( m_detect_update_time > command_time - std::chrono::milliseconds(GROUND_REGION_DETECT_CYCLE_MS))
 	{
 		*p_car_wheel_information = m_car_wheel_information;
         if(m_car_wheel_information.correctness)
             return Error_code::SUCCESS;
         else
-            return Error_manager(Error_code::VELODYNE_REGION_CERES_SOLVE_ERROR, Error_level::MINOR_ERROR, " Ground_region detect error ");
+            return Error_manager(Error_code::VELODYNE_REGION_CERES_SOLVE_ERROR, Error_level::MINOR_ERROR, " Ground_region detect error");
 	}
 	else
 	{
@@ -467,8 +469,10 @@ void Ground_region::thread_measure_func()
         {
             m_region_status = E_BUSY;
             detect_wheel_ceres3d::Detect_result t_result;
+
             Error_manager ec = detect(mp_cloud_collection, t_result);
             m_detect_update_time = std::chrono::system_clock::now();
+
             m_car_wheel_information.center_x = t_result.cx;
             m_car_wheel_information.center_y = t_result.cy;
             m_car_wheel_information.car_angle = t_result.theta;

velodyne_lidar/ground_region.h

@@ -32,7 +32,7 @@ public:
 
 		E_FAULT = 10, //故障
 	};
-   #define GROUND_REGION_DETECT_CYCLE_MS 200
+   #define GROUND_REGION_DETECT_CYCLE_MS 150
 
    Ground_region();
    ~Ground_region();

velodyne_lidar/velodyne_driver/rawdata.cc

@@ -252,7 +252,10 @@ namespace velodyne_rawdata
     config_.min_angle = min_angle*100;
     config_.max_angle = max_angle*100;
 
-    buildTimings();
+    if(!buildTimings())
+    {
+        return false;
+    }
 
     // // get path to angles.config file for this device
     // if (!private_nh.getParam("calibration", config_.calibrationFile))

velodyne_lidar/velodyne_driver/velodyne_lidar_device.cpp

@@ -95,7 +95,7 @@ Error_manager Velodyne_lidar_device::init(velodyne::velodyneLidarParams params)
 	m_communication_data_queue.wake_queue();
 
 	//初始化通信协议
-	t_error = m_protocol.setup(params.ip(), params.calibrationfile(), params.max_range(), params.min_range(), params.min_angle(), params.max_angle()) == true ? SUCCESS : Error_manager(PARAMETER_ERROR, MINOR_ERROR, "protocol parameter error");
+	t_error = m_protocol.setup(params.model(), params.calibrationfile(), params.max_range(), params.min_range(), params.min_angle(), params.max_angle()) == true ? SUCCESS : Error_manager(PARAMETER_ERROR, MINOR_ERROR, "protocol parameter error");
 	if (t_error != SUCCESS)
 	{
 		return t_error;
@@ -417,7 +417,7 @@ Error_manager Velodyne_lidar_device::get_last_cloud(std::mutex *p_mutex, pcl::Po
 		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
 							 "  POINTER IS NULL ");
 	}
-	if (m_parse_ring_time > command_time - std::chrono::milliseconds(m_params.rpm()/60))
+	if (m_parse_ring_time > command_time - std::chrono::milliseconds(int(60000.0/m_params.rpm())))
 	{
 		std::unique_lock<std::mutex> lck(*p_mutex);
 		{
@@ -448,7 +448,7 @@ Error_manager Velodyne_lidar_device::get_last_cloud(pcl::PointCloud<pcl::PointXY
 		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
 							 "  POINTER IS NULL ");
 	}
-	if (m_parse_ring_time > command_time - std::chrono::milliseconds(m_params.rpm()/60))
+	if (m_parse_ring_time > command_time - std::chrono::milliseconds(int(60000.0/m_params.rpm())))
 	{
 		std::lock_guard<std::mutex> lck(m_cloud_mutex);
 		if (m_ring_cloud.size() <= 0)