hrpsys_gazebo_atlas: sensor_padkontrol.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 from subprocess import check_call
00004 
00005 import roslib
00006 import rospy
00007 
00008 
00009 roslib.load_manifest("std_msgs")
00010 
00011 from std_msgs.msg import Empty, Float64
00012 from sensor_msgs.msg import Joy
00013 
00014 def joyCB(msg):
00015     print msg
00016     if msg.buttons[0] == 1:
00017         headPub.publish(Empty())
00018     elif msg.buttons[1] == 1:
00019         lhandPub.publish(Empty())
00020     elif msg.buttons[2] == 1:
00021         rhandPub.publish(Empty())
00022     elif msg.buttons[3] == 1:
00023         lFishPub.publish(Empty())
00024     elif msg.buttons[4] == 1:
00025         rFishPub.publish(Empty())
00026     elif msg.buttons[7] == 1:
00027         touchItPub.publish(Float64(msg.axes[0] * 101.0 - 1))
00028     # elif msg.buttons[5] == 1:
00029     #     check_call(["xterm"])
00030     # elif msg.buttons[6] == 1:
00031     #     check_call(["xterm"])
00032       
00033 
00034 
00035 rospy.init_node("sensor_padkontrol")
00036 
00037 rospy.Subscriber("/joy_pad", Joy, joyCB)
00038 headPub = rospy.Publisher("/head_snap/snapshot", Empty)
00039 lhandPub = rospy.Publisher("/lhand_snap/snapshot", Empty)
00040 rhandPub = rospy.Publisher("/rhand_snap/snapshot", Empty)
00041 lFishPub = rospy.Publisher("/lfisheys_snap/snapshot", Empty)
00042 rFishPub = rospy.Publisher("/rfisheys_snap/snapshot", Empty)
00043 touchItPub = rospy.Publisher("/touchit/thre", Float64)
00044 toggleIkModePub = rospy.Publisher("/jsk_interactive_marker_manipulation/toggle_ik_mode", Empty)
00045 toggleStartIkPub = rospy.Publisher("/jsk_interactive_marker_manipulation/toggle_start_ik", Empty)
00046 
00047 rospy.spin()
00048