head_control_by_trackball.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import threading
00004 from operator import add
00005 import math
00006 import rospy
00007 from sensor_msgs.msg import Joy, CameraInfo, JointState
00008 import actionlib
00009 import control_msgs.msg
00010 import hrpsys_ros_bridge.srv
00011 # try:
00012 #     from image_view2.msg import ImageMarker2
00013 # except:
00014 #     import roslib; roslib.load_manifest("jsk_teleop_joy")
00015 #     from image_view2.msg import ImageMarker2
00016 
00017 from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal 
00018 from trajectory_msgs.msg import JointTrajectoryPoint
00019     
00020 def inRange(val, min, max):
00021     if val < min:
00022         return min
00023     elif val > max:
00024         return max
00025     else:
00026         return val
00027     
00028 class TrackballController:
00029     camera_info = None
00030     joint_state = None
00031     current_cursor = (0, 0)
00032     camera_info_lock = threading.Lock()
00033     joint_state_lock = threading.Lock()
00034     def camCB(self, msg):
00035         with self.camera_info_lock:
00036             if self.camera_info == None:
00037                 # initialize current_cursor
00038                 self.current_cursor = (msg.width / 2, msg.height / 2)
00039             self.camera_info = msg
00040     def getCameraInfo(self):
00041         with self.camera_info_lock:
00042             return self.camera_info
00043     def procSequence(self, sequence):
00044         if len(sequence) > 0:
00045             # integral the sequence
00046             dx = - reduce(add, [msg.axes[0] for msg in sequence])
00047             dy = reduce(add, [msg.axes[1] for msg in sequence])
00048             print (dx, dy)
00049             dpitch_deg = dx / 2.0
00050             dyaw_deg = dy / 2.0
00051             dpitch_rad = dpitch_deg / 180.0 * math.pi
00052             dyaw_rad = dyaw_deg / 180.0 * math.pi
00053             joint_state = self.getJointState()
00054             if not joint_state:           #not yet /joint_state is ready
00055                 rospy.logwarn("joint_state is not yet available")
00056                 return
00057             if (self.pitch_joint_name in joint_state.name and
00058                 self.yaw_joint_name in joint_state.name):
00059                 pitch_index = joint_state.name.index(self.pitch_joint_name)
00060                 yaw_index = joint_state.name.index(self.yaw_joint_name)
00061                 current_pitch = joint_state.position[pitch_index]
00062                 current_yaw = joint_state.position[yaw_index]
00063                 print (current_pitch, current_yaw)
00064                 next_pitch = current_pitch + dpitch_rad
00065                 next_yaw = current_yaw + dyaw_rad
00066                 print (next_pitch, next_yaw)
00067                 # move
00068                 goal = FollowJointTrajectoryGoal()
00069                 goal.trajectory.header.stamp = joint_state.header.stamp
00070                 goal.trajectory.joint_names = [self.pitch_joint_name, 
00071                                                self.yaw_joint_name]
00072                 p = JointTrajectoryPoint()
00073                 p.positions = [next_pitch, next_yaw]
00074                 p.time_from_start = rospy.Duration(0.2)   #0.2 sec
00075                 goal.trajectory.points = [p]
00076                 self.ac_client.send_goal(goal)
00077                 self.ac_client.wait_for_result()
00078             else:
00079                 rospy.logwarn("the joint for pitch and yaw cannot be found in joint_states")
00080                 return
00081     def enableHeadGroupControl(self):
00082         print 'enable head joint group'
00083         self.enable_head_joint_group_srv(gname='head', jnames=[self.pitch_joint_name, self.yaw_joint_name])
00084         self.enable_head_control_flag = True
00085     def disableHeadGroupControl(self):
00086         print 'disable head joint group'
00087         self.disable_head_joint_group_srv(gname='head')
00088         self.enable_head_control_flag = False
00089     def jointCB(self, msg):
00090         with self.joint_state_lock:
00091             self.joint_state = msg
00092     def getJointState(self):
00093         with self.joint_state_lock:
00094             return self.joint_state
00095     def joyCB(self, msg):
00096         if not self.prev_buttons:
00097             self.prev_buttons = msg.buttons
00098         if (msg.buttons[0] == 1 and self.prev_buttons[0] == 0):
00099             self.enableHeadGroupControl()
00100         elif (msg.buttons[2] == 1 and self.prev_buttons[2] == 0):
00101             self.disableHeadGroupControl()
00102         elif (abs(msg.axes[0]) > 1.0 or abs(msg.axes[1]) > 1.0):
00103             self.procSequence([msg])
00104         self.prev_buttons = msg.buttons
00105     def joyCB2(self, msg):
00106         camera_info = self.getCameraInfo()
00107         if not camera_info:
00108             rospy.logfatal("camera_info is not yet available")
00109             return
00110         (raw_dx, raw_dy) = (msg.axes[0], msg.axes[1])
00111         current_x = self.current_cursor[0]
00112         current_y = self.current_cursor[1]
00113         next_x = inRange(raw_dx + current_x, 0, camera_info.width)
00114         next_y = inRange(-raw_dy + current_y, 0, camera_info.height)
00115         self.current_cursor = (next_x, next_y)
00116         self.publishCursor(msg.header.stamp, self.current_cursor)
00117     def publishCursor(self, stamp, pos):
00118         marker = ImageMarker2()
00119         marker.header.stamp = stamp
00120         marker.type = ImageMarker2.CIRCLE
00121         marker.position.x = pos[0]
00122         marker.position.y = pos[1]
00123         marker.scale = 10                  #5 px
00124         self.marker_pub.publish(marker)
00125     def main(self):
00126         self.pitch_joint_name = rospy.get_param("~pitch_joint", "head_pan_joint")
00127         self.yaw_joint_name = rospy.get_param("~yaw_joint", "head_tilt_joint")
00128         self.enable_head_joint_group_srv = rospy.ServiceProxy('/SequencePlayerServiceROSBridge/addJointGroup',
00129                                                          hrpsys_ros_bridge.srv.OpenHRP_SequencePlayerService_addJointGroup)
00130         self.disable_head_joint_group_srv = rospy.ServiceProxy('/SequencePlayerServiceROSBridge/removeJointGroup',
00131                                                          hrpsys_ros_bridge.srv.OpenHRP_SequencePlayerService_removeJointGroup)
00132         self.enableHeadGroupControl()
00133         self.prev_buttons = False
00134         self.joint_trajectory_action_name = rospy.get_param("~joint_trajectory_action", "/head_traj_controller/follow_joint_trajectory")
00135         self.ac_client = actionlib.SimpleActionClient(self.joint_trajectory_action_name,
00136                                                       control_msgs.msg.FollowJointTrajectoryAction)
00137         self.ac_client.wait_for_server()
00138         #self.marker_pub = rospy.Publisher("/image_marker", ImageMarker2)
00139         s = rospy.Subscriber("/trackball_joy", Joy, self.joyCB, queue_size=1)
00140         scam = rospy.Subscriber("camera_info", CameraInfo, self.camCB)
00141         sjs = rospy.Subscriber("/joint_states", JointState, self.jointCB)
00142         rospy.spin()
00143 
00144     
00145 
00146 def main():
00147     controller = TrackballController()
00148     controller.main()
00149     
00150 
00151 if __name__ == "__main__":
00152     rospy.init_node("head_control_by_trackball")
00153     main()
00154     
00155   


jsk_teleop_joy
Author(s): Ryohei Ueda
autogenerated on Fri Apr 19 2019 03:45:43