|
@@ -2,6 +2,7 @@ package main
|
|
|
|
|
|
import (
|
|
import (
|
|
"cicv-data-closedloop/pjisuv_msgs"
|
|
"cicv-data-closedloop/pjisuv_msgs"
|
|
|
|
+ "fmt"
|
|
"math"
|
|
"math"
|
|
)
|
|
)
|
|
|
|
|
|
@@ -19,7 +20,8 @@ func Rule(data *pjisuv_msgs.PerceptionObjects, velocityX float64, velocityY floa
|
|
theta := DegreesToRadians(yaw)
|
|
theta := DegreesToRadians(yaw)
|
|
vxrel := (float64(obj.Vxabs)-velocityX)*math.Cos(theta) + (float64(obj.Vyabs)-velocityY)*math.Sin(theta)
|
|
vxrel := (float64(obj.Vxabs)-velocityX)*math.Cos(theta) + (float64(obj.Vyabs)-velocityY)*math.Sin(theta)
|
|
ttc := -((float64(obj.X) - 5.2) / (vxrel + 0.001))
|
|
ttc := -((float64(obj.X) - 5.2) / (vxrel + 0.001))
|
|
- if ttc >= 0 && ttc <= 3 {
|
|
+ fmt.Println("TTC值为:", ttc)
|
|
|
|
+ if ttc >= 0 && ttc <= 5 {
|
|
return "TTC"
|
|
return "TTC"
|
|
}
|
|
}
|
|
}
|
|
}
|