public void SendRadarData()
{
if (Bridge == null || Bridge.Status != Comm.BridgeStatus.Connected)
{
return;
}
var apolloHeader = new Ros.ApolloHeader()
{
timestamp_sec = (System.DateTime.UtcNow - originTime).TotalSeconds,
module_name = "conti_radar",
sequence_num = seqId
};
var radarPos = transform.position;
var radarAim = transform.forward;
var radarRight = transform.right;
radarObjList.Clear();
utilColList.Clear();
utilColList.AddRange(radarDetectedColliders.Keys);
utilColList.RemoveAll(c => c == null);
//Debug.Log("radarDetectedColliders.Count: " + radarDetectedColliders.Count);
System.Func <Collider, int> GetDynPropInt = ((col) => {
var trafAiMtr = col.GetComponentInParent <TrafAIMotor>();
if (trafAiMtr != null)
{
return(trafAiMtr.currentSpeed > 1.0f ? 0 : 1);
}
return(1);
});
System.Func <Collider, Vector3> GetLinVel = ((col) => {
var trafAiMtr = col.GetComponentInParent <TrafAIMotor>();
if (trafAiMtr != null)
{
return(trafAiMtr.currentVelocity);
}
else
{
return(col.attachedRigidbody == null ? Vector3.zero : col.attachedRigidbody.velocity);
}
});
for (int i = 0; i < utilColList.Count; i++)
{
Collider col = utilColList[i];
Vector3 point = radarDetectedColliders[col].point;
Vector3 relPos = point - radarPos;
Vector3 carVel = gameObject.GetComponentInParent <Rigidbody>().velocity;
Vector3 relVel = carVel - GetLinVel(col);
//Debug.Log("id to be assigned to obstacle_id is " + radarDetectedColliders[col].id);
Vector3 size = col.bounds.size;
// angle is orientation of the obstacle in degrees as seen by radar, counterclockwise is positive
double angle = -Vector3.SignedAngle(transform.forward, col.transform.forward, transform.up);
if (angle > 90)
{
angle -= 180;
}
else if (angle < -90)
{
angle += 180;
}
radarObjList.Add(new Ros.Apollo.Drivers.ContiRadarObs()
{
header = apolloHeader,
clusterortrack = false,
obstacle_id = radarDetectedColliders[col].id,
longitude_dist = Vector3.Project(relPos, radarAim).magnitude,
lateral_dist = Vector3.Project(relPos, radarRight).magnitude *(Vector3.Dot(relPos, radarRight) > 0 ? -1 : 1),
longitude_vel = Vector3.Project(relVel, radarAim).magnitude *(Vector3.Dot(relVel, radarAim) > 0 ? -1 : 1),
lateral_vel = Vector3.Project(relVel, radarRight).magnitude *(Vector3.Dot(relVel, radarRight) > 0 ? -1 : 1),
rcs = 11.0, //
dynprop = GetDynPropInt(col), // seem to be constant
longitude_dist_rms = 0,
lateral_dist_rms = 0,
longitude_vel_rms = 0,
lateral_vel_rms = 0,
probexist = 1.0, //prob confidence
meas_state = radarDetectedColliders[col].newDetection ? 1 : 2, //1 new 2 exist
longitude_accel = 0,
lateral_accel = 0,
oritation_angle = angle,
longitude_accel_rms = 0,
lateral_accel_rms = 0,
oritation_angle_rms = 0,
length = size.z,
width = size.x,
obstacle_class = size.z > 5 ? 2 : 1, // 0: point; 1: car; 2: truck; 3: pedestrian; 4: motorcycle; 5: bicycle; 6: wide; 7: unknown
});
}
var msg = new Ros.Apollo.Drivers.ContiRadar
{
header = apolloHeader,
contiobs = radarObjList,
object_list_status = new Ros.Apollo.Drivers.ObjectListStatus_60A
{
nof_objects = utilColList.Count,
meas_counter = 22800,
interface_version = 0
}
};
ApolloWriterContiRadar.Publish(msg);
++seqId;
}
public void SendRadarData()
{
if (Bridge == null || Bridge.Status != Ros.Status.Connected)
{
return;
}
var apolloHeader = new Ros.ApolloHeader()
{
timestamp_sec = (System.DateTime.UtcNow - originTime).TotalSeconds,
module_name = "conti_radar",
sequence_num = seqId
};
var radarPos = transform.position;
var radarAim = transform.forward;
var radarRight = transform.right;
radarObjList.Clear();
utilColList.Clear();
utilColList.AddRange(radarDetectedColliders.Keys);
utilColList.RemoveAll(c => c == null);
//Debug.Log("radarDetectedColliders.Count: " + radarDetectedColliders.Count);
for (int i = 0; i < radarDetectedColliders.Count; i++)
{
Collider col = utilColList[i];
Vector3 point = radarDetectedColliders[col].point;
Vector3 relPos = point - radarPos;
Vector3 relVel = col.attachedRigidbody == null ? Vector3.zero : col.attachedRigidbody.velocity;
//Debug.Log("id to be assigned to obstacle_id is " + radarDetectedColliders[col].id);
radarObjList.Add(new Ros.Apollo.Drivers.ContiRadarObs()
{
header = apolloHeader,
clusterortrack = false,
obstacle_id = radarDetectedColliders[col].id,
longitude_dist = Vector3.Project(relPos, radarAim).magnitude,
lateral_dist = Vector3.Project(relPos, radarRight).magnitude,
longitude_vel = Vector3.Project(relVel, radarAim).magnitude,
lateral_vel = Vector3.Project(relVel, radarRight).magnitude,
rcs = 11.0, //
dynprop = 0, // seem to be constant
longitude_dist_rms = 0,
lateral_dist_rms = 0,
longitude_vel_rms = 0,
lateral_vel_rms = 0,
probexist = 1.0, //prob confidence
meas_state = radarDetectedColliders[col].newDetection ? 1 : 2, //1 new 2 exist
longitude_accel = 0,
lateral_accel = 0,
oritation_angle = 0,
longitude_accel_rms = 0,
lateral_accel_rms = 0,
oritation_angle_rms = 0,
length = 2.0,
width = 2.4,
obstacle_class = 1, // single type but need to find car number
});
}
var msg = new Ros.Apollo.Drivers.ContiRadar
{
header = apolloHeader,
contiobs = radarObjList,
object_list_status = new Ros.Apollo.Drivers.ObjectListStatus_60A
{
nof_objects = utilColList.Count,
meas_counter = 22800,
interface_version = 0
}
};
Bridge.Publish(ApolloTopicName, msg);
++seqId;
}
