XYZ от Pointcloud - PullRequest
Новая
       19

XYZ от Pointcloud

0 голосов
/ 03 апреля 2019

Я использую узел ros для камеры ensenso. Из этого я хочу прочитать данные pointcloud, которые публикуются в теме / point_cloud с типом сообщения PointCloud2, и преобразовать их в данные, которые можно использовать для подтверждения назначения для робота.

Я пытался использовать 

sensor_msgs.point_cloud2.read_points(pointCloud, 
                                    field_names=pointCloud.data, 
                                    skip_nans=True)

, который на самом деле возвращает три значения, из скрипта point_cloud2.py Я понимаю, что это на самом деле координаты x, y и z. Проблема в том, что это числа с плавающей запятой со значениями ниже 1.

Я также пытался читать данные в pointCloud.data напрямую, но это привело к необработанным несортированным данным, которые я не смогу применить. 

    def point_cloud_callback(self, pc_msg):
        rospy.logdebug("Get a point cloud")
        if self._msg_lock.acquire(False): 
            self._last_point_cloud_msg = pc_msg
            self._msg_lock.release()

    def print_pc(self):
        pointCloud = self._last_point_cloud_msg

        for point in sensor_msgs.point_cloud2.read_points(pointCloud,
                            field_names=pointCloud.data, 
                            skip_nans=True):
            pt_x = point[0]
            pt_y = point[1]
            pt_z = point[2]
            print pt_x, pt_y, pt_z

def main():
    rospy.init_node("simple_pc_subscriber")
    node = pc_subscriber()
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        node.print_pc()
        rate.sleep()

if __name__ == '__main__':
    main()

Я получаю результаты с плавающей запятой со значениями от 0 до 1. Я ожидаю получить координаты на основе пикселей, с которыми они совпадают. Разрешение камеры составляет 1280 * 1080, которое я ожидаю в качестве значений x и y, и для каждого пикселя значение глубины, представляющее координату Z, может использоваться для определения положения объекта.

...