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; }