class MoveFloorButton: def __init__(self): self.tl = TransformListener() self.perception_srv = rospy.Service("/clickable_world/detect_empty_floor", PerceiveButtons, self.do_perception) self.percept_pub = rospy.Publisher("/clickable_world/floor_button_vis", Marker) self.goal_pub = rospy.Publisher("/clickable_world/move_floor_goal", PoseStamped) self.action_srv = rospy.Service("/clickable_world/move_empty_floor", ButtonAction, self.do_action) self.floor_seg_srv = rospy.ServiceProxy("/move_floor_detect", SegmentFloor) self.floor_move_client = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("[move_floor_button] MoveFloorButton loaded.") def do_perception(self, req): # segment surfaces rospy.loginfo("[move_floor_button] Segmenting floor") self.surfaces = self.floor_seg_srv().surfaces for i in range(len(self.surfaces)): self.surfaces[i].color.r = 0 self.surfaces[i].color.g = 0 self.surfaces[i].color.b = 256 self.surfaces[i].color.a = 256 if len(self.surfaces) != 0: self.percept_pub.publish(self.surfaces[0]) resp = PerceiveButtonsResponse() resp.buttons = self.surfaces return resp def do_action(self, req): rospy.loginfo("[move_floor_button] MoveFloorButton clicked!") # put 3d pt in base frame req.pixel3d.header.stamp = rospy.Time(0) move_pt = self.tl.transformPoint("/base_link", req.pixel3d) floor_move_goal = MoveBaseGoal() floor_move_goal.target_pose.header.frame_id = move_pt.header.frame_id floor_move_goal.target_pose.pose.position = move_pt.point quat = tf_trans.quaternion_from_euler(0, 0, np.arctan2(move_pt.point.y, move_pt.point.x)) floor_move_goal.target_pose.pose.orientation = Quaternion(quat[0], quat[1], quat[2], quat[3]) floor_move_goal.target_pose.header.frame_id = "/base_link" floor_move_goal.target_pose.header.stamp = rospy.Time.now() try: self.goal_pub.publish(floor_move_goal.target_pose) self.floor_move_client.send_goal(floor_move_goal) self.floor_move_client.wait_for_result() cur_pose = self.floor_move_client.get_result() except rospy.ROSInterruptException: print "[move_floor_button] Move floor failed" return ButtonActionResponse()
def filter_by_distance(features, max_distance): # type: (List[Feature], float) -> List[Feature] cam_pixel_to_point = rospy.ServiceProxy('cam_pixel_to_point', CamPixelToPoint) tf_listener = TransformListener() filtered = [] for feature in features: cam_pos = Vector3() cam_pos.x, cam_pos.y = feature.centroid[0], feature.centroid[1] point = cam_pixel_to_point(cam_pos).point # type: PointStamped tf_listener.waitForTransform('camera_link', point.header.frame_id, rospy.Time(rospy.get_time()), rospy.Duration(1)) point = tf_listener.transformPoint('camera_link', point) if point.point.x <= max_distance: filtered.append(feature) return filtered
def feature_depths(features): cam_pixel_to_point = rospy.ServiceProxy('cam_pixel_to_point', CamPixelToPoint) tf_listener = TransformListener() depths = [] for feature in features: cam_pos = Vector3() cam_pos.x, cam_pos.y = feature.centroid[0], feature.centroid[1] point = cam_pixel_to_point(cam_pos).point # type: PointStamped tf_listener.waitForTransform('camera_link', point.header.frame_id, rospy.Time(rospy.get_time()), rospy.Duration(1)) point = tf_listener.transformPoint('camera_link', point) depths.append(np.linalg.norm([point.point.x, point.point.y, point.point.z])) return depths
def select_center(features): # type: (List[Feature]) -> Feature cam_pixel_to_point = rospy.ServiceProxy('cam_pixel_to_point', CamPixelToPoint) tf_listener = TransformListener() angles = [] center_ray = np.array([1.0, 0.0, 0.0]) for feature in features: cam_pos = Vector3() cam_pos.x, cam_pos.y = feature.centroid[0], feature.centroid[1] point = cam_pixel_to_point(cam_pos).point # type: PointStamped tf_listener.waitForTransform('camera_link', point.header.frame_id, rospy.Time(rospy.get_time()), rospy.Duration(1)) point = tf_listener.transformPoint('camera_link', point) ray = np.array([point.point.x, point.point.y, point.point.z]) ray /= np.linalg.norm(ray) angle = np.arccos(np.dot(center_ray, ray)) angles.append(abs(angle)) return features[np.argmin(angles)]
class ClosestPersonDetector(object): """ Merges the outputs from the leg_tracker package with the face_detector package from WillowGarage. Then from the combined data, it publishes the closest person. """ def __init__(self): # Internal parameters self.publish_rate = rospy.get_param("~publish_rate", 10.0) self.listener = TransformListener() self.desired_pose_frame = rospy.get_param("~desired_pose_frame", "base_link") self.position_match_threshold = rospy.get_param( "~position_match_threshold", 1.0) # Variables to keep track of state self.closest_person = None self.leg_detections = [] self.closest_person_lock = Lock() self.leg_detections_lock = Lock() # Internal helpers self.person_face_distance_func = lambda A, B: np.sqrt( (A.pose.position.x - B.pos.x)**2 + (A.pose.position.y - B.pos.y)**2 + (1.2 - B.pos.z)**2 # A person's face is about 4ft from the floor ) self.leg_detection_is_closest_face = lambda detected_person: ( self.closest_person is not None and self.closest_person. detection_context.pose_source == DetectionContext.POSE_FROM_FACE and self.closest_person.id == detected_person.id) # The subscribers and publishers self.face_sub = rospy.Subscriber( "face_detector/people_tracker_measurements_array", PositionMeasurementArray, self.face_callback) self.leg_sub = rospy.Subscriber("people_tracked", PersonArray, self.leg_callback) self.closest_person_pub = rospy.Publisher('~closest_person', Person, queue_size=10) # Debug self.debug_enabled = rospy.get_param("~debug", False) if self.debug_enabled: self.debug_pub1 = rospy.Publisher("~debug/1", Marker, queue_size=1) self.debug_pub2 = rospy.Publisher("~debug/2", Marker, queue_size=1) def leg_callback(self, msg): closest_distance = np.inf closest_person = None # Iterate over the people and find the closest with self.leg_detections_lock: self.leg_detections = [] for detected_person in msg.people: detected_pose = PoseStamped(header=msg.header, pose=detected_person.pose) try: detected_pose = self.listener.transformPose( self.desired_pose_frame, detected_pose) except ExtrapolationException as e: continue # Add the person to the list of leg_detections detected_person = Person(header=detected_pose.header, id=str(detected_person.id), pose=detected_pose.pose) detected_person.detection_context.pose_source = DetectionContext.POSE_FROM_LEGS with self.leg_detections_lock: self.leg_detections.append(detected_person) # If this is the closest person, save them. However, if this person # has the same ID as the person with a face, prefer that person_distance = np.sqrt(detected_pose.pose.position.x**2 + detected_pose.pose.position.y**2) if self.leg_detection_is_closest_face( detected_person) or person_distance < closest_distance: closest_distance = person_distance closest_person = detected_person # If the leg detection is the closest face, then disable new # closest detections associations if self.leg_detection_is_closest_face(detected_person): closest_distance = -np.inf # Debug if self.debug_enabled and closest_person is not None: marker = Marker(header=closest_person.header, ns="debug", id=1, type=Marker.SPHERE, action=Marker.ADD) marker.pose = closest_person.pose marker.scale.x = 0.5 marker.scale.y = 0.5 marker.scale.z = 0.5 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 self.debug_pub2.publish(marker) # Acquire a lock to the people and update the closest person's position # We don't want to be staring at feet... with self.closest_person_lock: if closest_person is None: self.closest_person = None elif self.closest_person is None or self.closest_person.id != closest_person.id: self.closest_person = closest_person else: # self.closest_person.id == closest_person.id self.closest_person.pose.position.x = closest_person.pose.position.x self.closest_person.pose.position.y = closest_person.pose.position.y def face_callback(self, msg): # Iterate through the message and find the closest face closest_face = None closest_distance = np.inf for face in msg.people: pos = PointStamped(header=face.header, point=face.pos) try: pos = self.listener.transformPoint(self.desired_pose_frame, pos) except ExtrapolationException as e: continue # Find the closest face distance = np.sqrt(pos.point.x**2 + pos.point.y**2 + pos.point.z**2) if distance < closest_distance: closest_distance = distance closest_face = face closest_face.header = pos.header closest_face.pos = pos.point # Debug if self.debug_enabled and closest_face is not None: marker = Marker(header=closest_face.header, ns="debug", id=0, type=Marker.SPHERE, action=Marker.ADD) marker.pose.position = closest_face.pos marker.pose.orientation.w = 1.0 marker.scale.x = 0.5 marker.scale.y = 0.5 marker.scale.z = 0.5 marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 self.debug_pub1.publish(marker) # Now associate the closest face to the leg detection that it is closest # to closest_person = None with self.leg_detections_lock: for detected_person in self.leg_detections: if closest_face is not None and self.person_face_distance_func( detected_person, closest_face) < self.position_match_threshold: closest_person = detected_person closest_person.pose.position = closest_face.pos break # Now check the distance between the closest face and the closest leg. # If the distance exceeds the threshold, then don't associate the face # with the leg with self.closest_person_lock: if closest_person is None or self.closest_person is None: pass else: self.closest_person = closest_person self.closest_person.detection_context.pose_source = DetectionContext.POSE_FROM_FACE def spin(self): """ Run the detector """ # Publish the detected closest person at the desired frequency rate = rospy.Rate(self.publish_rate) while not rospy.is_shutdown(): # Otherwise, check to see if we should publish the latest detection with self.closest_person_lock: if self.closest_person is not None: self.closest_person_pub.publish(self.closest_person) # Sleep rate.sleep()
class GraspObjButton: def __init__(self): self.arm = rospy.get_param("arm", "r") self.tl = TransformListener() self.perception_srv = rospy.Service("/clickable_world/detect_objects", PerceiveButtons, self.do_perception) self.action_srv = rospy.Service("/clickable_world/grasp_object", ButtonAction, self.do_action) self.obj_seg_srv = rospy.ServiceProxy("/object_button_detect", ObjectButtonDetect) self.grasp_client = actionlib.SimpleActionClient(self.arm + '_overhead_grasp', OverheadGraspAction) self.grasp_client.wait_for_server() self.grasp_setup_client = actionlib.SimpleActionClient(self.arm + '_overhead_grasp_setup', OverheadGraspSetupAction) self.grasp_setup_client.wait_for_server() def do_perception(self, req): rospy.loginfo("[grasp_obj_button] Perceiving object buttons...") # perceive object on table self.objects = self.obj_seg_srv().objects for i in range(len(self.objects)): self.objects[i].color.r = 256 self.objects[i].color.g = 0 self.objects[i].color.b = 0 self.objects[i].color.a = 256 # return object buttons resp = PerceiveButtonsResponse() resp.buttons = self.objects return resp def do_action(self, req): rospy.loginfo("[grasp_obj_button] Table clicked, grasping object...") # put 3d pt in grasping frame req.pixel3d.header.stamp = rospy.Time(0) grasp_pt = self.tl.transformPoint("/torso_lift_link", req.pixel3d) # move to grasp setup position setup_goal = OverheadGraspSetupGoal() setup_goal.disable_head = False setup_goal.open_gripper = True self.grasp_setup_client.send_goal(setup_goal) self.grasp_setup_client.wait_for_result() ############################################################ # Creating grasp goal grasp_goal = OverheadGraspGoal() grasp_goal.is_grasp = True grasp_goal.disable_head = False grasp_goal.disable_coll = False grasp_goal.grasp_type=OverheadGraspGoal.VISION_GRASP grasp_goal.x = grasp_pt.point.x grasp_goal.y = grasp_pt.point.y grasp_goal.behavior_name = "overhead_grasp" grasp_goal.sig_level = 0.999 ############################################################ # grasp object self.grasp_client.send_goal(grasp_goal) self.grasp_client.wait_for_result() result = self.grasp_client.get_result() rospy.loginfo("[grasp_obj_button] Grasp result: %s" % result) obj_in_hand = result.grasp_result == "Object grasped" # go back to grasp setup setup_goal.open_gripper = False self.grasp_setup_client.send_goal(setup_goal) self.grasp_setup_client.wait_for_result() return ButtonActionResponse()
class NavHeadController: def __init__(self): self.has_goal = False # pose and lock self.x = 1.0 self.y = 0.0 self.mutex = Lock() self.listener = TransformListener() self.client = actionlib.SimpleActionClient( "head_controller/point_head", PointHeadAction) self.client.wait_for_server() self.plan_sub = rospy.Subscriber( "move_base/TrajectoryPlannerROS/local_plan", Path, self.planCallback) self.stat_sub = rospy.Subscriber("move_base/status", GoalStatusArray, self.statCallback) def statCallback(self, msg): goal = False for status in msg.status_list: if status.status == GoalStatus.ACTIVE: goal = True break self.has_goal = goal def planCallback(self, msg): # get the goal pose_stamped = msg.poses[-1] pose = pose_stamped.pose # look ahead one meter R = quaternion_matrix([ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ]) point = [1, 0, 0, 1] M = R * point p = PointStamped() p.header.frame_id = pose_stamped.header.frame_id p.header.stamp = rospy.Time(0) p.point.x += pose_stamped.pose.position.x + M[0, 0] p.point.y += pose_stamped.pose.position.y + M[1, 0] p.point.z += pose_stamped.pose.position.z + M[2, 0] # transform to base_link p = self.listener.transformPoint("base_link", p) # update with self.mutex: if p.point.x < 0.65: self.x = 0.65 else: self.x = p.point.x if p.point.y > 0.5: self.y = 0.5 elif p.point.y < -0.5: self.y = -0.5 else: self.y = p.point.y def loop(self): while not rospy.is_shutdown(): if self.has_goal: goal = PointHeadGoal() goal.target.header.stamp = rospy.Time.now() goal.target.header.frame_id = "base_link" with self.mutex: goal.target.point.x = self.x goal.target.point.y = self.y self.x = 1 self.y = 0 goal.target.point.z = 0.0 goal.min_duration = rospy.Duration(1.0) self.client.send_goal(goal) self.client.wait_for_result() with self.mutex: goal.target.point.x = self.x goal.target.point.y = self.y self.x = 1 self.y = 0 goal.target.point.z = 0.75 self.client.send_goal(goal) self.client.wait_for_result() else: rospy.sleep(1.0)
class PlaceObject: def __init__(self): self.arm = rospy.get_param("arm", "r") self.tl = TransformListener() self.perception_srv = rospy.Service( "/clickable_world/place_table_perceive", PerceiveButtons, self.do_perception ) self.action_srv = rospy.Service("/clickable_world/place_object", ButtonAction, self.do_action) self.pc_capture_srv = rospy.ServiceProxy("/table_detection/surf_seg_capture_pc", std_srvs.srv.Empty) self.table_seg_srv = rospy.ServiceProxy("/table_detection/segment_surfaces", SegmentSurfaces) self.grasp_client = actionlib.SimpleActionClient(self.arm + "_overhead_grasp", OverheadGraspAction) self.grasp_client.wait_for_server() self.grasp_setup_client = actionlib.SimpleActionClient( self.arm + "_overhead_grasp_setup", OverheadGraspSetupAction ) self.grasp_setup_client.wait_for_server() def do_perception(self, req): rospy.loginfo("[place_object] Perceiving table...") # capture a few clouds rate = rospy.Rate(5) for i in range(5): self.pc_capture_srv() rate.sleep() # segment surfaces self.surfaces = self.table_seg_srv().surfaces for i in range(len(self.surfaces)): self.surfaces[i].color.r = 256 self.surfaces[i].color.g = 0 self.surfaces[i].color.b = 256 self.surfaces[i].color.a = 256 resp = PerceiveButtonsResponse() resp.buttons = self.surfaces return resp def do_action(self, req): rospy.loginfo("[place_object] Table clicked, placing object...") # put 3d pt in grasping frame req.pixel3d.header.stamp = rospy.Time(0) place_pt = self.tl.transformPoint("/torso_lift_link", req.pixel3d) # move to place setup position setup_goal = OverheadGraspSetupGoal() setup_goal.disable_head = False setup_goal.open_gripper = False self.grasp_setup_client.send_goal(setup_goal) self.grasp_setup_client.wait_for_result() ############################################################ # Creating place goal grasp_goal = OverheadGraspGoal() grasp_goal.is_grasp = False grasp_goal.disable_head = False grasp_goal.disable_coll = False grasp_goal.grasp_type = OverheadGraspGoal.MANUAL_GRASP grasp_goal.x = place_pt.point.x grasp_goal.y = place_pt.point.y grasp_goal.behavior_name = "overhead_grasp" grasp_goal.sig_level = 0.999 ############################################################ # place object self.grasp_client.send_goal(grasp_goal) self.grasp_client.wait_for_result() result = self.grasp_client.get_result() rospy.loginfo("[place_object] Place result: %s" % result) obj_in_hand = result.grasp_result == "Object placed" # go back to grasp setup setup_goal.open_gripper = True self.grasp_setup_client.send_goal(setup_goal) self.grasp_setup_client.wait_for_result() return ButtonActionResponse()
class NavHeadController: def __init__(self): self.last_vel_time = rospy.Time(0) # pose and lock self.x = 1.0 self.y = 0.0 self.mutex = Lock() self.listener = TransformListener() self.client = actionlib.SimpleActionClient("head_controller/point_head", PointHeadAction) self.client.wait_for_server() self.plan_sub = rospy.Subscriber("/safe_teleop_base/local_plan", Path, self.planCallback) self.vel_sub = rospy.Subscriber("/teleop/cmd_vel/unsafe", Twist, self.velCallback) def velCallback(self, msg): vel = msg.linear.x * msg.linear.x vel += msg.linear.y * msg.linear.y vel += msg.angular.z * msg.angular.z if vel > 0.0: self.last_vel_time = rospy.Time.now() def planCallback(self, msg): # get the goal pose_stamped = msg.poses[-1] pose = pose_stamped.pose # look ahead one meter R = quaternion_matrix([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]) point = [1, 0, 0, 1] M = R*point p = PointStamped() p.header.frame_id = pose_stamped.header.frame_id p.header.stamp = rospy.Time(0) p.point.x += pose_stamped.pose.position.x + M[0,0] p.point.y += pose_stamped.pose.position.y + M[1,0] p.point.z += pose_stamped.pose.position.z + M[2,0] # transform to base_link p = self.listener.transformPoint("base_link", p) # update with self.mutex: if p.point.x < 0.65: self.x = 0.65 else: self.x = p.point.x if p.point.y > 0.5: self.y = 0.5 elif p.point.y < -0.5: self.y = -0.5 else: self.y = p.point.y def loop(self): while not rospy.is_shutdown(): if abs((rospy.Time.now() - self.last_vel_time).to_sec()) < 1.0: goal = PointHeadGoal() goal.target.header.stamp = rospy.Time.now() goal.target.header.frame_id = "base_link" with self.mutex: goal.target.point.x = self.x goal.target.point.y = self.y self.x = 1 self.y = 0 goal.target.point.z = 0.0 goal.min_duration = rospy.Duration(1.0) self.client.send_goal(goal) self.client.wait_for_result() with self.mutex: goal.target.point.x = self.x goal.target.point.y = self.y self.x = 1 self.y = 0 goal.target.point.z = 0.75 self.client.send_goal(goal) self.client.wait_for_result() else: rospy.sleep(1.0)
class NavHeadController: def __init__(self): # enables self.enable_tilt = True self.enable_look = True self.has_goal = False # pose and lock self.x = 1.0 self.y = 0.0 self.mutex = Lock() self.listener = TransformListener() self.client = actionlib.SimpleActionClient( 'head_controller/point_head', PointHeadAction) self.client.wait_for_server() self.service = rospy.Service('~configure', TiltControl, self.serviceHandler) self.plan_sub = rospy.Subscriber( 'move_base/TrajectoryPlannerROS/local_plan', Path, self.planCallback) self.stat_sub = rospy.Subscriber('move_base/status', GoalStatusArray, self.statCallback) def serviceHandler(self, req): self.enable_tilt = req.enable_tilt self.enable_look = req.enable_look and self.enable_tilt # tilt has to be enabled to look return TiltControlResponse() def statCallback(self, msg): goal = False for status in msg.status_list: if status.status == GoalStatus.ACTIVE: goal = True break self.has_goal = goal def planCallback(self, msg): # Only need to do this if we are looking if not self.enable_look: return # get the goal pose_stamped = msg.poses[-1] pose = pose_stamped.pose # look ahead one meter R = quaternion_matrix([ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ]) point = [1, 0, 0, 1] M = R * point p = PointStamped() p.header.frame_id = pose_stamped.header.frame_id p.header.stamp = rospy.Time(0) p.point.x += pose_stamped.pose.position.x + M[0, 0] p.point.y += pose_stamped.pose.position.y + M[1, 0] p.point.z += pose_stamped.pose.position.z + M[2, 0] # transform to base_link p = self.listener.transformPoint('base_link', p) # update with self.mutex: if p.point.x < 0.65: self.x = 0.65 else: self.x = p.point.x if p.point.y > 0.5: self.y = 0.5 elif p.point.y < -0.5: self.y = -0.5 else: self.y = p.point.y def loop(self): while not rospy.is_shutdown(): if self.has_goal and self.enable_tilt: goal = PointHeadGoal() goal.target.header.stamp = rospy.Time.now() goal.target.header.frame_id = 'base_link' with self.mutex: goal.target.point.x = self.x goal.target.point.y = self.y self.x = 1 self.y = 0 goal.target.point.z = 0.75 goal.min_duration = rospy.Duration(1.0) self.client.send_goal(goal) self.client.wait_for_result() with self.mutex: goal.target.point.x = self.x goal.target.point.y = self.y self.x = 1 self.y = 0 goal.target.point.z = 0.0 self.client.send_goal(goal) self.client.wait_for_result() else: rospy.sleep(1.0)
class NavHeadController: def __init__(self): self.last_vel_time = rospy.Time(0) # pose and lock self.x = 1.0 self.y = 0.0 self.mutex = Lock() self.listener = TransformListener() self.client = actionlib.SimpleActionClient( "head_controller/point_head", PointHeadAction) self.client.wait_for_server() self.plan_sub = rospy.Subscriber("/safe_teleop_base/local_plan", Path, self.planCallback) self.vel_sub = rospy.Subscriber("/teleop/cmd_vel/unsafe", Twist, self.velCallback) def velCallback(self, msg): vel = msg.linear.x * msg.linear.x vel += msg.linear.y * msg.linear.y vel += msg.angular.z * msg.angular.z if vel > 0.0: self.last_vel_time = rospy.Time.now() def planCallback(self, msg): # get the goal pose_stamped = msg.poses[-1] pose = pose_stamped.pose # look ahead one meter R = quaternion_matrix([ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ]) point = [1, 0, 0, 1] M = R * point p = PointStamped() p.header.frame_id = pose_stamped.header.frame_id p.header.stamp = rospy.Time(0) p.point.x += pose_stamped.pose.position.x + M[0, 0] p.point.y += pose_stamped.pose.position.y + M[1, 0] p.point.z += pose_stamped.pose.position.z + M[2, 0] # transform to base_link p = self.listener.transformPoint("base_link", p) # update with self.mutex: if p.point.x < 0.65: self.x = 0.65 else: self.x = p.point.x if p.point.y > 0.5: self.y = 0.5 elif p.point.y < -0.5: self.y = -0.5 else: self.y = p.point.y def loop(self): while not rospy.is_shutdown(): if abs((rospy.Time.now() - self.last_vel_time).to_sec()) < 1.0: goal = PointHeadGoal() goal.target.header.stamp = rospy.Time.now() goal.target.header.frame_id = "base_link" with self.mutex: goal.target.point.x = self.x goal.target.point.y = self.y self.x = 1 self.y = 0 goal.target.point.z = 0.0 goal.min_duration = rospy.Duration(1.0) self.client.send_goal(goal) self.client.wait_for_result() with self.mutex: goal.target.point.x = self.x goal.target.point.y = self.y self.x = 1 self.y = 0 goal.target.point.z = 0.75 self.client.send_goal(goal) self.client.wait_for_result() else: rospy.sleep(1.0)
class Transformer: def __init__(self): self.__listener = TransformListener() def __del__(self): pass def transform_to(self, pose_target, target_frame="/odom_combined"): ''' Transforms the pose_target into the target_frame. :param pose_target: object to transform as PoseStamped/PointStamped/Vector3Stamped/CollisionObject/PointCloud2 :param target_frame: goal frame id :return: transformed object ''' if pose_target.header.frame_id == target_frame: return pose_target odom_pose = None i = 0 while odom_pose is None and i < 10: try: #now = rospy.Time.now() #self.__listener.waitForTransform(target_frame, pose_target.header.frame_id, now, rospy.Duration(4)) if type(pose_target) is CollisionObject: i = 0 new_co = deepcopy(pose_target) for cop in pose_target.primitive_poses: p = PoseStamped() p.header = pose_target.header p.pose.orientation = cop.orientation p.pose.position = cop.position p = self.transform_to(p, target_frame) if p is None: return None new_co.primitive_poses[i].position = p.pose.position new_co.primitive_poses[i].orientation = p.pose.orientation i += 1 new_co.header.frame_id = target_frame return new_co if type(pose_target) is PoseStamped: odom_pose = self.__listener.transformPose(target_frame, pose_target) break if type(pose_target) is Vector3Stamped: odom_pose = self.__listener.transformVector3(target_frame, pose_target) break if type(pose_target) is PointStamped: odom_pose = self.__listener.transformPoint(target_frame, pose_target) break if type(pose_target) is PointCloud2: odom_pose = self.__listener.transformPointCloud(target_frame, pose_target) break except Exception, e: print "tf error:::", e rospy.sleep(0.5) i += 1 rospy.logdebug("tf fail nr. " + str(i)) if odom_pose is None: rospy.logerr("FUUUUUUUUUUUUUU!!!! f*****g tf shit!!!!") return odom_pose
class PlaceObject: def __init__(self): self.arm = rospy.get_param("arm", "r") self.tl = TransformListener() self.perception_srv = rospy.Service( "/clickable_world/place_table_perceive", PerceiveButtons, self.do_perception) self.action_srv = rospy.Service("/clickable_world/place_object", ButtonAction, self.do_action) self.pc_capture_srv = rospy.ServiceProxy( "/table_detection/surf_seg_capture_pc", std_srvs.srv.Empty) self.table_seg_srv = rospy.ServiceProxy( "/table_detection/segment_surfaces", SegmentSurfaces) self.grasp_client = actionlib.SimpleActionClient( self.arm + '_overhead_grasp', OverheadGraspAction) self.grasp_client.wait_for_server() self.grasp_setup_client = actionlib.SimpleActionClient( self.arm + '_overhead_grasp_setup', OverheadGraspSetupAction) self.grasp_setup_client.wait_for_server() def do_perception(self, req): rospy.loginfo("[place_object] Perceiving table...") # capture a few clouds rate = rospy.Rate(5) for i in range(5): self.pc_capture_srv() rate.sleep() # segment surfaces self.surfaces = self.table_seg_srv().surfaces for i in range(len(self.surfaces)): self.surfaces[i].color.r = 256 self.surfaces[i].color.g = 0 self.surfaces[i].color.b = 256 self.surfaces[i].color.a = 256 resp = PerceiveButtonsResponse() resp.buttons = self.surfaces return resp def do_action(self, req): rospy.loginfo("[place_object] Table clicked, placing object...") # put 3d pt in grasping frame req.pixel3d.header.stamp = rospy.Time(0) place_pt = self.tl.transformPoint("/torso_lift_link", req.pixel3d) # move to place setup position setup_goal = OverheadGraspSetupGoal() setup_goal.disable_head = False setup_goal.open_gripper = False self.grasp_setup_client.send_goal(setup_goal) self.grasp_setup_client.wait_for_result() ############################################################ # Creating place goal grasp_goal = OverheadGraspGoal() grasp_goal.is_grasp = False grasp_goal.disable_head = False grasp_goal.disable_coll = False grasp_goal.grasp_type = OverheadGraspGoal.MANUAL_GRASP grasp_goal.x = place_pt.point.x grasp_goal.y = place_pt.point.y grasp_goal.behavior_name = "overhead_grasp" grasp_goal.sig_level = 0.999 ############################################################ # place object self.grasp_client.send_goal(grasp_goal) self.grasp_client.wait_for_result() result = self.grasp_client.get_result() rospy.loginfo("[place_object] Place result: %s" % result) obj_in_hand = result.grasp_result == "Object placed" # go back to grasp setup setup_goal.open_gripper = True self.grasp_setup_client.send_goal(setup_goal) self.grasp_setup_client.wait_for_result() return ButtonActionResponse()
class FollowFace: def __init__(self): self.moving = False self.listener = TransformListener() self.broadcaster = TransformBroadcaster() self.client = actionlib.SimpleActionClient("head_controller/point_head", PointHeadAction) self.client.wait_for_server() self.face_sub = rospy.Subscriber("face_detector/people_tracker_measurements_array", PositionMeasurementArray, self.faceCallback) self.lastFaceCallback = time.time() self.lastHeadTarget = None def faceCallback(self, msg): self.lastFaceCallback = time.time() # Find closest face to look at closestFace = None closestDistance = 999999999 for face in msg.people: pos = PointStamped() pos.header = face.header pos.point = face.pos try: pos = self.listener.transformPoint("base_link", pos) except ExtrapolationException: return distance = math.sqrt(pos.point.x ** 2 + pos.point.y ** 2 + pos.point.z ** 2) if distance < closestDistance: closestFace = pos closestDistance = distance if closestFace is None: return goal = PointHeadGoal() goal.min_duration = rospy.Duration(0.0) goal.target = closestFace distance = 999999999 if self.lastHeadTarget is not None: distance = math.sqrt( (closestFace.point.x - self.lastHeadTarget.point.x) ** 2 + (closestFace.point.y - self.lastHeadTarget.point.y) ** 2 + (closestFace.point.z - self.lastHeadTarget.point.z) ** 2 ) # Prevents jitter from insignificant face movements if distance > 0.02: self.lastHeadTarget = goal.target self.client.send_goal(goal) self.client.wait_for_result() def loop(self): while not rospy.is_shutdown(): # Reset head if no face found in 1 second if time.time() - self.lastFaceCallback > 1: self.lastHeadTarget = None # Reset head pos = PointStamped() pos.header.stamp = rospy.Time.now() pos.header.frame_id = "base_link" pos.point.x = 1 pos.point.y = 0 pos.point.z = 1.5 goal = PointHeadGoal() goal.min_duration = rospy.Duration(0.5) goal.target = pos #self.client.cancel_all_goals() self.client.send_goal(goal) rospy.sleep(0.1)
class NavHeadController: def __init__(self): # enables self.enable_tilt = True self.enable_look = True self.has_goal = False # pose and lock self.x = 1.0 self.y = 0.0 self.mutex = Lock() self.listener = TransformListener() self.client = actionlib.SimpleActionClient('head_controller/point_head', PointHeadAction) self.client.wait_for_server() self.service = rospy.Service('~configure', TiltControl, self.serviceHandler) self.plan_sub = rospy.Subscriber('move_base/TrajectoryPlannerROS/local_plan', Path, self.planCallback) self.stat_sub = rospy.Subscriber('move_base/status', GoalStatusArray, self.statCallback) def serviceHandler(self, req): self.enable_tilt = req.enable_tilt self.enable_look = req.enable_look and self.enable_tilt # tilt has to be enabled to look return TiltControlResponse() def statCallback(self, msg): goal = False for status in msg.status_list: if status.status == GoalStatus.ACTIVE: goal = True break self.has_goal = goal def planCallback(self, msg): # Only need to do this if we are looking if not self.enable_look: return # get the goal pose_stamped = msg.poses[-1] pose = pose_stamped.pose # look ahead one meter R = quaternion_matrix([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]) point = [1, 0, 0, 1] M = R*point p = PointStamped() p.header.frame_id = pose_stamped.header.frame_id p.header.stamp = rospy.Time(0) p.point.x += pose_stamped.pose.position.x + M[0,0] p.point.y += pose_stamped.pose.position.y + M[1,0] p.point.z += pose_stamped.pose.position.z + M[2,0] # transform to base_link p = self.listener.transformPoint('base_link', p) # update with self.mutex: if p.point.x < 0.65: self.x = 0.65 else: self.x = p.point.x if p.point.y > 0.5: self.y = 0.5 elif p.point.y < -0.5: self.y = -0.5 else: self.y = p.point.y def loop(self): while not rospy.is_shutdown(): if self.has_goal and self.enable_tilt: goal = PointHeadGoal() goal.target.header.stamp = rospy.Time.now() goal.target.header.frame_id = 'base_link' with self.mutex: goal.target.point.x = self.x goal.target.point.y = self.y self.x = 1 self.y = 0 goal.target.point.z = 0.75 goal.min_duration = rospy.Duration(1.0) self.client.send_goal(goal) self.client.wait_for_result() with self.mutex: goal.target.point.x = self.x goal.target.point.y = self.y self.x = 1 self.y = 0 goal.target.point.z = 0.0 self.client.send_goal(goal) self.client.wait_for_result() else: rospy.sleep(1.0)