def __init__(self): self.tf_listener = tf.TransformListener() # self.move_head_client = actionlib.SimpleActionClient(\ # 'head_traj_controller/point_head_action', PointHeadAction) self.head = Head(self.tf_listener) self.scan_for_markers_service = rospy.Service("scan_for_markers", ScanForMarkers, self.scan_for_markers) self.scan_fov_service = rospy.Service("scan_fov", ScanFOV, self.scan_fov) rospy.loginfo("scan_for_markers service started.")
class MarkerScanner(object): def __init__(self): self.tf_listener = tf.TransformListener() # self.move_head_client = actionlib.SimpleActionClient(\ # 'head_traj_controller/point_head_action', PointHeadAction) self.head = Head(self.tf_listener) self.scan_for_markers_service = rospy.Service("scan_for_markers", ScanForMarkers, self.scan_for_markers) self.scan_fov_service = rospy.Service("scan_fov", ScanFOV, self.scan_fov) rospy.loginfo("scan_for_markers service started.") def scan_for_markers(self, req): ids_left = set(req.ids) scan_poses = req.poses marker_dict = {} marker_header = None tries = 0 while ids_left: for scan_pose in scan_poses: # TODO adjust base/torso height to look directly at marker # if necessary if not ids_left: break print "GOT HERE" mp = PointStamped(req.header, scan_pose.position) mp.header.stamp = rospy.Time.now() self.look_at(mp) # self.fix_offset(mp) marker_infos_partial = MarkerInfos() for i in range(0, 5): # keep looking at same area for a while try: markers = rospy.wait_for_message(req.detector_service, MarkerInfos, timeout=1.0) except rospy.exceptions.ROSException: continue marker_header = markers.header for id, pose in zip(markers.ids, markers.poses): if id not in ids_left: continue rospy.loginfo("Detected marker %d." % id) ids_left.remove(id) marker_dict[id] = pose rospy.loginfo("Markers remaining: " + str(list(ids_left)).replace("[", "{").replace("]", "}")) tries += 1 if tries > MAX_SCAN_TRIES: rospy.logerr("Couldn't find markers: " + str(list(ids_left)).replace("[", "{").replace("]", "}")) break marker_poses = [] ids = [] for id in marker_dict: ids.append(id) marker_poses.append(marker_dict[id]) marker_header.stamp = rospy.Time.now() return ScanForMarkersResponse(MarkerInfos(marker_header, marker_poses, ids)) def scan_fov(self, req): marker_dict = {} marker_header = None for pan in range(req.start_pan, req.end_pan, req.angle_step): for tilt in range(req.start_tilt, req.end_tilt, req.angle_step): self.angle_head(pan * pi / 180, tilt * pi / 180) try: markers = rospy.wait_for_message(req.detector_service, MarkerInfos, timeout=1.0) except rospy.exceptions.ROSException: continue marker_header = markers.header for id, pose in zip(markers.ids, markers.poses): if id in marker_dict: continue rospy.loginfo("Detected marker %d." % id) marker_dict[id] = pose marker_poses = [] ids = [] for id in marker_dict: ids.append(id) marker_poses.append(marker_dict[id]) return ScanFOVResponse(MarkerInfos(marker_header, marker_poses, ids)) def look_at(self, point, pointing_frame="narrow_stereo_optical_frame"): """ self.move_head_client.wait_for_server() goal = PointHeadGoal() goal.target.header.frame_id = point.header.frame_id goal.target.point = point.point goal.pointing_frame = pointing_frame goal.max_velocity = 0.25 goal.min_duration = rospy.Duration(0.5) self.move_head_client.send_goal_and_wait(goal) return self.move_head_client.get_result() """ self.head.look_at(point, pointing_frame) # I believe as of Diamondback PointHeadAction still does not work # properly (Google "fixed pointheadaction") """ def fix_offset(self, point, pointing_frame = 'narrow_stereo_optical_frame'): self.tf_listener.waitForTransform(point.header.frame_id,\ pointing_frame, rospy.Time.now(),\ rospy.Duration(5.0)) pos, rot = self.tf_listener.lookupTransform(point.header.frame_id,\ pointing_frame, rospy.Time()) mp = point.point joint_states = rospy.wait_for_message('joint_states', JointState, 5.0) pan = joint_states.position[joint_states.name.index('head_pan_joint')] tilt = pi/2-atan(sqrt((pos[0]-mp.x)**2+(pos[1]-mp.y)**2)/(pos[2]-mp.z)) #HARDCODED pan-=pi/24 tilt+=pi/24 print pan, tilt self.angle_head(pan, tilt, pointing_frame) """ # adapted pr2_simple_head_motions def angle_head(self, pan, tilt, pointing_frame="double_stereo_link"): roll = 0 yaw = pan pitch = tilt (qx, qy, qz, qw) = tf.transformations.quaternion_from_euler(roll, pitch, yaw) final_quat = QuaternionStamped() final_quat.header.frame_id = "torso_lift_link" final_quat.header.stamp = rospy.Time.now() final_quat.quaternion.x = qx final_quat.quaternion.y = qy final_quat.quaternion.z = qz final_quat.quaternion.w = qw # Convert the angle to the current stereo frame self.tf_listener.waitForTransform(pointing_frame, "torso_lift_link", rospy.Time.now(), rospy.Duration(10.0)) rel_quat = self.tf_listener.transformQuaternion(pointing_frame, final_quat) rel_quat_tuple = (rel_quat.quaternion.x, rel_quat.quaternion.y, rel_quat.quaternion.z, rel_quat.quaternion.w) # Get the point which corresponds to one meter ahead of the stereo frame straight_target_q = (1.0, 0, 0, 0) # Rotate by the quaternion (x, y, z, trash) = tf.transformations.quaternion_multiply( rel_quat_tuple, tf.transformations.quaternion_multiply( straight_target_q, tf.transformations.quaternion_inverse(rel_quat_tuple) ), ) new_target = PointStamped() new_target.header.stamp = rospy.Time.now() new_target.header.frame_id = pointing_frame new_target.point.x = x new_target.point.y = y new_target.point.z = z # Now put in torso_lift_link frame self.tf_listener.waitForTransform("torso_lift_link", pointing_frame, rospy.Time.now(), rospy.Duration(10.0)) target = self.tf_listener.transformPoint("torso_lift_link", new_target) self.look_at(target)