class GazeboSignalBridgeNode(SignalBridgeNode):
def __init__(self):
super(GazeboSignalBridgeNode, self).__init__()
self.name += '_gazebo'
def ros_init(self):
# wait for services
rospy.loginfo('%s: waiting for /gazebo/reset_world...' % self.name)
rospy.wait_for_service('/gazebo/reset_world')
self.reset_gz_world = rospy.ServiceProxy("/gazebo/reset_world",
EmptySrv)
rospy.loginfo('%s: waiting for /gazebo/pause_physics...' % self.name)
rospy.wait_for_service('/gazebo/pause_physics')
self.pause = rospy.ServiceProxy('/gazebo/pause_physics', EmptySrv)
rospy.loginfo('%s: waiting for /gazebo/unpause_physics...' % self.name)
rospy.wait_for_service('/gazebo/unpause_physics')
self.unpause = rospy.ServiceProxy('/gazebo/unpause_physics', EmptySrv)
self.unpause()
super(GazeboSignalBridgeNode, self).ros_init()
def reset_robot(self, req):
try:
self.reset_gz_world()
self.unpause()
rospy.sleep(0.5)
except rospy.ServiceException, e:
rospy.logerr("%s: service call(s) failed in reset_robot: %s" %
(self.name, e))
return EmptySrvResponse()
def reset_srv_cb(self, msg):
'''
Reset the robot's position to its start state. Create new objects to
manipulate based on experimental parameters.
'''
self.pause()
self.experiment.reset()
self.resume()
return EmptySrvResponse()
def reset_robot(self, req):
rospy.loginfo("%s: resetting plant" % rospy.get_name())
self.has_trigger_start = False
self.trigger_reset_pub.publish() # tell marshall to prompt user
msg_ = "%s: in reset_plant, waiting for /rl/trigger_start..."
rospy.loginfo(msg_ % rospy.get_name())
# wait till received /aqua_rl/trigger_start (from joy/user)
while not self.has_trigger_start and not rospy.is_shutdown():
rospy.sleep(0.1)
rospy.sleep(0.1)
return EmptySrvResponse()
def reset_gridworld(self, req):
"""
Reset gridworld to initial conditions.
:param req: Ignored (trigger)
:return: Empty Response
"""
self.gw = GridWorld(self.gw_bounds, self.gw_res)
self.gw.add_grid('visited', 0.0)
self.gw.add_grid('hist',
1.0,
extra_dims=(self.hist_bins_a, self.hist_bins_q))
self.gw.add_grid('depth_mean', 0.0)
self.gw.add_grid('depth_var', 0.0)
self.gw.add_grid('width_mean', 0.0)
self.gw.add_grid('width_var', 0.0)
self.gw.add_grid('count', 0.0)
self.gw.add_grid('hist_p_prev',
1.0 / self.hist_bins_q,
extra_dims=(self.hist_bins_q, ))
self.position_history = []
self.no_viewpoints = 0
self.counter = 0
return EmptySrvResponse()
def on_switch_gait_engine(self, _):
self.gait_engine.switch_gait_sequencer()
return EmptySrvResponse()
def on_step_to_relaxed(self, srv):
self.controller.execute_step_to_relaxed()
return EmptySrvResponse()
def stop_robot(self, req):
rospy.loginfo("%s: stopping plant" % self.name)
return EmptySrvResponse()
def reset_robot(self, req):
rospy.loginfo("%s: resetting plant" % self.name)
return EmptySrvResponse()
def stop_robot(self, req):
self.trigger_stop_pub.publish()
while self.marshall_mode == 'rl':
rospy.sleep(0.01)
return EmptySrvResponse()
self.reset_gz_world()
self.unpause()
rospy.sleep(0.5)
except rospy.ServiceException, e:
rospy.logerr("%s: service call(s) failed in reset_robot: %s" %
(self.name, e))
return EmptySrvResponse()
def stop_robot(self, req):
try:
rospy.sleep(6.0)
self.pause()
except rospy.ServiceException, e:
rospy.logerr("%s: service call(s) failed in stop_robot: %s" %
(name, e))
return EmptySrvResponse()
class MarshallSignalBridgeNode(SignalBridgeNode):
def __init__(self):
self.has_trigger_start = False
self.marshall_mode = ''
super(MarshallSignalBridgeNode, self).__init__()
self.name += '_marshall'
def ros_init(self):
rospy.loginfo('%s: waiting for /rl_marshall/set_mode...' % self.name)
rospy.wait_for_service('/rl_marshall/set_mode')
self.trigger_reset_pub = rospy.Publisher('/rl/trigger_reset',
Empty,
def train_faces_callback(req):
train_faces_classifier()
return EmptySrvResponse()