void FixedUpdate()
{
// Publish Frequency Limit
if (lastUpdateTime.AddSeconds(1.0f / Frequency) > DateTime.Now)
{
return;
}
lastUpdateTime = DateTime.Now;
// Sanity Check
if (MapOrigin == null || Bridge == null || Bridge.Status != Status.Connected)
{
return;
}
// Get Data for Packing
float speed = RigidBody.velocity.magnitude;
var gps = MapOrigin.GetGpsLocation(transform.position);
if (creep_state == true)
{
creep_state = false;
}
else
{
temp_vehicle_speed = speed * 3.60f;
}
// Pack message data
msg = new ChassisData()
{
stamp = DateTime.Now,
frame_id = "",
steering_torque = 0.0f,
engine_rpm = Dynamics.CurrentRPM,
vehicle_speed = temp_vehicle_speed,
throttle_percent = Dynamics.AccellInput > 0 ? Dynamics.AccellInput : 0,
brake_percent = Dynamics.AccellInput < 0 ? -Dynamics.AccellInput : 0,
steering_angle = -Dynamics.CurrentSteerAngle * Dynamics.SteeringRatio,
brake_status = false,
cruise_start = false,
cruise_cancel = false,
takeover = false,
};
//speed during creep behaviour
if (msg.brake_percent == 0.0f && msg.vehicle_speed >= 7.92f && msg.throttle_percent == 0.0f)
{
creep_state = true;
msg.vehicle_speed = 2.2f * 3.60f;
temp_vehicle_speed = msg.vehicle_speed;
}
// Write Data to Message Queue
Writer.Write(msg, null);
}
public static Snowball.ChassisDataMsg ConvertFrom(ChassisData data)
{
return(new Snowball.ChassisDataMsg()
{
header = new Snowball.Header()
{
stamp = new Snowball.Time(data.stamp),
frame_id = data.frame_id
},
steering_torque = data.steering_torque,
engine_rpm = data.engine_rpm,
vehicle_speed = data.vehicle_speed,
throttle_percent = (float)data.throttle_percent,
brake_percent = (float)data.brake_percent,
steering_angle = (float)data.steering_angle,
brake_status = data.brake_status,
cruise_start = data.cruise_start,
cruise_cancel = data.cruise_cancel,
takeover = data.takeover,
});
}
