예제 #1
0
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))
예제 #2
0
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))