laser_assemble.py 文件源码

def run(self):
        """ Run our code """
        rospy.loginfo("Start laser test code")
        rospy.wait_for_service("assemble_scans2")

        # Todo - publish the spin logic...
        self.laser_publisher = rospy.Publisher("/multisense/lidar_cloud", PointCloud, queue_size=10)
        self.scan_service = rospy.ServiceProxy('assemble_scans', AssembleScans)
        #self.bridge = CvBridge()

        #self.zarj_os.zps.look_down()

        while True:
            begin = rospy.get_rostime()
            rospy.sleep(3.0)
            resp = self.scan_service(begin, rospy.get_rostime())
            self.laser_publisher.publish(resp.cloud)
            #print "Got cloud {}x{}".format(resp.cloud.height, resp.cloud.width)
            img = self.create_image_from_cloud(resp.cloud.points)
            cv2.destroyAllWindows()
            print "New image"
            cv2.imshow("My image", img)
            cv2.waitKey(1)
            #print resp
            #cv_image = self.bridge.imgmsg_to_cv2(resp.cloud, "bgr8")
            #cv2.imshow("Point cloud", cv_image)