7-17 现场 2,4号位增高，车高计算输入由车身点云改为总点云保证安全。将之前与多线对比用到的库文件加入代码跟踪。

locate/locater.cpp

@@ -345,8 +345,9 @@ Error_manager Locater::locate(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_src,
     }
 
     //第三步,计算车辆高度
+    // changed by yct, 传入完整点云而非仅车身点云
     float height_car=0;
-    code=measure_height(cloud_car,height_car);
+    code=measure_height(cloud_in,height_car);
     if(code!=SUCCESS)
     {
         return code;

terminor/terminal_command_executor.cpp

@@ -539,10 +539,19 @@ Error_manager Terminor_Command_Executor::scanning_measuring() {
     m_measure_information.locate_hight = dj_locate_information.locate_hight;
     // 20210527 added by yct
     // 6号位标定地面偏低，高度增加21mm
+    // 20210603 2号位加高10mm, 4号位加高5mm
     if(m_terminor_parameter.id()==5)
     {
         m_measure_information.locate_hight += 21;
     }
+    else if(m_terminor_parameter.id()==3)
+    {
+        m_measure_information.locate_hight += 5;
+    }
+    else if(m_terminor_parameter.id()==1)
+    {
+        m_measure_information.locate_hight += 10;
+    }
     m_measure_information.locate_correct = true;
 
     // 20210527 added by yct, 融合大疆轮子与万集点云进行结果校验,机械手内空2150，取2000计算
@@ -579,7 +588,7 @@ Error_manager Terminor_Command_Executor::scanning_measuring() {
     save_cloud_txt(project_path+"/outside.txt", t_outside_cloud);
     LOG(INFO) << "outside point count: "<<outside_point_count;
     // 安全判断，外点超过15个判断本次测量失败
-    if(outside_point_count > 15)
+    if((outside_point_count > 15 && m_terminor_parameter.id()!=1) || (m_terminor_parameter.id()==1 && outside_point_count > 25))
     {
         LOG(WARNING) << "机械手下降判断外点过多，存在风险: "<<outside_point_count;
         m_measure_information.locate_correct = false;

tool/common_data.cpp

@@ -0,0 +1,41 @@
+//
+// Created by huli on 2020/9/8.
+//
+float center_x;				//整车的中心点x值, 四轮的中心
+float center_y;				//整车的中心点y值, 四轮的中心
+float car_angle;			//整车的车身旋转角,
+float car_length;			//整车的长度, 用于规避碰撞
+float car_width;			//整车的宽度, 用于规避碰撞
+float car_height;			//整车的高度, 用于规避碰撞
+float wheel_base;			//整车的轮距, 前后轮的距离, 用于机器人或agv的抓车
+float wheel_width;			//整车的轮距, 左右轮的距离, 用于机器人或agv的抓车
+float front_theta;			//整车的前轮的旋转角
+
+bool correctness;			//整车的校准标记位
+
+#include "common_data.h"
+
+void Common_data::copy_data(Car_measure_information& car_measure_information_in, Car_wheel_information& car_wheel_information_out)
+{
+	car_wheel_information_out.center_x = car_measure_information_in.center_x;
+	car_wheel_information_out.center_y = car_measure_information_in.center_y;
+	car_wheel_information_out.car_angle = car_measure_information_in.car_angle;
+
+	car_wheel_information_out.wheel_base = car_measure_information_in.wheel_base;
+	car_wheel_information_out.wheel_width = car_measure_information_in.wheel_width;
+	car_wheel_information_out.front_theta = car_measure_information_in.front_theta;
+	car_wheel_information_out.correctness = car_measure_information_in.correctness;
+}
+void Common_data::copy_data(Car_wheel_information& car_wheel_information_in, Car_measure_information& car_measure_information_out)
+{
+	car_measure_information_out.center_x = car_wheel_information_in.center_x;
+	car_measure_information_out.center_y = car_wheel_information_in.center_y;
+	car_measure_information_out.car_angle = car_wheel_information_in.car_angle;
+	car_measure_information_out.car_length = 0;
+	car_measure_information_out.car_width = 0;
+	car_measure_information_out.car_height = 0;
+	car_measure_information_out.wheel_base = car_wheel_information_in.wheel_base;
+	car_measure_information_out.wheel_width = car_wheel_information_in.wheel_width;
+	car_measure_information_out.front_theta = car_wheel_information_in.front_theta;
+	car_measure_information_out.correctness = car_wheel_information_in.correctness;
+}

tool/common_data.h

@@ -0,0 +1,149 @@
+//
+// Created by huli on 2020/9/8.
+//
+
+#ifndef NNXX_TESTS_COMMON_DATA_H
+#define NNXX_TESTS_COMMON_DATA_H
+#include <chrono>
+#include <cmath>
+#include <string>
+class Common_data
+{
+public:
+	//万集雷达扫描周期66ms, (频率15hz), 一般设置大一些
+#define WANJI_716_SCAN_CYCLE_MS 		75
+
+
+	//整车的测量信息
+	struct Car_measure_information
+	{
+		float center_x = 0;				//整车的中心点x值, 四轮的中心
+		float center_y = 0;				//整车的中心点y值, 四轮的中心
+		float car_angle = 0;			//整车的车身旋转角,
+		float car_length = 0;			//整车的长度, 用于规避碰撞
+		float car_width = 0;			//整车的宽度, 用于规避碰撞
+		float car_height = 0;			//整车的高度, 用于规避碰撞
+		float wheel_base = 0;			//整车的轮距, 前后轮的距离, 用于机器人或agv的抓车
+		float wheel_width = 0;			//整车的轮距, 左右轮的距离, 用于机器人或agv的抓车
+		float front_theta = 0;			//整车的前轮的旋转角
+
+		bool correctness = false;			//整车的校准标记位
+
+	};
+
+	//四轮的测量信息
+	struct Car_wheel_information
+	{
+		float center_x = 0;				//整车的中心点x值, 四轮的中心
+		float center_y = 0;				//整车的中心点y值, 四轮的中心
+		float car_angle = 0;			//整车的车身旋转角,
+
+		float wheel_base = 0;			//整车的轮距, 前后轮的距离, 用于机器人或agv的抓车
+		float wheel_width = 0;			//整车的轮距, 左右轮的距离, 用于机器人或agv的抓车
+		float front_theta = 0;			//整车的前轮的旋转角
+
+		bool correctness = false;			//整车的校准标记位
+		public:
+		Car_wheel_information& operator+=(const Car_wheel_information& other)
+		{
+			this->center_x += other.center_x;
+			this->center_y += other.center_y;
+			this->car_angle += other.car_angle;
+			this->wheel_base += other.wheel_base;
+			this->wheel_width += other.wheel_width;
+			this->front_theta += other.front_theta;
+			this->correctness &= other.correctness;
+			return *this;
+		}
+		Car_wheel_information& operator-=(const Car_wheel_information& other)
+		{
+			this->center_x -= other.center_x;
+			this->center_y -= other.center_y;
+			this->car_angle -= other.car_angle;
+			this->wheel_base -= other.wheel_base;
+			this->wheel_width -= other.wheel_width;
+			this->front_theta -= other.front_theta;
+			this->correctness &= other.correctness;
+			return *this;
+		}
+		Car_wheel_information& operator/=(int scalar)
+		{
+			if(scalar==0)
+				return *this;
+			this->center_x /= scalar;
+			this->center_y /= scalar;
+			this->car_angle /= scalar;
+			this->wheel_base /= scalar;
+			this->wheel_width /= scalar;
+			this->front_theta /= scalar;
+			return *this;
+		}
+		// 定义评分规则, 
+		// wx=1 wy=1 wa=0.5 wb=1 ww=0.5 wft=0.5
+		float calc_score()
+		{
+			float weights[] = {1.0f, 1.0f, 0.5f, 1.0f, 0.5f, 0.5f};
+			float final_score = 0.0f;
+			final_score += fabs(weights[0] * this->center_x);
+			final_score += fabs(weights[1] * this->center_y);
+			final_score += fabs(weights[2] * this->car_angle);
+			final_score += fabs(weights[3] * this->wheel_base);
+			final_score += fabs(weights[4] * this->wheel_width);
+			final_score += fabs(weights[5] * this->front_theta);
+
+			return final_score;
+		}
+
+		std::string to_string()
+        {
+            char buf[512]={0};
+            sprintf(buf, "%.4f %.4f %.4f %.4f %.4f %.4f\n", center_x, center_y, car_angle, wheel_base, wheel_width, front_theta);
+            return std::string(buf);
+        }
+	};
+
+	// 带时间戳的四轮测量信息
+	struct Car_wheel_information_stamped
+	{
+		Car_wheel_information wheel_data;
+		std::chrono::system_clock::time_point measure_time;
+				public:
+		Car_wheel_information_stamped& operator+=(const Car_wheel_information_stamped& other)
+		{
+			this->wheel_data += other.wheel_data;
+			return *this;
+		}
+		Car_wheel_information_stamped& operator-=(const Car_wheel_information_stamped& other)
+		{
+			this->wheel_data -= other.wheel_data;
+			return *this;
+		}
+		Car_wheel_information_stamped operator-(const Car_wheel_information_stamped& other)
+		{
+            Car_wheel_information_stamped t_info;
+            t_info = *this;
+            t_info.wheel_data -= other.wheel_data;
+			return t_info;
+		}
+		Car_wheel_information_stamped& operator/=(int scalar)
+		{
+			this->wheel_data /= scalar;
+			return *this;
+		}
+		// 定义评分规则, 
+		// wx=1 wy=1 wa=0.5 wb=1 ww=0.5 wft=0.1
+		float calc_score()
+		{
+			return wheel_data.calc_score();
+		}
+	};
+
+	static void copy_data(Car_measure_information& car_measure_information_in, Car_wheel_information& car_wheel_information_out);
+	static void copy_data(Car_wheel_information& car_wheel_information_in, Car_measure_information& car_measure_information_out);
+
+
+
+};
+
+
+#endif //NNXX_TESTS_COMMON_DATA_H

tool/singleton.cpp

@@ -0,0 +1,4 @@
+
+#include "singleton.h"
+
+

tool/singleton.h

@@ -0,0 +1,81 @@
+
+/* Singleton 是单例类的模板。
+ * https://www.cnblogs.com/sunchaothu/p/10389842.html
+ * 单例 Singleton 是设计模式的一种，其特点是只提供唯一一个类的实例,具有全局变量的特点，在任何位置都可以通过接口获取到那个唯一实例;
+ * 全局只有一个实例：static 特性，同时禁止用户自己声明并定义实例（把构造函数设为 private 或者 protected）
+ * Singleton 模板类对这种方法进行了一层封装。
+ * 单例类需要从Singleton继承。
+ * 子类需要将自己作为模板参数T 传递给 Singleton<T> 模板;
+ * 同时需要将基类声明为友元，这样Singleton才能调用子类的私有构造函数。
+// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+// 父类的构造函数必须保护，子类的构造函数必须私有。
+// 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来操作 唯一的实例。
+ * */
+
+#ifndef __SINGLIETON_H
+#define __SINGLIETON_H
+
+//#include <iostream>
+
+template<typename T>
+class Singleton
+{
+public:
+	//获取单例的引用
+	static T& get_instance_references()
+	{
+		static T instance;
+		return instance;
+	}
+	//获取单例的指针
+	static T* get_instance_pointer()
+	{
+		return &(get_instance_references());
+	}
+
+	virtual ~Singleton()
+	{
+//		std::cout<<"destructor called!"<<std::endl;
+	}
+
+	Singleton(const Singleton&)=delete;					//关闭拷贝构造函数
+	Singleton& operator =(const Singleton&)=delete;		//关闭赋值函数
+
+protected:
+	//构造函数需要是 protected，这样子类才能继承；
+	Singleton()
+	{
+//		std::cout<<"constructor called!"<<std::endl;
+	}
+
+};
+
+/*
+// 如下是 使用样例：
+// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+// 父类的构造函数必须保护，子类的构造函数必须私有。
+// 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+
+class DerivedSingle:public Singleton<DerivedSingle>
+{
+ // 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+	friend class Singleton<DerivedSingle>;
+public:
+ // 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+	DerivedSingle(const DerivedSingle&)=delete;
+	DerivedSingle& operator =(const DerivedSingle&)= delete;
+ 	~DerivedSingle()=default;
+private:
+ // 父类的构造函数必须保护，子类的构造函数必须私有。
+	DerivedSingle()=default;
+};
+
+int main(int argc, char* argv[]){
+	DerivedSingle& instance1 = DerivedSingle::get_instance_references();
+	DerivedSingle* p_instance2 = DerivedSingle::get_instance_pointer();
+	return 0;
+}
+
+ */
+
+#endif

wj_lidar/fence_controller.cpp

@@ -319,6 +319,7 @@ Error_manager Fence_controller::execute_task(Task_Base* task)
 
                 pcl::PointCloud<pcl::PointXYZ>::Ptr total_cloud(new pcl::PointCloud<pcl::PointXYZ>);
                 bool has_few_cloud = false;
+                std::string t_cloud_info_str=" ";
                 for (int i = 0; i < m_lidars_vector.size(); ++i)
                 {
                     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -331,6 +332,8 @@ Error_manager Fence_controller::execute_task(Task_Base* task)
                         int t_laser_id = std::stoi(t_laser_ip.substr(t_laser_ip.size()-2));
                         if(t_laser_id == terminal_id+1 || t_laser_id == terminal_id+2||t_laser_id == terminal_id+8||t_laser_id == terminal_id+9)
                         {
+                            t_cloud_info_str.append(" ");
+                            t_cloud_info_str.append(std::to_string(t_laser_id)+"->"+std::to_string(cloud->size()));
                             has_few_cloud |= cloud->size() <= 5;
                         }
                     }
@@ -390,9 +393,9 @@ Error_manager Fence_controller::execute_task(Task_Base* task)
                     }
                     task_temp->set_result(wj_measure_temp);
                     task_temp->set_cloud(*total_cloud);
-                    char description[480] = {0};
-                    sprintf(description, "WJ center(%.1f, %.1f) size:(%.1f, %.1f)  angle:%.2f %s",
-                            x * 1000.0, y * 1000.0, wheelbase * 1000.0, width * 1000.0, c, has_few_cloud?"存在空点云":"雷达正常");
+                    char description[512] = {0};
+                    sprintf(description, "WJ center(%.1f, %.1f) size:(%.1f, %.1f)  angle:%.2f %s%s",
+                            x * 1000.0, y * 1000.0, wheelbase * 1000.0, width * 1000.0, c, has_few_cloud?"存在空点云 ":"雷达正常 ", t_cloud_info_str.c_str());
                     LOG(INFO) << description;
                     return Error_manager(ec);
                 }

wj_lidar/region_worker.cpp

@@ -312,6 +312,7 @@ void Region_worker::detect_loop(Region_worker *worker)
                 t_loc_info.set_locate_front_theta(front_theta);
                 t_loc_info.set_locate_correct(true);
                 t_ground_result_msg.mutable_locate_information_realtime()->CopyFrom(t_loc_info);
+//                LOG(WARNING)<<"发送： "<<t_ground_result_msg.DebugString();
                 worker->m_sock.send(t_ground_result_msg.SerializeAsString());
             }
         }

wj_lidar/region_worker.h

@@ -36,7 +36,7 @@ public:
 #define REGION_WORKER_HAS_CAR 2
 #define REGION_WORKER_DETECT_ERROR 3
 
-#define WJ_RESULT_PUB_STR "tcp://127.0.0.1:190"
+#define WJ_RESULT_PUB_STR "tcp://192.168.2.124:190"
 
 
 public:

wj_lidar/wj_716_lidar_protocol.cpp

@@ -392,7 +392,7 @@ Error_manager Wj_716_lidar_protocol::protocol(const char *data, const int len)
           point.y = 0;
           point.z = 0;
 	  //LOG(INFO)<<"indensity: "<<scaninden[j];
-	  if(scaninden[j] <49.0f || scaninden[j] > 1500.0f)
+	  if(scaninden[j] <45.0f || scaninden[j] > 1500.0f)
 	  {
           	point.x = INT_MAX;
                 //LOG(INFO)<<"limited indensity: "<<scaninden[j];