Go to the documentation of this file.00001
00002 import roslib
00003 roslib.load_manifest('nextage_hardware_service')
00004 import rospy
00005
00006 from nextage_ros_bridge import nextage_client
00007
00008 from hrpsys import rtm
00009 import argparse
00010
00011 from std_srvs.srv import Empty, EmptyRequest, EmptyResponse
00012
00013 def main():
00014 parser = argparse.ArgumentParser(description='ROS service interface for Nextage hardware service')
00015 parser.add_argument('--host', help='CORBA name server hostname')
00016 parser.add_argument('--port', help='CORBA name server port number')
00017 parser.add_argument('--modelfile', help='robot model file name')
00018 parser.add_argument('--robot', help='robot module name (RobotHardware0 for real robot, Robot()')
00019 args, unknown = parser.parse_known_args()
00020
00021 if args.host:
00022 rtm.nshost = args.host
00023 if args.port:
00024 rtm.nsport = args.port
00025 if not args.robot:
00026 args.robot = "RobotHardware0"
00027 if not args.modelfile:
00028 args.modelfile = ""
00029
00030 robot = nextage_client.NextageClient()
00031 robot.init(robotname=args.robot, url=args.modelfile)
00032
00033 rospy.init_node("nextage_hand_dio")
00034
00035 def make_on_off_service(name):
00036 def on_handler(req):
00037 getattr(robot, name)(True)
00038 return EmptyResponse()
00039 def off_handler(req):
00040 getattr(robot, name)(False)
00041 return EmptyResponse()
00042 on = rospy.Service("nextage_hand_dio/" + name + "/on", Empty, on_handler)
00043 off = rospy.Service("nextage_hand_dio/" + name + "/off", Empty, off_handler)
00044 return on, off
00045 make_on_off_service('turn_handlight_l')
00046 make_on_off_service('turn_handlight_r')
00047 make_on_off_service('turn_handlight_both')
00048
00049 def make_empty_service(name):
00050 def handler(req):
00051 getattr(robot, name)()
00052 return EmptyResponse()
00053 return rospy.Service("nextage_hand_dio/" + name, Empty, handler)
00054 make_empty_service('handtool_eject_l')
00055 make_empty_service('handtool_eject_r')
00056 make_empty_service('handtool_attach_l')
00057 make_empty_service('handtool_attach_r')
00058 make_empty_service('gripper_close_l')
00059 make_empty_service('gripper_close_r')
00060 make_empty_service('gripper_open_l')
00061 make_empty_service('gripper_open_r')
00062 make_empty_service('airhand_drawin_l')
00063 make_empty_service('airhand_drawin_r')
00064 make_empty_service('airhand_keep_l')
00065 make_empty_service('airhand_keep_r')
00066 make_empty_service('airhand_release_l')
00067 make_empty_service('airhand_release_r')
00068 make_empty_service('initialize_hand_dio')
00069
00070 rospy.spin()
00071