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)(() => Writer.Write(data))));
}
}
public static Apollo.Gps ApolloConvertFrom(GpsOdometryData data)
{
var angles = data.Orientation.eulerAngles;
float roll = angles.z;
float pitch = angles.x;
float yaw = -angles.y;
var q = UnityEngine.Quaternion.Euler(pitch, roll, yaw);
var dt = DateTimeOffset.FromUnixTimeMilliseconds((long)(data.Time * 1000.0)).UtcDateTime;
return(new Apollo.Gps()
{
header = new Apollo.Header()
{
timestamp_sec = (dt - GpsEpoch).TotalSeconds + 18.0,
sequence_num = data.Sequence,
},
localization = new Apollo.Pose()
{
// Position of the vehicle reference point (VRP) in the map reference frame.
// The VRP is the center of rear axle.
position = new Apollo.PointENU()
{
x = data.Easting, // East from the origin, in meters.
y = data.Northing, // North from the origin, in meters.
z = data.Altitude // Up from the WGS-84 ellipsoid, in meters.
},
// A quaternion that represents the rotation from the IMU coordinate
// (Right/Forward/Up) to the world coordinate (East/North/Up).
orientation = new Apollo.Quaternion()
{
qx = q.x,
qy = q.y,
qz = q.z,
qw = q.w,
},
// Linear velocity of the VRP in the map reference frame.
// East/north/up in meters per second.
linear_velocity = new Apollo.Point3D()
{
x = data.Velocity.x,
y = data.Velocity.z,
z = data.Velocity.y,
},
// The heading is zero when the car is facing East and positive when facing North.
heading = yaw, // not used ??
}
});
}
public static VehicleOdometry ROS2ConvertFrom(GpsOdometryData data)
{
var angles = data.Orientation.eulerAngles;
float yaw = -angles.y;
return(new VehicleOdometry()
{
stamp = ConvertTime(data.Time),
velocity_mps = data.ForwardSpeed,
front_wheel_angle_rad = data.WheelAngle,
rear_wheel_angle_rad = .0f,
});
}
public static apollo.localization.Gps ConvertFrom(GpsOdometryData data)
{
var angles = data.Orientation.eulerAngles;
float roll = angles.z;
float pitch = angles.x;
float yaw = -angles.y;
var q = Quaternion.Euler(pitch, roll, yaw);
return(new apollo.localization.Gps()
{
header = new apollo.common.Header()
{
timestamp_sec = data.Time,
sequence_num = data.Sequence,
},
localization = new apollo.localization.Pose()
{
position = new apollo.common.PointENU()
{
x = data.Easting + 500000, // East from the origin, in meters.
y = data.Northing, // North from the origin, in meters.
z = data.Altitude // Up from the WGS-84 ellipsoid, in meters.
},
// A quaternion that represents the rotation from the IMU coordinate
// (Right/Forward/Up) to the world coordinate (East/North/Up).
orientation = new apollo.common.Quaternion()
{
qx = q.x,
qy = q.y,
qz = q.z,
qw = q.w,
},
// Linear velocity of the VRP in the map reference frame.
// East/north/up in meters per second.
linear_velocity = new apollo.common.Point3D()
{
x = data.Velocity.x,
y = data.Velocity.z,
z = data.Velocity.y,
},
// The heading is zero when the car is facing East and positive when facing North.
heading = yaw, // not used ??
}
});
}
public static Odometry ConvertFrom(GpsOdometryData data)
{
return(new Odometry()
{
header = new Header()
{
stamp = ConvertTime(data.Time),
seq = data.Sequence,
frame_id = data.Frame,
},
child_frame_id = "base_link",
pose = new PoseWithCovariance()
{
pose = new Pose()
{
position = new Point()
{
x = data.Easting + (data.IgnoreMapOrigin ? -500000 : 0),
y = data.Northing,
z = data.Altitude,
},
orientation = Convert(data.Orientation),
}
},
twist = new TwistWithCovariance()
{
twist = new Twist()
{
linear = new Vector3()
{
x = data.ForwardSpeed,
y = 0.0,
z = 0.0,
},
angular = new Vector3()
{
x = 0.0,
y = 0.0,
z = -data.AngularVelocity.y,
}
},
}
});
}
public static Apollo.Gps ApolloConvertFrom(GpsOdometryData data)
{
var orientation = ConvertToRfu(data.Orientation);
float yaw = orientation.eulerAngles.z;
return(new Apollo.Gps()
{
header = new Apollo.Header()
{
timestamp_sec = GpsUtils.UtcSecondsToGpsSeconds(data.Time),
sequence_num = data.Sequence,
},
localization = new Apollo.Pose()
{
// Position of the vehicle reference point (VRP) in the map reference frame.
// The VRP is the center of rear axle.
position = new Apollo.PointENU()
{
x = data.Easting, // East from the origin, in meters.
y = data.Northing, // North from the origin, in meters.
z = data.Altitude // Up from the WGS-84 ellipsoid, in meters.
},
// A quaternion that represents the rotation from the IMU coordinate
// (Right/Forward/Up) to the world coordinate (East/North/Up).
orientation = ConvertApolloQuaternion(orientation),
// Linear velocity of the VRP in the map reference frame.
// East/north/up in meters per second.
linear_velocity = new Apollo.Point3D()
{
x = data.Velocity.x,
y = data.Velocity.z,
z = data.Velocity.y,
},
// The heading is zero when the car is facing East and positive when facing North.
heading = yaw, // not used ??
}
});
}
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))));
}
}
public static Ros.Odometry ConvertFrom(GpsOdometryData data)
{
return(new Ros.Odometry()
{
header = new Ros.Header()
{
stamp = ConvertTime(data.Time),
seq = data.Sequence,
frame_id = data.Frame,
},
child_frame_id = data.ChildFrame,
pose = new Ros.PoseWithCovariance()
{
pose = new Ros.Pose()
{
position = new Ros.Point()
{
x = data.Easting,
y = data.Northing,
z = data.Altitude,
},
orientation = Convert(data.Orientation),
},
covariance = new double[]
{
0.0001, 0, 0, 0, 0, 0,
0, 0.0001, 0, 0, 0, 0,
0, 0, 0.0001, 0, 0, 0,
0, 0, 0, 0.0001, 0, 0,
0, 0, 0, 0, 0.0001, 0,
0, 0, 0, 0, 0, 0.0001
}
},
twist = new Ros.TwistWithCovariance()
{
twist = new Ros.Twist()
{
linear = new Ros.Vector3()
{
x = data.ForwardSpeed,
y = 0.0,
z = 0.0,
},
angular = new Ros.Vector3()
{
x = 0.0,
y = 0.0,
z = -data.AngularVelocity.y,
}
},
covariance = new double[]
{
0.0001, 0, 0, 0, 0, 0,
0, 0.0001, 0, 0, 0, 0,
0, 0, 0.0001, 0, 0, 0,
0, 0, 0, 0.0001, 0, 0,
0, 0, 0, 0, 0.0001, 0,
0, 0, 0, 0, 0, 0.0001
}
}
});
}
