1 #!/usr/bin/env python
   2 # -*- coding: utf-8 -*-
   3 
   4 # Software License Agreement (BSD License)
   5 #
   6 # Copyright (c) 2016, Tokyo Opensource Robotics Kyokai Association
   7 # All rights reserved.
   8 #
   9 # Redistribution and use in source and binary forms, with or without
  10 # modification, are permitted provided that the following conditions
  11 # are met:
  12 #
  13 #  * Redistributions of source code must retain the above copyright
  14 #    notice, this list of conditions and the following disclaimer.
  15 #  * Redistributions in binary form must reproduce the above
  16 #    copyright notice, this list of conditions and the following
  17 #    disclaimer in the documentation and/or other materials provided
  18 #    with the distribution.
  19 #  * Neither the name of Tokyo Opensource Robotics Kyokai Association. nor the
  20 #    names of its contributors may be used to endorse or promote products
  21 #    derived from this software without specific prior written permission.
  22 #
  23 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  24 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  25 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  26 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  27 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  28 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  29 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  30 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  31 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  32 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  33 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  34 # POSSIBILITY OF SUCH DAMAGE.
  35 #
  36 # Author: Isaac I.Y. Saito
  37 
  38 # This should come earlier than later import.
  39 # See http://code.google.com/p/rtm-ros-robotics/source/detail?r=6773
  40 from nextage_ros_bridge import nextage_client
  41 
  42 from hrpsys import rtm
  43 import argparse
  44 from hironx_ros_bridge.ros_client import ROS_Client
  45 from leap_motion.msg import leapros
  46 import rospy
  47 
  48 class NXO_Sample():
  49     def __init__(self, nxo_if):
  50         '''
  51         @param nxo_if: type nextage_client.NextageClient
  52         '''
  53         self._robot = nxo_if
  54         :
  55 
  56     def _cb_leap(msg_hand):
  57         if(msg_hand.XXX):
  58             self._robot.checkEncoders()
  59         elif(msg_hand.YYY):
  60             self._robot.goOffPose()
  61 
  62     def subscribe_hand():
  63         '''
  64         Subscribes messages from leap_motion sensor. Once received the messages are processed in a callback function (_cb_leap).
  65         '''
  66         rospy.init_node('listener', anonymous=True)
  67         rospy.Subscriber("leapmotion/data", leapros, _cb_leap)
  68         rospy.spin()
  69 
  70 if __name__ == '__main__':
  71     parser = argparse.ArgumentParser(description='NEXTAGE Open command line interpreters')
  72     parser.add_argument('--host', help='corba name server hostname')
  73     parser.add_argument('--port', help='corba name server port number')
  74     parser.add_argument('--modelfile', help='robot model file nmae')
  75     parser.add_argument('--robot', help='robot modlule name (RobotHardware0 for real robot, Robot()')
  76     args, unknown = parser.parse_known_args()
  77 
  78     if args.host:
  79         rtm.nshost = args.host
  80     if args.port:
  81         rtm.nsport = args.port
  82     if not args.robot:
  83         args.robot = "RobotHardware0" if args.host else "HiroNX(Robot)0"
  84     if not args.modelfile:
  85         args.modelfile = ""
  86 
  87     # support old style format
  88     if len(unknown) >= 2:
  89         args.robot = unknown[0]
  90         args.modelfile = unknown[1]
  91     robot = nxc = nextage_client.NextageClient()
  92     # Use generic name for the robot instance. This enables users on the
  93     # script commandline (eg. ipython) to run the same commands without asking
  94     # them to specifically tell what robot they're using (eg. hiro, nxc).
  95     # This is backward compatible so that users can still keep using `nxc`.
  96     # See http://code.google.com/p/rtm-ros-robotics/source/detail?r=6926
  97     robot.init(robotname=args.robot, url=args.modelfile)
  98 
  99     ```