def test_read_pose(self):
""" Test if we can connect to the pose data stream, and read from it.
"""
morse = Morse()
pose_stream = morse.stream('Pose')
pose = pose_stream.get()
for coord in pose.values():
self.assertAlmostEqual(coord, 0.0, 2)
morse.close()
def test_get_pose_streams_service(self):
""" Tests if we can retrieve the list of published data streams.
"""
morse = Morse()
self.assertEquals(set(morse.streams()), set(["robot.pose"]))
morse.close()
def test_base_service_connection(self):
""" Simply tests if the simulator is reachable by its socket interface.
"""
morse = Morse()
morse.close()
return
lift_commands.append(floor)
for t in ["call","command"]:
for i in range(0,4):
rospy.Subscriber("/lift_sim/%s%s"%(t,doors[i]), Bool, on_floor_call_command, callback_args=(t,i))
morse = None
while morse is None:
try:
morse = Morse()
except ConnectionError as e:# ConnectionError as e:
rospy.loginfo("Waiting for Morse...")
print("[!!!!!] ",e,"Waiting for Morse...")
rospy.sleep(3)
while not rospy.is_shutdown():
if len(lift_commands)>1:
rospy.loginfo("Closing door %s"%doors[lift_commands[0]])
morse.rpc('lift.door%s'%doors[lift_commands[0]],'change_door_state',0)
lift_commands=lift_commands[1:]
rospy.loginfo("Moving to floor %s"%doors[lift_commands[0]])
morse.rpc('lift.platform','move_to_floor',lift_commands[0]-1)
rospy.loginfo("Opening door %s"%doors[lift_commands[0]])
morse.rpc('lift.door%s'%doors[lift_commands[0]],'change_door_state',1)
rospy.sleep(8)
morse.close()
for t in ["call", "command"]:
for i in range(0, 4):
rospy.Subscriber("/lift_sim/%s%s" % (t, doors[i]),
Bool,
on_floor_call_command,
callback_args=(t, i))
morse = None
while morse is None:
try:
morse = Morse()
except ConnectionError as e: # ConnectionError as e:
rospy.loginfo("Waiting for Morse...")
print("[!!!!!] ", e, "Waiting for Morse...")
rospy.sleep(3)
while not rospy.is_shutdown():
if len(lift_commands) > 1:
rospy.loginfo("Closing door %s" % doors[lift_commands[0]])
morse.rpc('lift.door%s' % doors[lift_commands[0]], 'change_door_state',
0)
lift_commands = lift_commands[1:]
rospy.loginfo("Moving to floor %s" % doors[lift_commands[0]])
morse.rpc('lift.platform', 'move_to_floor', lift_commands[0] - 1)
rospy.loginfo("Opening door %s" % doors[lift_commands[0]])
morse.rpc('lift.door%s' % doors[lift_commands[0]], 'change_door_state',
1)
rospy.sleep(8)
morse.close()
