INNER CODE UNIT · Python

x

MOLAorg/mola · mola_demos/demos/fake_sensor_publisher.py:131

    x = _MOVING_V * t
    y = _MOVING_A * math.sin(_MOVING_OMEGA * t)
    vy = _MOVING_A * _MOVING_OMEGA * math.cos(_MOVING_OMEGA * t)
    yaw = math.atan2(vy, _MOVING_V)
    return x, y, 0.0, yaw, 0.0, 0.0


def _gt_moving_twist(t):
    vx_w = _MOVING_V
    vy_w = _MOVING_A * _MOVING_OMEGA * math.cos(_MOVING_OMEGA * t)
    _, _, _, yaw, _, _ = _gt_moving_pose(t)
    vx = vx_w * math.cos(yaw) + vy_w * math.sin(yaw)
    vy = -vx_w * math.sin(yaw) + vy_w * math.cos(yaw)
    denom = _MOVING_V ** 2 + vy_w ** 2
    wz = (-_MOVING_V * _MOVING_A * _MOVING_OMEGA ** 2 * math.sin(_MOVING_OMEGA * t)
          / max(denom, 1e-9))
    return vx, vy, 0.0, 0.0, 0.0, wz

View source record →