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 FixedUpdate()
{
float steerInput = input.SteerInput;
float accelInput = input.AccelBrakeInput;
var hasWorkingSteerwheel = (steerwheelInput != null && SteeringWheelInputController.available);
if (!selfDriving || underKeyboardControl) //manual control or keyboard-interrupted self driving
{
//grab input values
if (!selfDriving)
{
steerInput = input.SteerInput;
accelInput = input.AccelBrakeInput;
}
else if (underKeyboardControl)
{
steerInput = keyboardInput.SteerInput;
accelInput = keyboardInput.AccelBrakeInput;
}
if (underKeyboardControl || underSteeringWheelControl)
{
float k = 0.4f + 0.6f * controller.CurrentSpeed / 30.0f;
accelInput = accelInput < 0.0f ? accelInput : accelInput *Mathf.Min(1.0f, k);
}
else
{
accelInput = -1;
steerInput = 0;
}
if (hasWorkingSteerwheel)
{
steerwheelInput.SetAutonomousForce(0);
}
}
else if (selfDriving && !underKeyboardControl) //autonomous control(uninterrupted)
{
if (hasWorkingSteerwheel)
{
var diff = Mathf.Abs(autoSteerAngle - steerInput);
if (autoSteerAngle < steerInput) // need to steer left
{
steerwheelInput.SetAutonomousForce((int)(diff * 10000));
}
else
{
steerwheelInput.SetAutonomousForce((int)(-diff * 10000));
}
}
//use autonomous command values
if (!hasWorkingSteerwheel || steerwheelInput.autonomousBehavior == SteerWheelAutonomousFeedbackBehavior.OutputOnly || steerwheelInput.autonomousBehavior == SteerWheelAutonomousFeedbackBehavior.None)
{
accelInput = autoInputAccel;
steerInput = autoSteerAngle;
}
else
{
//purpose of this is to use steering wheel as input even when in self-driving state
accelInput += autoInputAccel;
steerInput += autoSteerAngle;
}
}
if (controller.driveMode == DriveMode.Controlled)
{
controller.accellInput = accelInput;
}
controller.steerInput = steerInput;
if (isControlEnabled == true) // Publish control command for training
{
var simControl = 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.accellInput,
y = 0,
z = 0,
},
angular = new Ros.Vector3()
{
x = controller.steerInput,
y = 0,
z = 0,
}
}
};
LgsvlSimulatorCmdWriter.Publish(simControl);
}
}
