void FixedUpdate() { if (MapOrigin == null || Bridge == null || Bridge.Status != Status.Connected) { return; } if (IsFirstFixedUpdate) { lock (MessageQueue) { MessageQueue.Clear(); } IsFirstFixedUpdate = false; } var time = SimulatorManager.Instance.CurrentTime; if (time < LastTimestamp) { return; } var location = MapOrigin.GetGpsLocation(transform.position, IgnoreMapOrigin); var orientation = transform.rotation; orientation.Set(-orientation.z, orientation.x, -orientation.y, orientation.w); // converting to right handed xyz var data = new GpsData() { Name = Name, Frame = Frame, Time = SimulatorManager.Instance.CurrentTime, Sequence = SendSequence++, IgnoreMapOrigin = IgnoreMapOrigin, Latitude = location.Latitude, Longitude = location.Longitude, Altitude = location.Altitude, Northing = location.Northing, Easting = location.Easting, Orientation = orientation, }; lock (MessageQueue) { MessageQueue.Enqueue(Tuple.Create(time, (Action)(() => { if (Bridge != null && Bridge.Status == Status.Connected) { Publish(data); } }))); } }
void FixedUpdate() { if (MapOrigin == null || Bridge == null || Bridge.Status != Status.Connected) { return; } if (IsFirstFixedUpdate) { lock (MessageQueue) { MessageQueue.Clear(); } IsFirstFixedUpdate = false; } var time = SimulatorManager.Instance.CurrentTime; if (time < LastTimestamp) { return; } var location = MapOrigin.GetGpsLocation(transform.position, IgnoreMapOrigin); var data = new GpsOdometryData() { Name = Name, Frame = Frame, Time = SimulatorManager.Instance.CurrentTime, Sequence = SendSequence++, ChildFrame = ChildFrame, IgnoreMapOrigin = IgnoreMapOrigin, Latitude = location.Latitude, Longitude = location.Longitude, Altitude = location.Altitude, Northing = location.Northing, Easting = location.Easting, Orientation = transform.rotation, ForwardSpeed = Vector3.Dot(RigidBody.velocity, transform.forward), Velocity = RigidBody.velocity, AngularVelocity = RigidBody.angularVelocity, WheelAngle = (Dynamics.axles[0].left.steerAngle + Dynamics.axles[0].right.steerAngle) * 0.5f, }; lock (MessageQueue) { MessageQueue.Enqueue(Tuple.Create(time, (Action)(() => Writer.Write(data)))); } }
private void AddReferencePoint(MapOrigin origin) { var index = FindObjectsOfType <MapOriginReferencePoint>(true).Length + 1; var p = new GameObject("ReferencePoint" + index).AddComponent <MapOriginReferencePoint>(); if (SceneView.lastActiveSceneView != null) { var camera = SceneView.lastActiveSceneView.camera.transform; if (Physics.Raycast(new Ray(camera.position, camera.forward), out var hit)) { p.transform.position = hit.point; } } var gps = origin.GetGpsLocation(p.transform.position); p.latitue = gps.Latitude; p.longitude = gps.Longitude; var mapHolder = FindObjectOfType <MapHolder>().transform; var holder = mapHolder.Find("ReferencePoints"); if (holder == null) { holder = new GameObject("ReferencePoints").transform; holder.parent = mapHolder; } p.transform.parent = holder; Selection.activeGameObject = p.gameObject; }
public void Update() { if (MapOrigin == null) { return; } if (Time.time < NextSend) { return; } NextSend = Time.time + 1.0f / Frequency; float speed = RigidBody.velocity.magnitude; var gps = MapOrigin.GetGpsLocation(transform.position); var msg = new CanBusData() { Name = Name, Frame = Frame, Time = SimulatorManager.Instance.CurrentTime, Sequence = SendSequence++, Speed = speed, Throttle = Dynamics.AccellInput > 0 ? Dynamics.AccellInput : 0, Braking = Dynamics.AccellInput < 0 ? -Dynamics.AccellInput : 0, Steering = Dynamics.SteerInput, ParkingBrake = Dynamics.HandBrake, HighBeamSignal = Actions.CurrentHeadLightState == VehicleActions.HeadLightState.HIGH, LowBeamSignal = Actions.CurrentHeadLightState == VehicleActions.HeadLightState.LOW, HazardLights = Actions.HazardLights, FogLights = Actions.FogLights, LeftTurnSignal = Actions.LeftTurnSignal, RightTurnSignal = Actions.RightTurnSignal, Wipers = false, InReverse = Dynamics.Reverse, Gear = Mathf.RoundToInt(Dynamics.CurrentGear), EngineOn = Dynamics.IgnitionStatus == IgnitionStatus.On, EngineRPM = Dynamics.CurrentRPM, Latitude = gps.Latitude, Longitude = gps.Longitude, Altitude = gps.Altitude, Orientation = transform.rotation, Velocity = RigidBody.velocity, }; if (Bridge != null && Bridge.Status == Status.Connected) { Writer.Write(msg, null); } }
void FixedUpdate() { // Publish Frequency Limit if (lastUpdateTime.AddSeconds(1.0f / Frequency) > DateTime.Now) { return; } lastUpdateTime = DateTime.Now; // Sanity Check if (MapOrigin == null || Bridge == null || Bridge.Status != Status.Connected) { return; } // Get Data for Packing float speed = RigidBody.velocity.magnitude; var gps = MapOrigin.GetGpsLocation(transform.position); if (creep_state == true) { creep_state = false; } else { temp_vehicle_speed = speed * 3.60f; } // Pack message data msg = new ChassisData() { stamp = DateTime.Now, frame_id = "", steering_torque = 0.0f, engine_rpm = Dynamics.CurrentRPM, vehicle_speed = temp_vehicle_speed, throttle_percent = Dynamics.AccellInput > 0 ? Dynamics.AccellInput : 0, brake_percent = Dynamics.AccellInput < 0 ? -Dynamics.AccellInput : 0, steering_angle = -Dynamics.CurrentSteerAngle * Dynamics.SteeringRatio, brake_status = false, cruise_start = false, cruise_cancel = false, takeover = false, }; //speed during creep behaviour if (msg.brake_percent == 0.0f && msg.vehicle_speed >= 7.92f && msg.throttle_percent == 0.0f) { creep_state = true; msg.vehicle_speed = 2.2f * 3.60f; temp_vehicle_speed = msg.vehicle_speed; } // Write Data to Message Queue Writer.Write(msg, null); }
public void Init(MapOrigin origin) { this.origin = origin; var gps = origin.GetGpsLocation(origin.transform.position); latitude = Math.Round(gps.Latitude, 6); longitude = Math.Round(gps.Longitude, 6); minSize = new Vector2(250, 120); maxSize = new Vector2(300, 120); }
public void RecordJSONLog(Detected3DObject detected3DObject, string labelId, GameObject parent, float cycleTime) { var sensorRotation = transform.rotation; var sensorPosition = transform.position; var globalPos = parent.transform.position; var globalRot = parent.transform.rotation; if (!prevTimes.ContainsKey(labelId)) { prevTimes.Add(labelId, SimulatorManager.Instance.CurrentTime); } // Get GPS coordinates. bool IgnoreMapOrigin = false; GpsLocation location = new GpsLocation(); Vector3 position = transform.position; location = MapOrigin.GetGpsLocation(position, IgnoreMapOrigin); if (SimulatorManager.Instance.CurrentTime - prevTimes[labelId] > cycleTime) { // string labelId = null; var groundTruth3D = new JSONGroundTruth3D() { Id = detected3DObject.Id, Label = detected3DObject.Label, Position = (float3)detected3DObject.Position, Rotation = detected3DObject.Rotation, Velocity = detected3DObject.LinearVelocity, Dimension = detected3DObject.Scale, Time = SimulatorManager.Instance.CurrentTime, GpsPosition = new Vector3((float)(location.Easting + (IgnoreMapOrigin ? -500000 : 0)), (float)location.Northing, (float)location.Altitude), GpsRotation = sensorRotation, GlobalPosition = globalPos, GlobalRotation = globalRot }; if (!LogJsonDetected.ContainsKey(labelId)) { var jsonGroundTruth3Ds = new List <JSONGroundTruth3D>(); jsonGroundTruth3Ds.Add(groundTruth3D); LogJsonDetected.Add(labelId, jsonGroundTruth3Ds); } else { LogJsonDetected[labelId].Add(groundTruth3D); } prevTimes[labelId] = SimulatorManager.Instance.CurrentTime; } }
void FixedUpdate() { if (MapOrigin == null || Bridge == null || Bridge.Status != Status.Connected) { return; } if (IsFirstFixedUpdate) { lock (MessageQueue) { MessageQueue.Clear(); } IsFirstFixedUpdate = false; //Creation of the data file to add the values /*using (System.IO.StreamWriter sw = System.IO.File.CreateText(@"C:\Users\doria\Desktop\GPS_test.txt")) * { * sw.WriteLine("Test"); * }*/ } var time = SimulatorManager.Instance.CurrentTime; if (time < LastTimestamp) { return; } var location = MapOrigin.GetGpsLocation(transform.position, IgnoreMapOrigin); var orientation = transform.rotation; var angularVelocity = RigidBody.angularVelocity; float dLatitude = 0; float dLongitude = 0; float dAltitude = 0; if (activateFailure && time > failTime && time < failStopTime) { health = false; } else { health = true; } if (health) { orientation.Set(-orientation.z, orientation.x, -orientation.y, orientation.w); // converting to right handed xyz angularVelocity.Set(-angularVelocity.z, angularVelocity.x, -angularVelocity.y); // converting to right handed xyz if (activateBias) { dLatitude = genBias(sLatitude); dLongitude = genBias(sLongitude); dAltitude = genBias(sAltitude); location.Latitude += dLatitude; location.Longitude += dLongitude; location.Altitude += dAltitude; } } else { location.Latitude = 0; location.Longitude = 0; location.Altitude = 0; orientation.Set(0, 0, 0, 1); // converting to right handed xyz angularVelocity.Set(0, 0, 0); // converting to right handed xyz } data = new GpsOdometryData() { Name = Name, Frame = Frame, Time = SimulatorManager.Instance.CurrentTime, Sequence = SendSequence++, ChildFrame = ChildFrame, IgnoreMapOrigin = IgnoreMapOrigin, Latitude = location.Latitude, Longitude = location.Longitude, Altitude = location.Altitude, Northing = location.Northing, Easting = location.Easting, Orientation = orientation, ForwardSpeed = Vector3.Dot(RigidBody.velocity, transform.forward), Velocity = RigidBody.velocity, AngularVelocity = angularVelocity, WheelAngle = Dynamics.WheelAngle, }; lock (MessageQueue) { MessageQueue.Enqueue(Tuple.Create(time, (Action)(() => Writer.Write(data)))); } }
private void Awake() { mapOrigin = MapOrigin.Find(); gpsLocation = mapOrigin.GetGpsLocation(Vector3.zero); }
void WhileInRange(Collider other) { GameObject egoGO = transform.parent.gameObject; GameObject parent = other.transform.parent.gameObject; if (parent == egoGO) { return; } if (!(other.gameObject.layer == LayerMask.NameToLayer("GroundTruth")) || !parent.activeInHierarchy) { return; } if (!Detected.ContainsKey(other)) { uint id; string label = null; float linear_vel; float angular_vel; float egoPosY = egoGO.GetComponent <VehicleActions>().Bounds.center.y; float score = 0.0f; if (parent.layer == LayerMask.NameToLayer("Agent")) { var egoC = parent.GetComponent <VehicleController>(); var egoA = parent.GetComponent <VehicleActions>(); var rb = parent.GetComponent <Rigidbody>(); id = egoC.GTID; label = "Sedan"; linear_vel = UnityEngine.Vector3.Dot(rb.velocity, parent.transform.forward); angular_vel = -rb.angularVelocity.y; } else if (parent.layer == LayerMask.NameToLayer("NPC")) { var npcC = parent.GetComponent <NPCController>(); id = npcC.GTID; label = npcC.NPCType; linear_vel = UnityEngine.Vector3.Dot(npcC.GetVelocity(), parent.transform.forward); angular_vel = -npcC.GetAngularVelocity().y; } else if (parent.layer == LayerMask.NameToLayer("Pedestrian")) { var pedC = parent.GetComponent <PedestrianController>(); id = pedC.GTID; label = "Pedestrian"; linear_vel = UnityEngine.Vector3.Dot(pedC.CurrentVelocity, parent.transform.forward); angular_vel = -pedC.CurrentAngularVelocity.y; } else { return; } UnityEngine.Vector3 size = ((BoxCollider)other).size; // Convert from (Right/Up/Forward) to (Forward/Left/Up) size.Set(size.z, size.x, size.y); if (size.magnitude == 0) { return; } //Bounding Box Orientation //var Rot = Quaternion.Euler(parent.transform.rotation.eulerAngles); var rot_euler = parent.transform.rotation.eulerAngles; var Rot = UnityEngine.Quaternion.Euler(rot_euler); //Bounding Box Position if (MapOrigin == null) { return; } UnityEngine.Vector3 bounding_box_center = parent.transform.TransformPoint(((BoxCollider)other).center); var location = MapOrigin.GetGpsLocation(bounding_box_center /*parent.transform.position*/); if (!string.IsNullOrEmpty(label)) { score = 1.0f; } Detected.Add(other, new Detected3DObjectPerc() { Id = id, Label = label, Score = score, easting = location.Easting, northing = location.Northing, Rotation = Rot, Scale = size, LinearVelocity = new UnityEngine.Vector3(linear_vel, 0, 0), // Linear velocity in forward direction of objects, in meters/sec AngularVelocity = new UnityEngine.Vector3(0, 0, angular_vel), // Angular velocity around up axis of objects, in radians/sec }); } }
void WhileInRange(Collider other) { GameObject egoGO = transform.parent.gameObject; GameObject parent = other.transform.parent.gameObject; if (parent == egoGO) { return; } if (!(other.gameObject.layer == LayerMask.NameToLayer("GroundTruth")) || !parent.activeInHierarchy) { return; } if (!Detected.ContainsKey(other)) { uint id; string label; float linear_vel; float angular_vel; if (parent.layer == LayerMask.NameToLayer("NPC")) { var npcC = parent.GetComponent <NPCController>(); id = npcC.GTID; label = npcC.NPCType; linear_vel = UnityEngine.Vector3.Dot(npcC.GetVelocity(), parent.transform.forward); angular_vel = -npcC.GetAngularVelocity().y; } else { return; } //Bounding Box Size UnityEngine.Vector3 size = ((BoxCollider)other).size; if (size.magnitude == 0) { return; } // Convert from (Right/Up/Forward) to (Forward/Left/Up) size.Set(size.z, size.x, size.y); //Bounding Box Orientation //var Rot = Quaternion.Euler(parent.transform.rotation.eulerAngles); var rot_euler = parent.transform.rotation.eulerAngles; //90 degree phase shift to match with localization rot_euler.y = 90 - rot_euler.y; var Rot = UnityEngine.Quaternion.Euler(rot_euler); //Bounding Box Position if (MapOrigin == null) { return; } UnityEngine.Vector3 bounding_box_center = parent.transform.TransformPoint(((BoxCollider)other).center); var location = MapOrigin.GetGpsLocation(bounding_box_center /*parent.transform.position*/); Detected.Add(other, new Detected3DObjectAdhoc() { Id = id, Label = label, Score = 1.0f, easting = location.Easting, northing = location.Northing, Rotation = Rot, Scale = size, LinearVelocity = new UnityEngine.Vector3(linear_vel, 0, 0), // Linear velocity in forward direction of objects, in meters/sec AngularVelocity = new UnityEngine.Vector3(0, 0, angular_vel), // Angular velocity around up axis of objects, in radians/sec }); } }
void FixedUpdate() { if (MapOrigin == null || Bridge == null || Bridge.Status != Status.Connected) { return; } if (IsFirstFixedUpdate) { lock (MessageQueue) { MessageQueue.Clear(); } IsFirstFixedUpdate = false; } var time = SimulatorManager.Instance.CurrentTime; if (time < LastTimestamp) { return; } var location = MapOrigin.GetGpsLocation(transform.position, IgnoreMapOrigin); var orientation = transform.rotation; orientation.Set(-orientation.z, orientation.x, -orientation.y, orientation.w); // converting to right handed xyz var angularVelocity = RigidBody.angularVelocity; angularVelocity.Set(-angularVelocity.z, angularVelocity.x, -angularVelocity.y); // converting to right handed xyz var data = new GpsOdometryData() { Name = Name, Frame = Frame, Time = SimulatorManager.Instance.CurrentTime, Sequence = SendSequence++, ChildFrame = ChildFrame, IgnoreMapOrigin = IgnoreMapOrigin, Latitude = location.Latitude, Longitude = location.Longitude, Altitude = location.Altitude, Northing = location.Northing, Easting = location.Easting, Orientation = orientation, ForwardSpeed = Vector3.Dot(RigidBody.velocity, transform.forward), Velocity = RigidBody.velocity, AngularVelocity = angularVelocity, WheelAngle = Dynamics.WheelAngle, }; lock (MessageQueue) { MessageQueue.Enqueue(Tuple.Create(time, (Action)(() => { if (Bridge != null && Bridge.Status == Status.Connected) { Publish(data); } }))); } }
void WhileInRange(Collider other) { GameObject egoGO = transform.parent.gameObject; GameObject parent = other.transform.parent.gameObject; if (parent == egoGO) { return; } if (!(other.gameObject.layer == LayerMask.NameToLayer("GroundTruth")) || !parent.activeInHierarchy) { return; } uint id; string label; Vector3 velocity; float angular_speed; // Angular speed around up axis of objects, in radians/sec if (parent.layer == LayerMask.NameToLayer("Agent")) { var egoC = parent.GetComponent <VehicleController>(); var rb = parent.GetComponent <Rigidbody>(); id = egoC.GTID; label = "Sedan"; velocity = rb.velocity; angular_speed = rb.angularVelocity.y; } else if (parent.layer == LayerMask.NameToLayer("NPC")) { var npcC = parent.GetComponent <NPCController>(); id = npcC.GTID; label = npcC.NPCLabel; velocity = npcC.GetVelocity(); angular_speed = npcC.GetAngularVelocity().y; } else if (parent.layer == LayerMask.NameToLayer("Pedestrian")) { var pedC = parent.GetComponent <PedestrianController>(); id = pedC.GTID; label = "Pedestrian"; velocity = pedC.CurrentVelocity; angular_speed = pedC.CurrentAngularVelocity.y; } else { return; } Vector3 size = ((BoxCollider)other).size; if (size.magnitude == 0) { return; } // Linear speed in forward direction of objects, in meters/sec float speed = Vector3.Dot(velocity, parent.transform.forward); // Local position of object in ego local space Vector3 relPos = transform.InverseTransformPoint(parent.transform.position); // Relative rotation of objects wrt ego frame Quaternion relRot = Quaternion.Inverse(transform.rotation) * parent.transform.rotation; var mapRotation = MapOrigin.transform.localRotation; velocity = Quaternion.Inverse(mapRotation) * velocity; var heading = parent.transform.localEulerAngles.y - mapRotation.eulerAngles.y; // Center of bounding box GpsLocation location = MapOrigin.GetGpsLocation(((BoxCollider)other).bounds.center); GpsData gps = new GpsData() { Easting = location.Easting, Northing = location.Northing, Altitude = location.Altitude, }; if (!Detected.ContainsKey(id)) { var det = new Detected3DObject() { Id = id, Label = label, Score = 1.0f, Position = relPos, Rotation = relRot, Scale = size, LinearVelocity = new Vector3(speed, 0, 0), AngularVelocity = new Vector3(0, 0, angular_speed), Velocity = velocity, Gps = gps, Heading = heading, TrackingTime = 0f, }; Detected.Add(id, new Tuple <Detected3DObject, Collider>(det, other)); } else { var det = Detected[id].Item1; det.Position = relPos; det.Rotation = relRot; det.LinearVelocity = new Vector3(speed, 0, 0); det.AngularVelocity = new Vector3(0, 0, angular_speed); det.Acceleration = (velocity - det.Velocity) / Time.fixedDeltaTime; det.Velocity = velocity; det.Gps = gps; det.Heading = heading; det.TrackingTime += Time.fixedDeltaTime; } CurrentIDs.Add(id); }
void FixedUpdate() { // Publish Frequency Limit if (lastUpdateTime.AddSeconds(1.0f / Frequency) > DateTime.Now) { return; } lastUpdateTime = DateTime.Now; // Sanity Check if (MapOrigin == null || Bridge == null || Bridge.Status != Status.Connected) { return; } // Get Data for Packing var location = MapOrigin.GetGpsLocation(transform.position, IgnoreMapOrigin); // INSPVA WriterInsPVA.Write(new novatelInsPvaData() { stamp = DateTime.Now, frame_id = FrameId, week = 0, seconds = 0, latitude = location.Latitude, longitude = location.Longitude, height = location.Altitude, north_velocity = RigidBody.velocity.z, east_velocity = RigidBody.velocity.x, up_velocity = RigidBody.velocity.y, roll = transform.rotation.eulerAngles.x, pitch = transform.rotation.eulerAngles.z, azimuth = transform.rotation.eulerAngles.y, status = 3 }); // BESTPOS WriterBestPos.Write(new novatelBestPosData() { stamp = DateTime.Now, frame_id = FrameId, lat = location.Latitude, lon = location.Longitude, hgt = location.Altitude, sol_status = 0, sol_status_str = "SOL_COMPUTED", pos_type = 50, pos_type_str = "NARROW_INT", }); // RAWIMUX Vector3 deltaV = prev_velocity - RigidBody.velocity; prev_velocity = RigidBody.velocity; // Gyro Vector3 deltaA = RigidBody.angularVelocity; // IMU Status int imuStatus = 0; // Status Sequence Counter rawSeqNum++; // Device-Specific values switch (ImuType) { case ImuTypeEnum.IMU_KVH_1750: // LSB Scale Factors deltaV /= (float)(0.1 / (3600.0 * 256.0)); deltaA /= (float)(0.05 / Math.Pow(2, 15)); // Gyro & Accel Valid Status imuStatus |= 0x00000077; imuStatus |= (rawSeqNum << 8); imuStatus |= (ImuCelsius << 16); break; default: Debug.Log("Unknown Novatel IMU type: " + ImuType); break; } WriterRawImuX.Write(new novatelRawImuXData() { stamp = DateTime.Now, frame_id = FrameId, error = 0, type = (byte)ImuType, week = 0, seconds = 0, status = imuStatus, z_accel = (int)deltaV.y, y_accel_neg = -(int)deltaV.z, x_accel = (int)deltaV.x, z_gyro = -(int)deltaA.y, y_gyro_neg = -(int)deltaA.z, x_gyro = (int)deltaA.x, }); WriterCorrIMU.Write(new novatelCorrIMUData() { stamp = DateTime.Now, frame_id = FrameId, week = 0, seconds = 0, pitch_rate = (int)deltaA.x, roll_rate = (int)deltaA.z, yaw_rate = -(int)deltaA.y, lateral_acc = (int)deltaV.x, longitudinal_acc = (int)deltaV.z, vertical_acc = (int)deltaV.y, }); WriterHeading.Write(new novatelHeadingData() { stamp = DateTime.Now, frame_id = FrameId, sol_status = 0, sol_status_str = "0", pos_type = 0, pos_type_str = "0", length = 0, heading = 0, pitch = 0, heading_std_dev = 0, pitch_std_dev = 0, station_id = "0", num_svs = 0, num_sol_in_svs = 0, num_obs = 0, num_multi = 0, sol_source = 0, ext_sol_stat = 0, galileo_beidou_sig_mask = 0, gps_glonass_sig_mask = 0, }); // MARK3COUNT if (DateTime.Now > lastUpdateTimeMarkCount) { // Publish Frequency Limit (1Hz) lastUpdateTimeMarkCount = DateTime.Now.AddSeconds(1.0f); // Get Wheel Collider WheelCollider RR = Dynamics.axles[1].right; // Publish Message WriterMarkCount.Write(new novatelMarkCountData() { stamp = DateTime.Now, frame_id = FrameId, mark_num = 3, period = 1000000, count = (ushort)Math.Round(RR.rpm * EncoderRPMtoPPS), }); } }