void FixedUpdate()
{
float steerInput = input.SteerInput;
float accelInput = input.AccelBrakeInput;
var hasWorkingSteerwheel = (steerwheelInput != null && steerwheelInput.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;
}
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);
}
}