4 from operator
import add
9 import control_msgs.msg
10 import hrpsys_ros_bridge.srv
17 from control_msgs.msg
import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
18 from trajectory_msgs.msg
import JointTrajectoryPoint
31 current_cursor = (0, 0)
32 camera_info_lock = threading.Lock()
33 joint_state_lock = threading.Lock()
46 dx = - reduce(add, [msg.axes[0]
for msg
in sequence])
47 dy = reduce(add, [msg.axes[1]
for msg
in sequence])
51 dpitch_rad = dpitch_deg / 180.0 * math.pi
52 dyaw_rad = dyaw_deg / 180.0 * math.pi
55 rospy.logwarn(
"joint_state is not yet available")
61 current_pitch = joint_state.position[pitch_index]
62 current_yaw = joint_state.position[yaw_index]
63 print((current_pitch, current_yaw))
64 next_pitch = current_pitch + dpitch_rad
65 next_yaw = current_yaw + dyaw_rad
66 print((next_pitch, next_yaw))
68 goal = FollowJointTrajectoryGoal()
69 goal.trajectory.header.stamp = joint_state.header.stamp
72 p = JointTrajectoryPoint()
73 p.positions = [next_pitch, next_yaw]
74 p.time_from_start = rospy.Duration(0.2)
75 goal.trajectory.points = [p]
79 rospy.logwarn(
"the joint for pitch and yaw cannot be found in joint_states")
82 print(
'enable head joint group')
86 print(
'disable head joint group')
100 elif (msg.buttons[2] == 1
and self.
prev_buttons[2] == 0):
102 elif (abs(msg.axes[0]) > 1.0
or abs(msg.axes[1]) > 1.0):
108 rospy.logfatal(
"camera_info is not yet available")
110 (raw_dx, raw_dy) = (msg.axes[0], msg.axes[1])
113 next_x =
inRange(raw_dx + current_x, 0, camera_info.width)
114 next_y =
inRange(-raw_dy + current_y, 0, camera_info.height)
118 marker = ImageMarker2()
119 marker.header.stamp = stamp
120 marker.type = ImageMarker2.CIRCLE
121 marker.position.x = pos[0]
122 marker.position.y = pos[1]
124 self.marker_pub.publish(marker)
129 hrpsys_ros_bridge.srv.OpenHRP_SequencePlayerService_addJointGroup)
131 hrpsys_ros_bridge.srv.OpenHRP_SequencePlayerService_removeJointGroup)
136 control_msgs.msg.FollowJointTrajectoryAction)
139 s = rospy.Subscriber(
"/trackball_joy", Joy, self.
joyCB, queue_size=1)
140 scam = rospy.Subscriber(
"camera_info", CameraInfo, self.
camCB)
141 sjs = rospy.Subscriber(
"/joint_states", JointState, self.
jointCB)
151 if __name__ ==
"__main__":
152 rospy.init_node(
"head_control_by_trackball")
enable_head_joint_group_srv
def procSequence(self, sequence)
disable_head_joint_group_srv
def inRange(val, min, max)
def disableHeadGroupControl(self)
joint_trajectory_action_name
def enableHeadGroupControl(self)
def publishCursor(self, stamp, pos)