public static Apollo.Gps ApolloConvertFrom(GpsOdometryData data)
{
var angles = data.Orientation.eulerAngles;
float yaw = -angles.y;
var dt = DateTimeOffset.FromUnixTimeMilliseconds((long)(data.Time * 1000.0)).UtcDateTime;
return(new Apollo.Gps()
{
header = new Apollo.Header()
{
timestamp_sec = GpsUtils.UtcToGpsSeconds(dt),
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(data.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 ??
}
});
}
public static apollo.drivers.gnss.GnssBestPose ConvertFrom(GpsData data)
{
float Accuracy = 0.01f; // just a number to report
double Height = 0; // sea level to WGS84 ellipsoid
var dt = DateTimeOffset.FromUnixTimeMilliseconds((long)(data.Time * 1000.0)).UtcDateTime;
var measurement_time = GpsUtils.UtcToGpsSeconds(dt);
return(new apollo.drivers.gnss.GnssBestPose()
{
header = new apollo.common.Header()
{
sequence_num = data.Sequence,
frame_id = data.Frame,
timestamp_sec = measurement_time,
},
measurement_time = measurement_time,
sol_status = apollo.drivers.gnss.SolutionStatus.SolComputed,
sol_type = apollo.drivers.gnss.SolutionType.NarrowInt,
latitude = data.Latitude, // in degrees
longitude = data.Longitude, // in degrees
height_msl = Height, // height above mean sea level in meters
undulation = 0, // undulation = height_wgs84 - height_msl
datum_id = apollo.drivers.gnss.DatumId.Wgs84, // datum id number
latitude_std_dev = Accuracy, // latitude standard deviation (m)
longitude_std_dev = Accuracy, // longitude standard deviation (m)
height_std_dev = Accuracy, // height standard deviation (m)
base_station_id = Encoding.UTF8.GetBytes("0"), //CopyFrom((byte)"0"), // base station id
differential_age = 2.0f, // differential position age (sec)
solution_age = 0.0f, // solution age (sec)
num_sats_tracked = 15, // number of satellites tracked
num_sats_in_solution = 15, // number of satellites used in solution
num_sats_l1 = 15, // number of L1/E1/B1 satellites used in solution
num_sats_multi = 12, // number of multi-frequency satellites used in solution
extended_solution_status = 33, // extended solution status - OEMV and greater only
galileo_beidou_used_mask = 0,
gps_glonass_used_mask = 51
});
}
public static Apollo.GnssBestPose ConvertFrom(GpsData data)
{
float Accuracy = 0.01f; // just a number to report
double Height = 0; // sea level to WGS84 ellipsoid
var dt = DateTimeOffset.FromUnixTimeMilliseconds((long)(data.Time * 1000.0)).UtcDateTime;
var measurement_time = GpsUtils.UtcToGpsSeconds(dt);
return(new Apollo.GnssBestPose()
{
header = new Apollo.Header()
{
timestamp_sec = measurement_time,
sequence_num = data.Sequence++,
},
measurement_time = measurement_time,
sol_status = 0,
sol_type = 50,
latitude = data.Latitude,
longitude = data.Longitude,
height_msl = Height,
undulation = 0,
datum_id = 61, // datum id number
latitude_std_dev = Accuracy, // latitude standard deviation (m)
longitude_std_dev = Accuracy, // longitude standard deviation (m)
height_std_dev = Accuracy, // height standard deviation (m)
base_station_id = "0", // base station id
differential_age = 2.0f, // differential position age (sec)
solution_age = 0.0f, // solution age (sec)
num_sats_tracked = 15, // number of satellites tracked
num_sats_in_solution = 15, // number of satellites used in solution
num_sats_l1 = 15, // number of L1/E1/B1 satellites used in solution
num_sats_multi = 12, // number of multi-frequency satellites used in solution
extended_solution_status = 33, // extended solution status - OEMV and greater only
galileo_beidou_used_mask = 0,
gps_glonass_used_mask = 51
});
}
public static apollo.canbus.Chassis ConvertFrom(CanBusData data)
{
var eul = data.Orientation.eulerAngles;
float dir;
if (eul.y >= 0)
{
dir = 45 * Mathf.Round((eul.y % 360) / 45.0f);
}
else
{
dir = 45 * Mathf.Round((eul.y % 360 + 360) / 45.0f);
}
var dt = DateTimeOffset.FromUnixTimeMilliseconds((long)(data.Time * 1000.0)).UtcDateTime;
var measurement_time = GpsUtils.UtcToGpsSeconds(dt);
var gpsTime = DateTimeOffset.FromUnixTimeSeconds((long)measurement_time).DateTime.ToLocalTime();
return(new apollo.canbus.Chassis()
{
header = new apollo.common.Header()
{
timestamp_sec = measurement_time,
module_name = "chassis",
sequence_num = data.Sequence,
},
engine_started = data.EngineOn,
engine_rpm = data.EngineRPM,
speed_mps = data.Speed,
odometer_m = 0,
fuel_range_m = 0,
throttle_percentage = data.Throttle * 100.0f,
brake_percentage = data.Braking * 100.0f,
steering_percentage = -data.Steering * 100,
parking_brake = data.ParkingBrake,
//high_beam_signal = data.HighBeamSignal,
//low_beam_signal = data.LowBeamSignal,
//left_turn_signal = data.LeftTurnSignal,
//right_turn_signal = data.RightTurnSignal,
wiper = data.Wipers,
driving_mode = apollo.canbus.Chassis.DrivingMode.CompleteAutoDrive,
gear_location = data.InReverse ? apollo.canbus.Chassis.GearPosition.GearReverse : apollo.canbus.Chassis.GearPosition.GearDrive,
chassis_gps = new apollo.canbus.ChassisGPS()
{
latitude = data.Latitude,
longitude = data.Longitude,
gps_valid = true,
year = gpsTime.Year,
month = gpsTime.Month,
day = gpsTime.Day,
hours = gpsTime.Hour,
minutes = gpsTime.Minute,
seconds = gpsTime.Second,
compass_direction = dir,
pdop = 0.1,
is_gps_fault = false,
is_inferred = false,
altitude = data.Altitude,
heading = eul.y,
hdop = 0.1,
vdop = 0.1,
quality = apollo.canbus.GpsQuality.Fix3d,
num_satellites = 15,
gps_speed = data.Velocity.magnitude,
}
});
}