public static Autoware.VehicleOdometry ROS2ConvertFrom(VehicleOdometryData data)
{
return(new Autoware.VehicleOdometry()
{
stamp = ConvertTime(data.Time),
velocity_mps = data.Speed,
front_wheel_angle_rad = UnityEngine.Mathf.Deg2Rad * data.SteeringAngleFront,
rear_wheel_angle_rad = UnityEngine.Mathf.Deg2Rad * data.SteeringAngleBack,
});
}
public static Lgsvl.VehicleOdometry ConvertFrom(VehicleOdometryData data)
{
return(new Lgsvl.VehicleOdometry()
{
header = new Ros.Header()
{
stamp = ConvertTime(data.Time),
},
velocity = data.Speed,
front_wheel_angle = UnityEngine.Mathf.Deg2Rad * data.SteeringAngleFront,
rear_wheel_angle = UnityEngine.Mathf.Deg2Rad * data.SteeringAngleBack,
});
}
public void Update()
{
// distance analysis
Distance += Vector3.Distance(transform.position, PrevPos) / 1000;
PrevPos = transform.position;
// traffic speed violation
float speed = Dynamics.Velocity.magnitude;
if (speed > Lane?.CurrentMapLane?.speedLimit)
{
SpeedViolationLane = Lane.CurrentMapLane;
SpeedViolationCount += Time.fixedDeltaTime;
UpdateMinMax(speed, ref SpeedViolationMin, ref SpeedViolationMax);
}
else
{
if (SpeedViolationCount > 0 && SpeedViolationLane != null)
{
SpeedViolationEvent(Controller.GTID, SpeedViolationLane);
}
SpeedViolationCount = 0f;
SpeedViolationMax = 0f;
SpeedViolationLane = null;
}
msg = new VehicleOdometryData()
{
Time = SimulatorManager.Instance.CurrentTime,
Speed = speed,
SteeringAngleFront = Dynamics.WheelAngle,
SteeringAngleBack = 0f,
};
if (Time.time < NextSend)
{
return;
}
NextSend = Time.time + 1.0f / Frequency;
if (Bridge != null && Bridge.Status == Status.Connected)
{
Publish(msg);
}
}
public void Update()
{
if (Time.time < NextSend)
{
return;
}
NextSend = Time.time + 1.0f / Frequency;
float speed = RigidBody.velocity.magnitude;
msg = new VehicleOdometryData()
{
Time = SimulatorManager.Instance.CurrentTime,
Speed = speed,
SteeringAngleFront = Dynamics.WheelAngle,
SteeringAngleBack = 0f,
};
if (Bridge != null && Bridge.Status == Status.Connected)
{
Publish(msg);
}
}
