def _extrapolate_to_now(vs: VehicleState, staleness: float,
lin_acc_slope: float, ang_acc_slope: float):
"""Here we just linearly extrapolate the acceleration to "now" from the previous state for
each vehicle and then use standard kinematics to project the velocity and position from that."""
# The following ~10 lines are a hack b/c I'm too stupid to figure out
# how to do calculus on quaternions...
heading = vs.pose.heading
heading_delta_vec = staleness * (
vs.angular_velocity + 0.5 * vs.angular_acceleration * staleness +
ang_acc_slope * staleness**2 / 6.0)
heading += vec_to_radians(heading_delta_vec[:2]) + (0.5 * math.pi)
heading %= 2 * math.pi
vs.pose.orientation = fast_quaternion_from_angle(heading)
# XXX: also need to remove the cached heading_ since we've changed orientation
vs.pose.heading_ = None
# I assume the following should be updated based on changing
# heading from above, but I'll leave that for now...
vs.pose.position += staleness * (
vs.linear_velocity + 0.5 * vs.linear_acceleration * staleness +
lin_acc_slope * staleness**2 / 6.0)
vs.linear_velocity += staleness * (vs.linear_acceleration +
0.5 * lin_acc_slope * staleness)
vs.speed = np.linalg.norm(vs.linear_velocity)
vs.angular_velocity += staleness * (vs.angular_acceleration +
0.5 * ang_acc_slope * staleness)
vs.linear_acceleration += staleness * lin_acc_slope
vs.angular_acceleration += staleness * ang_acc_slope
def _entity_to_vs(entity: EntityState) -> VehicleState:
veh_id = entity.entity_id
veh_type = ROSDriver._decode_entity_type(entity.entity_type)
veh_dims = Dimensions(entity.length, entity.width, entity.height)
vs = VehicleState(
source="EXTERNAL",
vehicle_id=veh_id,
vehicle_config_type=veh_type,
pose=Pose(
ROSDriver._xyz_to_vect(entity.pose.position),
ROSDriver._xyzw_to_vect(entity.pose.orientation),
),
dimensions=veh_dims,
linear_velocity=ROSDriver._xyz_to_vect(entity.velocity.linear),
angular_velocity=ROSDriver._xyz_to_vect(entity.velocity.angular),
linear_acceleration=ROSDriver._xyz_to_vect(
entity.acceleration.linear),
angular_acceleration=ROSDriver._xyz_to_vect(
entity.acceleration.angular),
)
vs.set_privileged()
vs.speed = np.linalg.norm(vs.linear_velocity)
return vs
