void Update()
{
if (targetEnv != ROSTargetEnvironment.APOLLO && targetEnv != ROSTargetEnvironment.APOLLO35 && targetEnv != ROSTargetEnvironment.AUTOWARE)
{
return;
}
if (Bridge == null || Bridge.Status != Comm.BridgeStatus.Connected || !PublishMessage)
{
return;
}
if (Time.time < NextSend)
{
return;
}
if (targetEnv == ROSTargetEnvironment.APOLLO)
{
Vector3 vel = mainRigidbody.velocity;
Vector3 eul = mainRigidbody.rotation.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);
}
// (TODO) check for leap second issues.
var gps_time = DateTimeOffset.FromUnixTimeSeconds((long)gps.measurement_time).DateTime.ToLocalTime();
var apolloMessage = new Ros.Apollo.ChassisMsg()
{
engine_started = true,
engine_rpm = controller.currentRPM,
speed_mps = vel.magnitude,
odometer_m = 0,
fuel_range_m = 0,
throttle_percentage = input_controller.throttle * 100,
brake_percentage = input_controller.brake * 100,
steering_percentage = -controller.steerInput * 100,
// steering_torque_nm
parking_brake = controller.handbrakeApplied,
high_beam_signal = (controller.headlightMode == 2),
low_beam_signal = (controller.headlightMode == 1),
left_turn_signal = controller.leftTurnSignal,
right_turn_signal = controller.rightTurnSignal,
// horn
wiper = (controller.wiperStatus != 0),
// disengage_status
driving_mode = Ros.Apollo.Chassis.DrivingMode.COMPLETE_AUTO_DRIVE,
// error_code
// gear_location
// steering_timestamp
// signal
// engage_advice
chassis_gps = new Ros.Apollo.Chassis.ChassisGPS()
{
latitude = gps.latitude,
longitude = gps.longitude,
gps_valid = gps.PublishMessage,
year = gps_time.Year,
month = gps_time.Month,
day = gps_time.Day,
hours = gps_time.Hour,
minutes = gps_time.Minute,
seconds = gps_time.Second,
compass_direction = dir,
pdop = 0.1,
is_gps_fault = false,
is_inferred = false,
altitude = gps.height,
heading = eul.y,
hdop = 0.1,
vdop = 0.1,
quality = Ros.Apollo.Chassis.GpsQuality.FIX_3D,
num_satellites = 15,
gps_speed = vel.magnitude,
},
header = new Ros.ApolloHeader()
{
timestamp_sec = Ros.Time.Now().secs,
module_name = "chassis",
sequence_num = seq,
},
};
ApolloChassisWriter.Publish(apolloMessage);
}
else if (targetEnv == ROSTargetEnvironment.APOLLO35)
{
Vector3 vel = mainRigidbody.velocity;
Vector3 eul = mainRigidbody.rotation.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);
}
// (TODO) check for leap second issues.
var gps_time = DateTimeOffset.FromUnixTimeSeconds((long)gps.measurement_time).DateTime.ToLocalTime();
System.DateTime Unixepoch = new System.DateTime(1970, 1, 1, 0, 0, 0, System.DateTimeKind.Utc);
double measurement_time = (double)(System.DateTime.UtcNow - Unixepoch).TotalSeconds;
var apolloMessage = new Apollo.Canbus.Chassis()
{
EngineStarted = true,
EngineRpm = controller.currentRPM,
SpeedMps = vel.magnitude,
OdometerM = 0,
FuelRangeM = 0,
ThrottlePercentage = input_controller.throttle * 100,
BrakePercentage = input_controller.brake * 100,
SteeringPercentage = -controller.steerInput * 100,
// steering_torque_nm
ParkingBrake = controller.handbrakeApplied,
HighBeamSignal = (controller.headlightMode == 2),
LowBeamSignal = (controller.headlightMode == 1),
LeftTurnSignal = controller.leftTurnSignal,
RightTurnSignal = controller.rightTurnSignal,
// horn
Wiper = (controller.wiperStatus != 0),
// disengage_status
DrivingMode = Apollo.Canbus.Chassis.Types.DrivingMode.CompleteAutoDrive,
// error_code
// gear_location
// steering_timestamp
// signal
// engage_advice
ChassisGps = new Apollo.Canbus.ChassisGPS()
{
Latitude = gps.latitude,
Longitude = gps.longitude,
GpsValid = gps.PublishMessage,
Year = gps_time.Year,
Month = gps_time.Month,
Day = gps_time.Day,
Hours = gps_time.Hour,
Minutes = gps_time.Minute,
Seconds = gps_time.Second,
CompassDirection = dir,
Pdop = 0.1,
IsGpsFault = false,
IsInferred = false,
Altitude = gps.height,
Heading = eul.y,
Hdop = 0.1,
Vdop = 0.1,
Quality = Apollo.Canbus.GpsQuality.Fix3D,
NumSatellites = 15,
GpsSpeed = vel.magnitude,
},
Header = new Apollo.Common.Header()
{
TimestampSec = measurement_time,
ModuleName = "chassis",
SequenceNum = seq,
},
};
Apollo35ChassisWriter.Publish(apolloMessage);
}
}
void Update()
{
if (targetEnv != ROSTargetEnvironment.APOLLO && targetEnv != ROSTargetEnvironment.AUTOWARE && targetEnv != ROSTargetEnvironment.LGSVL)
{
return;
}
if (Bridge == null || Bridge.Status != Ros.Status.Connected || !PublishMessage)
{
return;
}
if (Time.time < NextSend)
{
return;
}
if (targetEnv == ROSTargetEnvironment.APOLLO)
{
Vector3 vel = mainRigidbody.velocity;
Vector3 eul = mainRigidbody.rotation.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);
}
// (TODO) check for leap second issues.
var gps_time = DateTimeOffset.FromUnixTimeSeconds((long)gps.measurement_time).DateTime.ToLocalTime();
var apolloMessage = new Ros.Apollo.ChassisMsg()
{
engine_started = true,
engine_rpm = controller.currentRPM,
speed_mps = vel.magnitude,
odometer_m = 0,
fuel_range_m = 0,
throttle_percentage = input_controller.throttle * 100,
brake_percentage = input_controller.brake * 100,
steering_percentage = -controller.steerInput * 100,
// steering_torque_nm
parking_brake = controller.handbrakeApplied,
high_beam_signal = (controller.headlightMode == 2),
low_beam_signal = (controller.headlightMode == 1),
left_turn_signal = controller.leftTurnSignal,
right_turn_signal = controller.rightTurnSignal,
// horn
wiper = (controller.wiperStatus != 0),
// disengage_status
driving_mode = Ros.Apollo.Chassis.DrivingMode.COMPLETE_AUTO_DRIVE,
// error_code
// gear_location
// steering_timestamp
// signal
// engage_advice
chassis_gps = new Ros.Apollo.Chassis.ChassisGPS()
{
latitude = gps.latitude_orig,
longitude = gps.longitude_orig,
gps_valid = gps.PublishMessage,
year = gps_time.Year,
month = gps_time.Month,
day = gps_time.Day,
hours = gps_time.Hour,
minutes = gps_time.Minute,
seconds = gps_time.Second,
compass_direction = dir,
pdop = 0.1,
is_gps_fault = false,
is_inferred = false,
altitude = gps.height,
heading = eul.y,
hdop = 0.1,
vdop = 0.1,
quality = Ros.Apollo.Chassis.GpsQuality.FIX_3D,
num_satellites = 15,
gps_speed = vel.magnitude,
},
header = new Ros.ApolloHeader()
{
timestamp_sec = Ros.Time.Now().secs,
module_name = "chassis",
sequence_num = seq,
},
};
Bridge.Publish(ApolloTopic, apolloMessage);
}
if (targetEnv == ROSTargetEnvironment.LGSVL)
{
var simulatorMessage = new Ros.TwistStamped()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = seq++,
frame_id = "",
},
twist = new Ros.Twist()
{
linear = new Ros.Vector3()
{
x = controller.steerInput,
y = 0,
z = 0,
},
angular = new Ros.Vector3()
{
x = 0,
y = 0,
z = 0,
},
},
};
Bridge.Publish(SimulatorTopic, simulatorMessage);
}
}
void Update()
{
if (targetEnv != ROSTargetEnvironment.APOLLO && targetEnv != ROSTargetEnvironment.APOLLO35 && targetEnv != ROSTargetEnvironment.AUTOWARE)
{
return;
}
if (Bridge == null || Bridge.Status != Comm.BridgeStatus.Connected || !PublishMessage)
{
return;
}
if (Time.time < NextSend)
{
return;
}
NextSend = Time.time + 1.0f / Frequency;
if (targetEnv == ROSTargetEnvironment.APOLLO)
{
Vector3 vel = mainRigidbody.velocity;
Vector3 eul = mainRigidbody.rotation.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);
}
// (TODO) check for leap second issues.
var gps_time = DateTimeOffset.FromUnixTimeSeconds((long)gps.measurement_time).DateTime.ToLocalTime();
float accel = input_controller.controller.accellInput * 100;
var apolloMessage = new Ros.Apollo.ChassisMsg()
{
engine_started = true,
engine_rpm = controller.currentRPM,
speed_mps = vel.magnitude,
odometer_m = 0,
fuel_range_m = 0,
throttle_percentage = accel > 0 ? accel : 0,
brake_percentage = accel < 0 ? -accel : 0,
steering_percentage = -controller.steerInput * 100,
// steering_torque_nm
parking_brake = controller.handbrakeApplied,
high_beam_signal = (controller.headlightMode == 2),
low_beam_signal = (controller.headlightMode == 1),
left_turn_signal = controller.leftTurnSignal,
right_turn_signal = controller.rightTurnSignal,
// horn
wiper = (controller.wiperStatus != 0),
// disengage_status
driving_mode = Ros.Apollo.Chassis.DrivingMode.COMPLETE_AUTO_DRIVE,
// error_code
gear_location = controller.InReverse ? Ros.Apollo.Chassis.GearPosition.GEAR_REVERSE : Ros.Apollo.Chassis.GearPosition.GEAR_DRIVE,
// steering_timestamp
// signal
// engage_advice
chassis_gps = new Ros.Apollo.Chassis.ChassisGPS()
{
latitude = gps.latitude,
longitude = gps.longitude,
gps_valid = gps.PublishMessage,
year = gps_time.Year,
month = gps_time.Month,
day = gps_time.Day,
hours = gps_time.Hour,
minutes = gps_time.Minute,
seconds = gps_time.Second,
compass_direction = dir,
pdop = 0.1,
is_gps_fault = false,
is_inferred = false,
altitude = gps.height,
heading = eul.y,
hdop = 0.1,
vdop = 0.1,
quality = Ros.Apollo.Chassis.GpsQuality.FIX_3D,
num_satellites = 15,
gps_speed = vel.magnitude,
},
header = new Ros.ApolloHeader()
{
timestamp_sec = Ros.Time.Now().secs,
module_name = "chassis",
sequence_num = seq,
},
};
ApolloChassisWriter.Publish(apolloMessage);
}
else if (targetEnv == ROSTargetEnvironment.APOLLO35)
{
Vector3 vel = mainRigidbody.velocity;
Vector3 eul = mainRigidbody.rotation.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);
}
// (TODO) check for leap second issues.
var gps_time = DateTimeOffset.FromUnixTimeSeconds((long)gps.measurement_time).DateTime.ToLocalTime();
System.DateTime Unixepoch = new System.DateTime(1970, 1, 1, 0, 0, 0, System.DateTimeKind.Utc);
double measurement_time = (double)(System.DateTime.UtcNow - Unixepoch).TotalSeconds;
float accel = input_controller.controller.accellInput * 100;
var apolloMessage = new apollo.canbus.Chassis()
{
engine_started = true,
engine_rpm = controller.currentRPM,
speed_mps = vel.magnitude,
odometer_m = 0,
fuel_range_m = 0,
throttle_percentage = accel > 0 ? accel : 0,
brake_percentage = accel < 0 ? -accel : 0,
steering_percentage = -controller.steerInput * 100,
// steering_torque_nm
parking_brake = controller.handbrakeApplied,
high_beam_signal = (controller.headlightMode == 2),
low_beam_signal = (controller.headlightMode == 1),
left_turn_signal = controller.leftTurnSignal,
right_turn_signal = controller.rightTurnSignal,
// horn
wiper = (controller.wiperStatus != 0),
// disengage_status
driving_mode = apollo.canbus.Chassis.DrivingMode.COMPLETE_AUTO_DRIVE,
// error_code
gear_location = controller.InReverse ? apollo.canbus.Chassis.GearPosition.GEAR_REVERSE : apollo.canbus.Chassis.GearPosition.GEAR_DRIVE,
// steering_timestamp
// signal
// engage_advice
chassis_gps = new apollo.canbus.ChassisGPS()
{
latitude = gps.latitude,
longitude = gps.longitude,
gps_valid = gps.PublishMessage,
year = gps_time.Year,
month = gps_time.Month,
day = gps_time.Day,
hours = gps_time.Hour,
minutes = gps_time.Minute,
seconds = gps_time.Second,
compass_direction = dir,
pdop = 0.1,
is_gps_fault = false,
is_inferred = false,
altitude = gps.height,
heading = eul.y,
hdop = 0.1,
vdop = 0.1,
quality = apollo.canbus.GpsQuality.FIX_3D,
num_satellites = 15,
gps_speed = vel.magnitude,
},
header = new apollo.common.Header()
{
timestamp_sec = measurement_time,
module_name = "chassis",
sequence_num = seq,
},
};
Apollo35ChassisWriter.Publish(apolloMessage);
}
}