Beispiel #1
0
    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;
    }
Beispiel #2
0
    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;
    }