Python TransformListener.transformPointCloud方法代码示例

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import transformPointCloud [as 别名]
 collision_operation.object2 = coll_map_res.collision_support_surface_name
 collision_operation.operation = CollisionOperation.DISABLE
 ordered_collision_operations.collision_operations = [collision_operation]
 
 if not move_arm_to_grasping_joint_pose(joint_names, joint_positions, allowed_contacts, gripper_paddings, ordered_collision_operations):
     exit(1)
     
 rospy.loginfo('Pickup stage has successfully finished. Will place the object now')
 
 ############################################################################
 ####################### PLACE STAGE START HERE #############################
 
 listener = TransformListener()
 
 # move grasped object and find a good grasp do we can approach and place
 obj_pcluster = listener.transformPointCloud('base_link', coll_map_res.graspable_objects[0].cluster)
 x = [point.x for point in obj_pcluster.points]
 y = [point.y for point in obj_pcluster.points]
 z = [point.z for point in obj_pcluster.points]
 offset = [0.0, -0.2, 0.0]
 
 req = GraspPlanningRequest()
 req.arm_name = 'left_arm'
 req.target.cluster.header.frame_id = 'base_link'
 req.target.cluster.points = [Point32(x[i]+offset[0], y[i]+offset[1], z[i]+offset[2]) for i in range(len(x))]
 req.target.type = GraspableObject.POINT_CLUSTER
 req.collision_object_name = coll_map_res.collision_object_names[0]
 req.collision_support_surface_name = coll_map_res.collision_support_surface_name
 
 rospy.loginfo('trying to find a good grasp for graspable object %s' % coll_map_res.collision_object_names[0])
 grasping_result = grasp_planning_srv(req)

# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import transformPointCloud [as 别名]
class Filter_PointCloud(object):
    """ 
    The Filter_PointCloud class filters erroneous cloud data while it's still in
    2D space.
    """
    
    def __init__(self):
        """ Start the mapper. """

        rospy.init_node('filter_pointcloud')

        self.tf = TransformListener()

        # Setting the queue_size to 1 will prevent the subscriber from
        # buffering cloud messages.  This is important because the
        # callback is likely to be too slow to keep up with the cloud
        # messages. If we buffer those messages we will fall behind
        # and end up processing really old clouds.  Better to just drop
        # old clouds and always work with the most recent available.
        rospy.Subscriber('/aries/front_pointcloud',
                         PointCloud, self.cloud_callback, queue_size=1)

        self.current_cloud = PointCloud() # current cloud message
        self.received_cloud = False      # True if we've received a new cloud, false if not

        # Creates publisher for filtered point cloud topic
        self._cloud_pub = rospy.Publisher('/aries/filtered_front_pointcloud', PointCloud, queue_size=10)


    def cloud_callback(self, cloud):
        '''
        This function is called everytime a message is transmitted over /aries/front_pointcloud topic
        '''
        # Update current cloud
        self.current_cloud = cloud
        # Set received cloud flag to True
        self.received_cloud = True

    def process_cloud(self, cloud):
        self.received_cloud = False

        # Extracts coordinates into numpy arrays
        y = np.array([v.x for v in cloud.points])
        x = np.array([v.y for v in cloud.points])

        # Set number of standard deviations to allow coordinates to vary within
        nStd = 4

        N = len(x)
        stop = False

        # Containers for discarded points
        #outlierX = np.array([])
        #outlierY = np.array([])
        
        # Iteratively removes outliers and fine tunes the regression line.
        while not stop and N > 0:
            A = np.vstack([x, np.ones(len(x))]).T
            # Performs linear regression
            slope, intercept = np.linalg.lstsq(A, y)[0]

            # Generates best-fit line
            yLinear = slope*x + intercept

            # Calculate errors of the points
            errors = y - yLinear

            # Determines standard deviation of the errors
            sigErr = np.std(errors)

            # Converts errors into multiples of standard deviations
            magError = (np.absolute(errors)/sigErr)
            
            # Finds the largest outlier and its index
            val = np.amax(magError)
            ind = np.argmax(magError)
            
            # Checks if the largest outlier is outside of the specified bounds
            if(val > nStd):
                # Removes the outlier point
                N -= 1
                #outlierX = np.append(outlierX, x[ind])
                #outlierY = np.append(outlierY, y[ind])
                x = np.delete(x, ind)
                y = np.delete(y, ind)
                # print str(val) + " " + str(nStd)
            else:
                # All remaining points lie within boundaries, exit the loop
                stop = True

        # Updates the cloud message with the new coordinates, minus the outliers/noise
        cloud.points = [Point32(x=outY, y=outX, z=0) for outX, outY in zip(x, y)]

        # Transforms the point cloud into the /map frame for mapping
        self.tf.waitForTransform("/front_laser", "/map", rospy.Time(0), rospy.Duration(4.0))
        cloud = self.tf.transformPointCloud("/map", cloud)

        # Only points with potential obstacles need to be mapped.
        # This removes points from the point cloud within safe z-height ranges
        newPoints = []
#.........这里部分代码省略.........