mock_robot.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 from actionlib import SimpleActionServer, SimpleActionClient
00005 
00006 from nodelet.srv import NodeletLoad, NodeletUnload
00007 from stdr_msgs.msg import SpawnRobotAction, SpawnRobotResult
00008 from stdr_msgs.msg import RegisterRobotAction, RegisterRobotGoal
00009 from stdr_msgs.srv import MoveRobot, MoveRobotResponse
00010 
00011 import threading
00012 
00013 class MockRobot():
00014 
00015     def __init__(self):
00016         self.load_nodelet = rospy.Service('robot_manager/load_nodelet',
00017                 NodeletLoad, self.load_nodelet_cb)
00018 
00019         self.unload_nodelet = rospy.Service('robot_manager/unload_nodelet',
00020                 NodeletUnload, self.unload_nodelet_cb)
00021 
00022         self.register_robot_client = \
00023             SimpleActionClient('stdr_server/register_robot', RegisterRobotAction)
00024         self.register_robot_client.wait_for_server()
00025 
00026         self.name = None
00027 
00028     def load_nodelet_cb(self, request):
00029         rospy.loginfo('Requested new robot')
00030         # Spawn a new thread, as this service has to return first
00031         req_thread = threading.Thread(target=self.register_robot, args=(request.name,))
00032         req_thread.start()
00033         return True
00034 
00035     def unload_nodelet_cb(self, request):
00036         rospy.loginfo('Request to delete "%s"' % request.name)
00037         self.move_service.shutdown()
00038         return True
00039 
00040     def register_robot(self, name):
00041         rospy.sleep(1)
00042         rospy.loginfo('Register "%s" to server' % name)
00043         register_goal = RegisterRobotGoal()
00044         register_goal.name = name
00045         self.register_robot_client.send_goal(register_goal)
00046         self.name = name
00047         # Spawn move robot service
00048         self.move_service = rospy.Service(name + '/replace', MoveRobot,
00049                                 self.move_robot)
00050 
00051     def move_robot(self, req):
00052         rospy.loginfo('Request to move %s' % self.name)
00053         return MoveRobotResponse()
00054 
00055 if __name__ == '__main__':
00056     rospy.init_node('mock_robot', anonymous=True)
00057     robot = MockRobot()
00058     rospy.spin()


stdr_server
Author(s): Chris Zalidis
autogenerated on Tue Feb 7 2017 03:46:33