def robot_adapter():
rospy.init_node('robot_adapter')
params = rospy.get_param('~')
rospy.loginfo('My params:\n%s' % pformat(params))
required = ['code']
for req in required:
if not req in params:
msg = 'Necessary parameter %r not given.' % req
msg += '\nGiven configuration:\n%s' % pformat(params)
raise Exception(msg)
Global.dt = params.get('dt', 0.1)
sleep = params.get('sleep', 0.0)
code = params['code']
rospy.loginfo('Using code: %s' % pformat(code))
check_valid_code_spec(code)
try:
Global.robot = instantiate_spec(code)
except:
msg = 'Could not instantiate robot code, using:\n'
msg += ' class name: %s\n' % code[0]
msg += ' parameters: %s\n' % code[1]
rospy.logerr(msg)
raise
publisher = rospy.Publisher('~observations',
BootstrappingObservations, latch=True)
# Reset simulation
Global.robot.new_episode()
# Start service
service = rospy.Service('~commands',
BootstrappingCommands, commands_request)
# Broadcast observations anyway every few seconds
last_observations_sent = 0
maximum_interval = 2
while not rospy.is_shutdown():
now = time.time()
publish = False
if Global.publish:
Global.publish = False
publish = True
if now > last_observations_sent + maximum_interval:
last_observations_sent = now
publish = True
if publish:
publish_observations(Global.robot, publisher)
rospy.sleep(sleep)
def agent_adapter(params, logger):
logger.info('My params:\n%s' % pformat(params))
required = ['code', 'id_agent']
optional = ['state_db_dir', 'publish_interval']
for req in required:
if not req in params:
msg = 'Necessary parameter %r not given.' % req
msg += '\nGiven configuration:\n%s' % pformat(params)
raise Exception(msg)
state_db_dir = params.get('state_db_dir', LearningStateDB.DEFAULT_DIR)
id_agent = params['id_agent']
code = params['code']
Global.publish_interval = params.get('publish_interval', 0)
Global.publish = Global.publish_interval > 0
Global.count = 0
if Global.publish:
Global.ros_publisher = ROSPublisher()
check_valid_code_spec(code)
AgentInterface.logger = RospyLogger(id_agent)
Global.agent = instantiate_spec(code)
logger.info('agent instantiated: %s' % Global.agent)
# Wait until the robot is connected
result = rospy.wait_for_service('commands')
logger.info('Connected to robot.')
Global.set_commands = rospy.ServiceProxy('commands', BootstrappingCommands)
# Subscribe to the observations
subscriber = rospy.Subscriber('observations', BootstrappingObservations,
handle_observations)
# Read one observation
logger.info('Waiting for one observation...')
while not rospy.is_shutdown():
if Global.observations is not None:
break
rospy.sleep(0.1)
# Close this subscriber
subscriber.unregister()
ob = Global.observations
id_robot = ob.id_robot
spec = BootSpec.from_ros_structure(ob)
Global.ros2python = ROS2Python(spec)
logger.info('Obtained one observation; id_robot: %s, spec: %s' % (id_robot, spec))
load_agent_state(Global.agent, id_agent, id_robot, state_db_dir)
Global.agent.init(spec.sensels_shape, spec.commands_spec) # XXX
# Subscribe to the observations
subscriber = rospy.Subscriber('observations',
BootstrappingObservations, event_loop)
rospy.spin()
