commit

aarch64/pjisuv/common/config/yaml/pjisuv-008-cloud-config.yaml

@@ -0,0 +1,135 @@
+---
+monitor:
+  url: http://36.110.106.142:12341/web_server/monitor/insert
+platform:
+  url-device-auth: http://1.202.169.139:8081/device/auth
+  url-task-poll: http://1.202.169.139:8081/device/task/poll
+  url-task: http://1.202.169.139:8081/device/task
+bag-number: 120
+config-refresh-interval: 60
+disk:
+  name: /dev/vdb # 磁盘名称
+  used: 900000000000 # 磁盘占用阈值，单位bytes
+#bag-data-dir: /userdata/cicv-data-closedloop/data/
+#bag-copy-dir: /userdata/cicv-data-closedloop/copy/
+#time-to-label-json-path: /userdata/cicv-data-closedloop/timeToLabel.json
+#triggers-dir: /userdata/cicv-data-closedloop/triggers/
+bag-data-dir: /mnt/media/sda1/cicv-data-closedloop/data/
+bag-copy-dir: /mnt/media/sda1/cicv-data-closedloop/copy/
+time-to-label-json-path: /mnt/media/sda1/cicv-data-closedloop/timeToLabel.json
+triggers-dir: /mnt/media/sda1/cicv-data-closedloop/triggers/
+time-window-send-gap: 6
+tcp-port: 12340
+rpc-port: 12341
+ros:
+  master-address: 192.168.1.102:11311
+  nodes:
+    - /camera_output
+    - /ins
+    - /lidar_pretreatment
+    - /sensorfusion
+    - /control
+hosts:
+  - name: node1
+    ip: 192.168.1.102
+    rosbag:
+      path: "/opt/ros/melodic/bin/rosbag"
+      envs:
+        - "LD_LIBRARY_PATH=/opt/ros/melodic/lib:/usr/lib::/usr/lib:/userdata/third_lib/libaicc"
+        - "ROS_ETC_DIR=/opt/ros/melodic/etc/ros"
+        - "USER=root"
+        - "ROS_OS_OVERRIDE=openembedded"
+        - "PWD=/mnt/media/sda1/cicv-data-closedloop"
+        - "HOME=/home/root"
+        - "CMAKE_PREFIX_PATH=/opt/ros/melodic:/usr"
+        - "ROS_ROOT=/opt/ros/melodic/share/ros"
+        - "ROS_MASTER_URI=http://192.168.1.102:11311"
+        - "ROS_VERSION=1"
+        - "ROS_PYTHON_VERSION=2"
+        - "ROS_IP=192.168.1.102"
+        - "PYTHONPATH=/opt/ros/melodic/lib/python2.7/site-packages:/usr/lib/python2.7/site-packages"
+        - "ROS_PACKAGE_PATH=/opt/ros/melodic/share"
+        - "PATH=/opt/ros/melodic/bin:/usr/bin/cyber/tools/cyber_service:/usr/bin/cyber/tools/cyber_node:/usr/bin/cyber/tools/cyber_channel:/usr/bin/cyber/tools/cyber_launch:/usr/bin/cyber/tools/cyber_monitor:/usr/bin/cyber/tools/cyber_recorder:/apollo/bazel-bin/cyber:/usr/local/bin:/usr/bin:/bin:/usr/local/sbin:/usr/sbin:/sbin"
+        - "PKG_CONFIG_PATH=/usr/lib/pkgconfig"
+        - "ROS_DISTRO=melodic"
+    topics:
+      - /camera_image # /camera_output
+      - /cicv_amr_trajectory # /planner_s4 换成了 /planner_s4_gd
+      - /pj_control_pub # /control
+      - /data_read
+  - name: node2
+    ip: 192.168.1.103
+    rosbag:
+      path: "/opt/ros/melodic/bin/rosbag"
+      envs:
+        - "LD_LIBRARY_PATH=/opt/ros/melodic/lib:/usr/lib::/usr/lib:/userdata/third_lib"
+        - "ROS_ETC_DIR=/opt/ros/melodic/etc/ros"
+        - "USER=root"
+        - "ROS_OS_OVERRIDE=openembedded"
+        - "PWD=/mnt/media/sda1/cicv-data-closedloop"
+        - "HOME=/home/root"
+        - "CMAKE_PREFIX_PATH=/opt/ros/melodic:/usr"
+        - "ROS_ROOT=/opt/ros/melodic/share/ros"
+        - "ROS_MASTER_URI=http://192.168.1.102:11311"
+        - "ROS_VERSION=1"
+        - "ROS_PYTHON_VERSION=2"
+        - "ROS_IP=192.168.1.103"
+        - "PYTHONPATH=/opt/ros/melodic/lib/python2.7/site-packages:/usr/lib/python2.7/site-packages"
+        - "ROS_PACKAGE_PATH=/opt/ros/melodic/share"
+        - "PATH=/opt/ros/melodic/bin:/usr/bin/cyber/tools/cyber_service:/usr/bin/cyber/tools/cyber_node:/usr/bin/cyber/tools/cyber_channel:/usr/bin/cyber/tools/cyber_launch:/usr/bin/cyber/tools/cyber_monitor:/usr/bin/cyber/tools/cyber_recorder:/apollo/bazel-bin/cyber:/usr/local/bin:/usr/bin:/bin:/usr/local/sbin:/usr/sbin:/sbin "
+        - "PKG_CONFIG_PATH=/usr/lib/pkgconfig"
+        - "ROS_DISTRO=melodic"
+    topics:
+      - /points_concat  # /lidar_pretreatment
+      - /tpperception # /sensorfusion
+      - /cicv_location  # /ins
+full-collect: true  #如果不是全量采集需要根据triggers过滤
+triggers:
+  - label: rapidaccel 
+    topics:
+      - /tpperception
+      - /points_concat
+      - /cicv_location
+      - /camera_image
+  - label: brake
+    topics:
+      - /tpperception
+      - /points_concat
+      - /cicv_location
+      - /camera_image
+  - label: EmergencyStop
+    topics:
+      - /tpperception
+      - /points_concat
+      - /cicv_location
+      - /camera_image
+  - label: AutoDLimit
+    topics:
+      - /tpperception
+      - /points_concat
+      - /cicv_location
+      - /camera_image
+  - label: lanechange
+    topics:
+      - /tpperception
+      - /points_concat
+      - /cicv_location
+      - /camera_image
+  - label: brakefault
+    topics:
+      - /tpperception
+      - /points_concat
+      - /cicv_location
+      - /camera_image
+  - label: takeover
+    topics:
+      - /tpperception
+      - /points_concat
+      - /cicv_location
+      - /camera_image
+  - label: TTC
+    topics:
+      - /tpperception
+      - /points_concat
+      - /cicv_location
+      - /camera_image

aarch64/pjisuv/common/config/yaml/pjisuv-008-soc1-local-config.yaml

@@ -0,0 +1,19 @@
+node:
+  name: node1
+  ip: 192.168.1.102
+# 数据闭环平台参数
+equipment-no: pjisuv-008
+secret-key: uvye8r2dfh7lwwl8
+# 获取oss连接信息的接口url
+url-get-oss-config: http://36.110.106.156:18379/oss/config?token=nXonLUcMtGcrQqqKiyygIwyVbvizE0wD
+# 金龙车数据前缀
+oss-base-prefix: pjisuv/
+# oss上的配置文件的名称
+cloud-config-filename: cloud-config.yaml
+# 将oss上的配置文件下载到本地的路径
+cloud-config-local-path: /mnt/media/sda1/cicv-data-closedloop/config/cloud-config.yaml
+restart-cmd:
+  dir: "/mnt/media/sda1/cicv-data-closedloop/"
+  name: "sh"
+  args:
+    - "start-soc1.sh"

aarch64/pjisuv/common/config/yaml/pjisuv-008-soc2-local-config.yaml

@@ -0,0 +1,19 @@
+node:
+  name: node2
+  ip: 192.168.1.103
+# 数据闭环平台参数
+equipment-no: pjisuv-008
+secret-key: uvye8r2dfh7lwwl8
+# 获取oss连接信息的接口url
+url-get-oss-config: http://36.110.106.156:18379/oss/config?token=nXonLUcMtGcrQqqKiyygIwyVbvizE0wD
+# 金龙车数据前缀
+oss-base-prefix: pjisuv/
+# oss上的配置文件的名称
+cloud-config-filename: cloud-config.yaml
+# 将oss上的配置文件下载到本地的路径
+cloud-config-local-path: /mnt/media/sda1/cicv-data-closedloop/config/cloud-config.yaml
+restart-cmd:
+  dir: "/mnt/media/sda1/cicv-data-closedloop/"
+  name: "sh"
+  args:
+    - "start-soc2.sh"

aarch64/pjisuv/master/config/master_trigger_config.go

@@ -173,6 +173,10 @@ var (
 	TopicOfFaultInfo = "/fault_info"
 	//RuleOfFaultInfo  []func(data *pjisuv_msgs.FaultVec) string
 
+	//39
+	TopicOfPjVehicleFdbPub = "/pj_vehicle_fdb_pub"
+	RuleOfPjVehicleFdbPub  []func(data *pjisuv_msgs.VehicleFdb, param entity.PjisuvParam) string
+
 	LabelMapTriggerId sync.Map
 )
 
@@ -472,6 +476,13 @@ func InitTriggerConfig() {
 				continue
 			}
 			RuleOfDataRead = append(RuleOfDataRead, f)
+		} else if TopicOfPjVehicleFdbPub == topic2 { //39
+			f, ok := rule.(func(data *pjisuv_msgs.VehicleFdb, param entity.PjisuvParam) string)
+			if ok != true {
+				c_log.GlobalLogger.Error("插件", triggerLocalPath, "中的Topic方法必须是(func(data *pjisuv_msgs.VehicleFdb) string):", err)
+				continue
+			}
+			RuleOfPjVehicleFdbPub = append(RuleOfPjVehicleFdbPub, f)
 		} else {
 			c_log.GlobalLogger.Error("未知的topic：", topic2)
 			continue

aarch64/pjisuv/master/service/produce_window.go

@@ -21,7 +21,7 @@ import (
 
 // 所有共享变量
 var (
-	extendParam commonEntity.PjisuvParam
+	pjisuvParam commonEntity.PjisuvParam
 	// /cicv_location
 	mutexOfCicvLocation sync.RWMutex
 	// /tpperception
@@ -30,6 +30,8 @@ var (
 	mutexOfPjControlPub sync.RWMutex
 	// /data_read
 	mutexOfDataRead sync.RWMutex
+	// /pj_vehicle_fdb_pub
+	mutexOfPjVehicleFdbPub sync.RWMutex
 )
 
 // PrepareTimeWindowProducerQueue 负责监听所有主题并修改时间窗口
@@ -48,7 +50,7 @@ func PrepareTimeWindowProducerQueue() {
 				for {
 					time.Sleep(time.Duration(3500) * time.Millisecond)
 					for _, f := range masterConfig.RuleOfCicvExtend {
-						label := f(extendParam)
+						label := f(pjisuvParam)
 						if label != "" {
 							saveTimeWindow(label, util.GetNowTimeCustom(), commonEntity.GetLastTimeWindow())
 							break
@@ -367,10 +369,13 @@ func PrepareTimeWindowProducerQueue() {
 					// 更新共享变量
 					mutexOfCicvLocation.RLock()
 					{
-						extendParam.VelocityYOfCicvLocation = data.VelocityX
-						extendParam.VelocityYOfCicvLocation = data.VelocityY
-						extendParam.YawOfCicvLocation = data.Yaw
-						extendParam.AngularVelocityZOfCicvLocation = data.AngularVelocityZ
+						pjisuvParam.VelocityYOfCicvLocation = data.VelocityX
+						pjisuvParam.VelocityYOfCicvLocation = data.VelocityY
+						pjisuvParam.VelocityYOfCicvLocation = data.VelocityZ
+						pjisuvParam.YawOfCicvLocation = data.Yaw
+						pjisuvParam.AngularVelocityZOfCicvLocation = data.AngularVelocityZ
+						pjisuvParam.PositionXOfCicvLocation = data.PositionX
+						pjisuvParam.PositionYOfCicvLocation = data.PositionY
 					}
 					mutexOfCicvLocation.RUnlock()
 
@@ -646,11 +651,11 @@ func PrepareTimeWindowProducerQueue() {
 					// 更新共享变量
 					mutexOfPjControlPub.RLock()
 					{
-						extendParam.NumCountPjiControlCommandOfPjControlPub++
-						if extendParam.NumCountPjiControlCommandOfPjControlPub == 10 {
-							extendParam.EgoSteeringCmdOfPjControlPub = append(extendParam.EgoSteeringCmdOfPjControlPub, data.ICPVCmdStrAngle)
-							extendParam.EgoThrottleCmdOfPjControlPub = append(extendParam.EgoThrottleCmdOfPjControlPub, data.ICPVCmdAccPelPosAct)
-							extendParam.NumCountPjiControlCommandOfPjControlPub = 0
+						pjisuvParam.NumCountPjiControlCommandOfPjControlPub++
+						if pjisuvParam.NumCountPjiControlCommandOfPjControlPub == 10 {
+							pjisuvParam.EgoSteeringCmdOfPjControlPub = append(pjisuvParam.EgoSteeringCmdOfPjControlPub, data.ICPVCmdStrAngle)
+							pjisuvParam.EgoThrottleCmdOfPjControlPub = append(pjisuvParam.EgoThrottleCmdOfPjControlPub, data.ICPVCmdAccPelPosAct)
+							pjisuvParam.NumCountPjiControlCommandOfPjControlPub = 0
 						}
 					}
 					mutexOfPjControlPub.RUnlock()
@@ -877,10 +882,10 @@ func PrepareTimeWindowProducerQueue() {
 							if obj.X <= 5 || math.Abs(float64(obj.Y)) >= 10 {
 								continue
 							}
-							if _, ok := extendParam.ObjDicOfTpperception[obj.Id]; !ok {
-								extendParam.ObjDicOfTpperception[obj.Id] = []float32{}
+							if _, ok := pjisuvParam.ObjDicOfTpperception[obj.Id]; !ok {
+								pjisuvParam.ObjDicOfTpperception[obj.Id] = []float32{}
 							}
-							extendParam.ObjDicOfTpperception[obj.Id] = append(extendParam.ObjDicOfTpperception[obj.Id], obj.Y)
+							pjisuvParam.ObjDicOfTpperception[obj.Id] = append(pjisuvParam.ObjDicOfTpperception[obj.Id], obj.Y)
 						}
 					}
 					mutexOfTpperception.RUnlock()
@@ -891,7 +896,7 @@ func PrepareTimeWindowProducerQueue() {
 						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
 						var faultLabel string
 						for _, f := range masterConfig.RuleOfTpperception {
-							faultLabel = f(data, extendParam)
+							faultLabel = f(data, pjisuvParam)
 							if faultLabel != "" {
 								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
 								break
@@ -1047,13 +1052,13 @@ func PrepareTimeWindowProducerQueue() {
 					// 更新共享变量
 					mutexOfDataRead.RLock()
 					{
-						extendParam.NumCountDataReadOfDataRead++
-						if extendParam.NumCountDataReadOfDataRead == 10 {
-							extendParam.EgoSteeringRealOfDataRead = append(extendParam.EgoSteeringRealOfDataRead, data.ActStrWhAng)
-							extendParam.EgoThrottleRealOfDataRead = append(extendParam.EgoThrottleRealOfDataRead, data.AccPed2)
-							extendParam.NumCountDataReadOfDataRead = 0
+						pjisuvParam.NumCountDataReadOfDataRead++
+						if pjisuvParam.NumCountDataReadOfDataRead == 10 {
+							pjisuvParam.EgoSteeringRealOfDataRead = append(pjisuvParam.EgoSteeringRealOfDataRead, data.ActStrWhAng)
+							pjisuvParam.EgoThrottleRealOfDataRead = append(pjisuvParam.EgoThrottleRealOfDataRead, data.AccPed2)
+							pjisuvParam.NumCountDataReadOfDataRead = 0
 						}
-						extendParam.StrgAngleRealValueOfDataRead = data.ActStrWhAng
+						pjisuvParam.StrgAngleRealValueOfDataRead = data.ActStrWhAng
 					}
 					mutexOfDataRead.RUnlock()
 					subscribersTimeMutexes[i].Lock()
@@ -1103,6 +1108,39 @@ func PrepareTimeWindowProducerQueue() {
 			})
 		}
 
+		// 39
+		if topic == masterConfig.TopicOfPjVehicleFdbPub && len(masterConfig.RuleOfPjVehicleFdbPub) > 0 {
+			subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
+				Node:  commonConfig.RosNode,
+				Topic: topic,
+				Callback: func(data *pjisuv_msgs.VehicleFdb) {
+					// 更新共享变量
+					mutexOfPjControlPub.RLock()
+					{
+						pjisuvParam.AutomodeOfPjVehicleFdbPub = data.Automode
+					}
+					mutexOfPjControlPub.RUnlock()
+					subscribersTimeMutexes[i].Lock()
+					if time.Since(subscribersTimes[i]).Seconds() > 1 {
+						subscribersMutexes[i].Lock()
+						faultHappenTime := util.GetNowTimeCustom()         // 获取当前故障发生时间
+						lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
+						var faultLabel string
+						for _, f := range masterConfig.RuleOfPjVehicleFdbPub {
+							faultLabel = f(data, pjisuvParam)
+							if faultLabel != "" {
+								saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
+								subscribersTimes[i] = time.Now()
+								break
+							}
+						}
+						subscribersMutexes[i].Unlock()
+					}
+					subscribersTimeMutexes[i].Unlock()
+				},
+			})
+		}
+
 		if err != nil {
 			c_log.GlobalLogger.Info("创建订阅者报错：", err)
 			//TODO 如何回传日志

common/entity/extend_param.go

@@ -4,8 +4,11 @@ type PjisuvParam struct {
 	// /cicv_location
 	VelocityXOfCicvLocation        float64
 	VelocityYOfCicvLocation        float64
+	VelocityZOfCicvLocation        float64
 	YawOfCicvLocation              float64
 	AngularVelocityZOfCicvLocation float64
+	PositionXOfCicvLocation        float64
+	PositionYOfCicvLocation        float64
 
 	// /tpperception
 	ObjDicOfTpperception map[uint32][]float32
@@ -19,6 +22,8 @@ type PjisuvParam struct {
 	EgoSteeringRealOfDataRead    []float64
 	EgoThrottleRealOfDataRead    []float64
 	StrgAngleRealValueOfDataRead float64
+	// /pj_vehicle_fdb_pub
+	AutomodeOfPjVehicleFdbPub int16
 }
 
 type KinglongParam struct {

trigger/pjisuv/cicv_location/overswing/main/overswing.go

@@ -0,0 +1,42 @@
+package main
+
+import (
+	"cicv-data-closedloop/pjisuv_msgs"
+	"math"
+)
+
+/*
+def callback_cicv_location(data):
+    global angular_velocity_z
+    global Ego_position_x
+    global Ego_position_y
+    global Ego_yaw
+
+
+    Ego_position_x=data.position_x
+    Ego_position_y=data.position_y
+    Ego_yaw=data.yaw
+    #print(Ego_yaw)
+    angular_velocity_z=data.angular_velocity_z
+    #print(angular_velocity_z)
+    if abs(angular_velocity_z)>=27:
+        event_label='overswing' #横摆角速度过大
+        print(event_label)
+
+*/
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+func Label() string {
+	return "overswing"
+}
+
+func Rule(data *pjisuv_msgs.PerceptionLocalization) string {
+	if math.Abs(data.AngularVelocityZ) >= 27.0 {
+		return "overswing"
+	} else {
+		return ""
+	}
+}

trigger/pjisuv/map_polygon/outofroad/main/outofroad.go

@@ -0,0 +1,22 @@
+package main
+
+import (
+	"cicv-data-closedloop/common/entity"
+	"cicv-data-closedloop/pjisuv_msgs"
+)
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+func Label() string {
+	return "DriverTakeOver"
+}
+
+func Rule(data *pjisuv_msgs.VehicleFdb, param entity.PjisuvParam) string {
+	if data.Automode == 0 && param.AutomodeOfPjVehicleFdbPub == 1 {
+		return "DriverTakeOver"
+	} else {
+		return ""
+	}
+}

trigger/pjisuv/pj_vehicle_fdb_pub/DriverTakeOver/main/DriverTakeOver.go

@@ -0,0 +1,33 @@
+package main
+
+import (
+	"cicv-data-closedloop/common/entity"
+	"cicv-data-closedloop/pjisuv_msgs"
+)
+
+/*
+def callback_pj_vehicle_fdb_pub(data):
+    global pre_Automode_stat_flag
+    if data.Automode==0 and pre_Automode_stat_flag==1:
+        event_label='DriverTakeOver'    #人工接管
+        print(event_label)
+    pre_Automode_stat_flag=data.Automode
+
+
+*/
+
+func Topic() string {
+	return "/cicv_location"
+}
+
+func Label() string {
+	return "DriverTakeOver"
+}
+
+func Rule(data *pjisuv_msgs.VehicleFdb, param entity.PjisuvParam) string {
+	if data.Automode == 0 && param.AutomodeOfPjVehicleFdbPub == 1 {
+		return "DriverTakeOver"
+	} else {
+		return ""
+	}
+}

trigger/pjisuv/tpperception/TTC/main/TTC.go

@@ -6,6 +6,26 @@ import (
 	"math"
 )
 
+/*
+def callback_tpperception(data):
+    global obj_dic
+    obj_list=data.objs
+    for obj in obj_list:
+
+        if  obj.x>=5  and abs(obj.y)<=10: # 根据这个条件过滤一遍目标物 ,  用于前车切入切出判别
+            if obj.id not in obj_dic:
+                obj_dic[obj.id]=[]
+            obj_dic[obj.id].append(obj.y)
+
+        if abs(obj.y)<=2 and obj.x>=6: # 根据这个条件过滤一遍目标物 ,  用于计算ＴＴＣ
+
+            TTC=-(obj.x-4)/(obj.vxrel+0.0001)
+            if 0<=TTC<=3:
+                event_label='TTC'
+                print(event_label)
+                break
+*/
+
 func Topic() string {
 	return "/tpperception"
 }
@@ -15,14 +35,11 @@ func Label() string {
 	return "TTC"
 }
 
-func Rule(data *pjisuv_msgs.PerceptionObjects, param entity.ExtendParam) string {
+func Rule(data *pjisuv_msgs.PerceptionObjects, param entity.PjisuvParam) string {
 	for _, obj := range data.Objs {
-		if math.Abs(float64(obj.Y)) <= 2 && obj.X >= 8 {
-			theta := DegreesToRadians(param.YawOfCicvLocation)
-			vxrel := (float64(obj.Vxabs)-param.VelocityXOfCicvLocation)*math.Cos(theta) + (float64(obj.Vyabs)-param.VelocityYOfCicvLocation)*math.Sin(theta)
-			ttc := -((float64(obj.X) - 5.2) / (vxrel + 0.001))
-			//fmt.Println("TTC值为：", ttc)
-			if ttc >= 0 && ttc <= 5 {
+		if math.Abs(float64(obj.Y)) <= 2 && obj.X >= 6 {
+			ttc := -((float64(obj.X) - 4) / (float64(obj.Vxrel) + 0.001))
+			if ttc >= 0 && ttc <= 3 {
 				return "TTC"
 			}
 		}