def reconstruct_servo_state_nomap(id_explog, id_robot, out_base, goal_at):
""" reconstruct the servo state for navigation logs """
explog = get_conftools_explogs().instance(id_explog)
bag = explog.get_bagfile()
# get a list of state transitions
transitions = reconstruct(bag, topic='/youbot_safety/in_cmd_vel')
print(transitions)
robot = get_conftools_robots().instance(id_robot)
orig_robot = robot.get_inner_components()[-1]
if not isinstance(orig_robot, ROSRobot):
msg = 'Expected ROSRobot, got %s' % describe_type(robot)
raise ValueError(msg)
orig_robot.read_from_log(explog)
def read_data():
goal = None
t0 = None
servo_state = STATE_WAIT
for obs in iterate_robot_observations(robot, sleep=0):
timestamp = obs.timestamp
if t0 is None:
t0 = timestamp
observations = obs.observations
if goal is None and timestamp - t0 > goal_at:
goal = observations
commands = obs.commands
# if still have transitions
if transitions:
next_transition = transitions[0][0]
if timestamp > next_transition:
servo_state = transitions[0][1]
transitions.pop(0)
# if np.linalg.norm(commands) == 0:
# servo_state = STATE_WAIT
# else:
# servo_state = STATE_SERVOING
#
# n = np.sum(commands != 0)
# print "%20s" % servo_state + "n: %5s " % n + ",".join('%+10.6f' % s for s in commands)
# goal = controller.get_current_goal()
yield 'y', timestamp, observations
yield 'servo_state', timestamp, servo_state
yield 'commands', timestamp, commands
if goal is None:
yield 'y_goal', timestamp, observations
else:
yield 'y_goal', timestamp, goal
iterator = read_data()
pg('read_reconstructed_data',
config=dict(iterator=iterator, out_base=out_base))
def safe_pickle_dump(value, filename, protocol=pickle.HIGHEST_PROTOCOL):
# TODO: add debug
with safe_write(filename) as f:
try:
pickle.dump(value, f, protocol)
except Exception:
# TODO: add debug check
msg = 'Cannot pickle object of class %s' % describe_type(value)
logger.error(msg)
raise
def reconstruct_servo_state(id_explog, id_robot, nmap, out_base,
navigation_params):
""" reconstruct the servo state for navigation logs """
explog = get_conftools_explogs().instance(id_explog)
robot = get_conftools_robots().instance(id_robot)
orig_robot = robot.get_inner_components()[-1]
if not isinstance(orig_robot, ROSRobot):
msg = 'Expected ROSRobot, got %s' % describe_type(robot)
raise ValueError(msg)
orig_robot.read_from_log(explog)
controller = NavigationController(nmap, **navigation_params)
def read_data():
servo_state = None
for obs in iterate_robot_observations(robot, sleep=0):
timestamp = obs.timestamp
observations = obs.observations
commands = obs.commands
if servo_state != STATE_SERVOING and np.linalg.norm(commands) == 0:
servo_state = STATE_WAIT
else:
servo_state = STATE_SERVOING
controller.process_observations(observations)
goal = controller.get_current_goal()
yield 'y', timestamp, observations
yield 'servo_state', timestamp, servo_state
yield 'commands', timestamp, commands
yield 'y_goal', timestamp, goal
iterator = read_data()
pg('read_reconstructed_data',
config=dict(iterator=iterator, out_base=out_base))