INNER CODE UNIT · Python
_gt_static_twist
MOLAorg/mola · mola_demos/demos/fake_sensor_publisher.py:126
def _gt_static_twist(_t):
return 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
def _gt_moving_pose(t):
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)