public static Lgsvl.CanBusDataRos RosConvertFrom(CanBusData data)
{
return(new Lgsvl.CanBusDataRos()
{
header = new Ros.Header()
{
stamp = ConvertTime(data.Time),
frame_id = data.Frame,
},
speed_mps = data.Speed,
throttle_pct = data.Throttle,
brake_pct = data.Braking,
steer_pct = data.Steering,
parking_brake_active = false, // parking brake is not supported in Simulator side
high_beams_active = data.HighBeamSignal,
low_beams_active = data.LowBeamSignal,
hazard_lights_active = data.HazardLights,
fog_lights_active = data.FogLights,
left_turn_signal_active = data.LeftTurnSignal,
right_turn_signal_active = data.RightTurnSignal,
wipers_active = data.Wipers,
reverse_gear_active = data.InReverse,
selected_gear = (data.InReverse ? Lgsvl.Gear.GEAR_REVERSE : Lgsvl.Gear.GEAR_DRIVE),
engine_active = data.EngineOn,
engine_rpm = data.EngineRPM,
gps_latitude = data.Latitude,
gps_longitude = data.Longitude,
gps_altitude = data.Altitude,
orientation = Convert(data.Orientation),
linear_velocities = ConvertToVector(data.Velocity),
});
}
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);
}
}
public static Apollo.ChassisMsg ConvertFrom(CanBusData data)
{
var eul = data.Orientation.eulerAngles;
float dir;
if (eul.y >= 0)
{
dir = 45 * UnityEngine.Mathf.Round((eul.y % 360) / 45.0f);
}
else
{
dir = 45 * UnityEngine.Mathf.Round((eul.y % 360 + 360) / 45.0f);
}
var dt = DateTimeOffset.FromUnixTimeMilliseconds((long)(data.Time * 1000.0)).UtcDateTime;
var measurement_time = (dt - GpsEpoch).TotalSeconds + 18.0;
var gpsTime = DateTimeOffset.FromUnixTimeSeconds((long)measurement_time).DateTime.ToLocalTime();
return(new Apollo.ChassisMsg()
{
header = new Apollo.Header()
{
timestamp_sec = data.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,
brake_percentage = data.Braking,
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.Chassis.DrivingMode.COMPLETE_AUTO_DRIVE,
gear_location = data.InReverse ? Apollo.Chassis.GearPosition.GEAR_REVERSE : Apollo.Chassis.GearPosition.GEAR_DRIVE,
chassis_gps = new Apollo.Chassis.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.Chassis.GpsQuality.FIX_3D,
num_satellites = 15,
gps_speed = data.Velocity.magnitude,
}
});
}
public static apollo.canbus.Chassis ConvertFrom(CanBusData data)
{
var orientation = ConvertToRfu(data.Orientation);
var eul = orientation.eulerAngles;
float dir;
if (eul.z >= 0)
{
dir = 45 * Mathf.Round((eul.z % 360) / 45.0f);
}
else
{
dir = 45 * Mathf.Round((eul.z % 360 + 360) / 45.0f);
}
var measurement_time = GpsUtils.UtcSecondsToGpsSeconds(data.Time);
var gpsTime = DateTimeOffset.FromUnixTimeSeconds((long)measurement_time).DateTime.ToLocalTime();
return(new apollo.canbus.Chassis()
{
header = new apollo.common.Header()
{
timestamp_sec = data.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.z,
hdop = 0.1,
vdop = 0.1,
quality = apollo.canbus.GpsQuality.Fix3d,
num_satellites = 15,
gps_speed = data.Velocity.magnitude,
}
});
}
public static VehicleStateReport ROS2ConvertFrom(CanBusData data)
{
// No fuel supported in Simulator side
byte fuel = 0;
// Blinker
// BLINKER_OFF = 0, BLINKER_LEFT = 1, BLINKER_RIGHT = 2, BLINKER_HAZARD = 3
// No Hazard Light in Simulator side
byte blinker = 0;
if (data.HazardLights)
{
blinker = 3;
}
else if (data.LeftTurnSignal)
{
blinker = 1;
}
else if (data.RightTurnSignal)
{
blinker = 2;
}
// Headlight
// HEADLIGHT_OFF = 0, HEADLIGHT_ON = 1, HEADLIGHT_HIGH = 2
byte headlight = 0;
if (data.LowBeamSignal)
{
headlight = 1;
}
else if (data.HighBeamSignal)
{
headlight = 2;
}
else
{
headlight = 0;
}
// Wiper
// WIPER_OFF = 0, WIPER_LOW = 1, WIPER_HIGH = 2, WIPER_CLEAN = 3
// No WIPER_HIGH and WIPER_CLEAN in Simulator side
byte wiper = 0;
if (data.Wipers)
{
wiper = 1;
}
// Gear
// GEAR_DRIVE = 0, GEAR_REVERSE = 1, GEAR_PARK = 2, GEAR_LOW = 3, GEAR_NEUTRAL = 4
// No GEAR_PARK, GEAR_LOW, GEAR_NEUTRAL in Simulator side
byte gear = 0;
if (data.InReverse)
{
gear = 1;
}
else
{
gear = 0;
}
// Mode
// No information about mode in Simulator side.
byte mode = 0;
// Hand Brake
bool handBrake = false;
// Horn
bool horn = false;
return(new VehicleStateReport()
{
stamp = ConvertTime(data.Time),
fuel = fuel,
blinker = blinker,
headlight = headlight,
wiper = wiper,
gear = gear,
mode = mode,
hand_brake = handBrake,
horn = horn,
});
}
public void Update()
{
if (MapOrigin == null)
{
return;
}
if (Time.time < NextSend)
{
return;
}
NextSend = Time.time + 1.0f / Frequency;
float speed = Dynamics.Speed;
MaxSpeed = Mathf.Max(MaxSpeed, speed);
var gps = MapOrigin.PositionToGpsLocation(transform.position);
var orientation = transform.rotation;
orientation.Set(-orientation.z, orientation.x, -orientation.y, orientation.w); // converting to right handed xyz
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 == HeadLightState.HIGH,
LowBeamSignal = Actions.CurrentHeadLightState == 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.CurrentIgnitionStatus == IgnitionStatus.On,
EngineRPM = Dynamics.CurrentRPM,
Latitude = gps.Latitude,
Longitude = gps.Longitude,
Altitude = gps.Altitude,
Orientation = orientation,
Velocity = Dynamics.Velocity,
};
if (Bridge != null && Bridge.Status == Status.Connected)
{
Publish(msg);
}
}
