#!/usr/bin/env python
import argparse
from drivebot.msg import TrainingExample
import json
import rospy
import sys
import time
import util as u

parser = argparse.ArgumentParser()
parser.add_argument('--training-eg-topic', default="/drivebot/training_egs", 
                    help="ros topic to publish TrainingExamples to")
parser.add_argument('--rate', type=float, default=0.0,
                    help="rate (hz) to publish. 0 => fast as possible")
opts = parser.parse_args()

training = rospy.Publisher(opts.training_eg_topic, TrainingExample, queue_size=200)
rospy.init_node('publish_events_to_topic')
rate = None if opts.rate <= 0 else rospy.Rate(opts.rate)

for line in sys.stdin:
    if rospy.is_shutdown():
        break
    event = json.loads(line)
    eg = u.training_eg_msg(event['state_1'], event['action'], event['reward'], event['state_2'])
    training.publish(eg)
    if rate is not None:
        rate.sleep()
                     
    
Exemple #2
0
import util as u

parser = argparse.ArgumentParser()
parser.add_argument('--training-eg-topic',
                    default="/drivebot/training_egs",
                    help="ros topic to publish TrainingExamples to")
parser.add_argument('--rate',
                    type=float,
                    default=0.0,
                    help="rate (hz) to publish. 0 => fast as possible")
opts = parser.parse_args()

training = rospy.Publisher(opts.training_eg_topic,
                           TrainingExample,
                           queue_size=200)
rospy.init_node('publish_events_to_topic')
rate = None if opts.rate <= 0 else rospy.Rate(opts.rate)

for line in sys.stdin:
    if rospy.is_shutdown():
        break
    event = json.loads(line)
    if 'action' in event:
        print > sys.stderr, "DEPRECATED event; expected 'discrete_action', not 'action'"
        event['discrete_action'] = event['action']
    eg = u.training_eg_msg(event['state_1'], event['discrete_action'],
                           event['reward'], event['state_2'])
    training.publish(eg)
    if rate is not None:
        rate.sleep()
Exemple #3
0
        # flush a single event to episode and train with it
        if last_state is not None:
            event = OrderedDict()
            event['bot_id'] = opts.robot_id
            event['epi_id'] = episode_id
            event['eve_id'] = event_id
            event['ranges_1'] = last_ranges
            event['state_1'] = last_state
            event['action'] = last_action
            event['reward'] = reward
            event['ranges_2'] = current_ranges
            event['state_2'] = current_state
            print "EVENT\tepi_id=%s eve_id=%s no_rewards_run_len=%s" % (episode_id, \
                    event_id, no_rewards_run_len)
            episode.append(event)
            training.publish(u.training_eg_msg(last_state, last_action, reward,
                                               current_state))
            event_id += 1

        # flush last_XYZ for next event
        last_ranges = current_ranges
        last_state = current_state
        last_action = action

        # let sim run...
        rate.sleep()

    # write episode to log
    print >>episode_log, json.dumps(episode)
    episode_log.flush()

steering.publish(Twist())  # shutdown movement of bot
Exemple #4
0
        # flush a single event to episode and train with it
        if last_state is not None:
            event = OrderedDict()
            event['bot_id'] = opts.robot_id
            event['epi_id'] = episode_id
            event['eve_id'] = event_id
            event['time'] = int(time.time())
            event['ranges_1'] = last_ranges
            event['state_1'] = last_state
            event['discrete_action'] = last_action
            event['reward'] = reward
            event['ranges_2'] = current_ranges
            event['state_2'] = current_state
            print "EVENT\te=%s\tno_rewards_run_len=%s" % (event, no_rewards_run_len)
            episode.append(event)
            training.publish(u.training_eg_msg(last_state, last_action, reward,
                                               current_state))
            event_id += 1

        # flush last_XYZ for next event
        last_ranges = current_ranges
        last_state = current_state
        last_action = action

        # let sim run...
        rate.sleep()

    # write episode to log. use json just to piss everyone off.
    print >>episode_log, json.dumps(episode)
    episode_log.flush()

steering.publish(Twist())  # shutdown movement of bot