lowSpdTruckAhead.go 2.6 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495
  1. package main
  2. import (
  3. "cicv-data-closedloop/pjisuv_msgs"
  4. "fmt"
  5. "math"
  6. "sync"
  7. )
  8. type Object struct {
  9. ID uint32
  10. StableAge int
  11. LastExistTime float64
  12. }
  13. var objectsStability = make(map[uint32]Object)
  14. func Topic() string {
  15. return "/tpperception"
  16. }
  17. // 禁止存在下划线_
  18. func Label() string {
  19. return "LowSpdTruckAhead"
  20. }
  21. // speedCheck 目标物速度检测
  22. func speedCheck(obj *pjisuv_msgs.PerceptionObject) bool {
  23. const targetMinSpeed = 2 / 3.6 // m/s
  24. const targetMaxSpeed = 20 / 3.6 // m/s
  25. return targetMinSpeed < obj.Speed && obj.Speed < targetMaxSpeed
  26. }
  27. // typeCheck 目标物类型检测
  28. // × 金龙车:CAR_TYPE=0, TRUCK_TYPE=1, PEDESTRIAN_TYPE=2, CYCLIST_TYPE=3, UNKNOWN_TYPE=4, UNKNOWN_MOVABLE_TYPE=5, UNKNOWN_UNMOVABLE_TYPE=6
  29. // √ 多功能车:UNKNOWN TYPE=O, PEDESTRIAN TYPE=1, CAR TYPE=2, TRUCK TYPE=3, Bicycle TYPE=4, Tricycle TYPE=5, Traffic Cone TYPE=6
  30. func typeCheck(obj *pjisuv_msgs.PerceptionObject) bool {
  31. const targetType uint8 = 3
  32. return targetType == obj.Type
  33. }
  34. // posCheck 判断目标物位置关系
  35. func posCheck(obj *pjisuv_msgs.PerceptionObject) bool {
  36. const laneWidth = 3.5 // m
  37. return obj.X > 0 && math.Abs(float64(obj.Y)) < laneWidth*1.5
  38. }
  39. // Rule 检测前方大车低速行驶
  40. func Rule(shareVars *sync.Map, msg *pjisuv_msgs.PerceptionObjects) string {
  41. defer func() {
  42. if r := recover(); r != nil {
  43. fmt.Println(fmt.Sprintf("Recovered from panic: [%s], [%#v]", Label(), r))
  44. }
  45. }()
  46. OutsideWorkshopFlag, _ := shareVars.Load("OutsideWorkshopFlag")
  47. OutsideWorkshopFlag = OutsideWorkshopFlag.(bool)
  48. const MinStableFrameCount = 5
  49. const MaxTimeBetweenFrame = 0.5
  50. for _, obj := range msg.Objs {
  51. // 判断目标物是否满足筛选条件
  52. if speedCheck(&obj) && posCheck(&obj) && typeCheck(&obj) {
  53. // 目标物初见存档
  54. if prevObjRecord, exists := objectsStability[obj.Id]; exists == false {
  55. objectsStability[obj.Id] = Object{
  56. ID: obj.Id,
  57. StableAge: 1,
  58. LastExistTime: msg.Header.TimeStamp,
  59. }
  60. } else {
  61. currObjRecord := Object{
  62. ID: obj.Id,
  63. StableAge: 1,
  64. LastExistTime: msg.Header.TimeStamp,
  65. }
  66. // 目标物确认/更新/重置
  67. if currObjRecord.LastExistTime-prevObjRecord.LastExistTime <= MaxTimeBetweenFrame { // 目标物消失不超过0.5s
  68. currObjRecord.StableAge = prevObjRecord.StableAge + 1
  69. if currObjRecord.StableAge == MinStableFrameCount && OutsideWorkshopFlag == true {
  70. delete(objectsStability, obj.Id)
  71. return Label()
  72. }
  73. }
  74. objectsStability[obj.Id] = currObjRecord
  75. }
  76. }
  77. }
  78. return ""
  79. }