test debuging

import pytest

import numpy as np
from pyrobot import Robot




bot = Robot('locobot', base_config={'base_controller': 'movebase',
                                  'base_planner': 'movebase'},
            use_arm=False, use_camera=False)
state_before = np.array(bot.base.get_state('odom'))
bot.base.set_vel(fwd_speed=0.2, turn_speed=0.0, exe_time=2)
bot.base.stop()
state_after = np.array(bot.base.get_state('odom'))
dist = np.linalg.norm(state_after[:2] - state_before[:2])
assert 0.3 < dist < 0.5

state_before = state_after
bot.base.set_vel(fwd_speed=0.0, turn_speed=0.5, exe_time=2)
bot.base.stop()
state_after = np.array(bot.base.get_state('odom'))
dist = np.linalg.norm(state_after[:2] - state_before[:2])
dt = state_after[2] - state_before[2]
dt = np.mod(dt + np.pi, 2 * np.pi) - np.pi
assert dist < 0.1
assert 0.75 < dt < 1.25