eric-wieser/ros_numpy

IndexError: index is out of bounds for axis 0 with size 3

germal opened this issue · 3 comments

Hello
I have found the following code that assign to a rgb image the related pointcloud of the object detected ( the topic /camera/depth/points) .
I have the error

if math.isnan(array[path_points[i][j][0]][path_points[i][j][1]][0]) or
IndexError: index 124 is out of bounds for axis 0 with size 3

related to the following ros_numpy array

array=rosnp.point_cloud2.pointcloud2_to_xyz_array(self.cloud_msg,False)

Could you please help me to debug ?
Thanks a lot

#!/usr/bin/env python3

import os, sys
import numpy as np
import math
import statistics
import matplotlib.pyplot as plt
from matplotlib.path import Path

import rospy
import ros_numpy as rosnp

from sensor_msgs.msg import PointCloud2
from std_msgs.msg import String

from yolact_ros_msgs.msg import Detections
from yolact_ros_msgs.msg import Detection
from yolact_ros_msgs.msg import Box
from yolact_ros_msgs.msg import Mask
from yolact_ros_msgs.msg import Mask_Depth, Mask_Coordinates

import skimage.data
#Debug printing whole array
np.set_printoptions(threshold=sys.maxsize)

class MaskTo3D(object):

nok_class = ['person','car','bycicle','motorcycle','airplane','bus','train','truck','boat','traffic light','fire hydrant'
            'stop sign','parking meter','sheep','cow','elephant','bear','zebra','giraffe','sheep','horse']
ok_class = []

#change to ros.get_param
depth_topic="/camera/depth/points"



def __init__(self):
    rospy.init_node("depth_mask",anonymous=True)
    mask_msg="/yolact_ros/detections"
    self.pointcloud_sub=rospy.Subscriber("/camera/depth/color/points", PointCloud2, self.pcl_callback)
    self.sub_mask_msg=rospy.Subscriber(mask_msg,Detections,self.mask_msg_callback)
    self.pub_mask_depth=rospy.Publisher("/mask_coordinates", Mask_Depth, queue_size=1)
    self.cloud_msg=PointCloud2()
    self.rate=rospy.Rate(10)
    #fig,self.ax=plt.subplots()
    #plt.show(block=True)


def pcl_callback(self,req):
    #get the data of the PointCloud from RGBD camera
    self.cloud_msg=req

def mask_msg_callback(self,req):
    img=skimage.data.chelsea()
    array=rosnp.point_cloud2.pointcloud2_to_xyz_array(self.cloud_msg,False)
    label_store,score_store,box_store,mask_store,path_points =[],[],[],[],[]

    #custom msg creation with 3D coordinates of each pixel in each masks
    message=Mask_Depth()
    message.header.stamp=rospy.Time.now()

    rospy.loginfo("Parsing detections")
    for item in req.detections:
        label_store.append(item.class_name)
        score_store.append(item.score)
        box_store.append(item.box)
        mask_store.append(item.mask)

    for i in range(len(label_store)):
        #print(box_store[i].x1,box_store[i].x2, box_store[i].y1,box_store[i].y2)
        #Think of retrieving img size from Image topic
        X,Y = np.mgrid[:img.shape[1], :img.shape[0]]
        points = np.vstack((X.ravel(),Y.ravel())).T
        vertices = np.asarray([(box_store[i].x1,box_store[i].y1),
                               (box_store[i].x1,box_store[i].y2),
                               (box_store[i].x2,box_store[i].y1),
                               (box_store[i].x2,box_store[i].y2)])
        path = Path(vertices)        
        mask=path.contains_points(points)
        path_points.append(points[np.where(mask)])
        #print(type(path_points))
        #print(path_points[0][0][0])
        #print(path_points[0][0][1])
        #print(len(label_store),len(score_store),len(box_store),len(mask_store),len(path_points))


    for i in range(len(label_store)):
         #create custom message with 3D points of mask
        mask_message = Mask_Coordinates()
        mask_message.class_name = label_store[i]
        mask_message.score = score_store[i]

        list_x,list_y,list_z = [],[],[]
        score=score_store[i]
        for j in range(0,len(path_points[i])):
        #    print(array[path_points[i][j][0]][path_points[i][j][1]][0])
        #    print(path_points[i][j][0])
        #    print(path_points[i][j][1])
            if math.isnan(array[path_points[i][j][0]][path_points[i][j][1]][0]) or \
               math.isnan(array[path_points[i][j][0]][path_points[i][j][1]][1]) or \
               math.isnan(array[path_points[i][j][0]][path_points[i][j][1]][2]):
                pass
            else:
                list_x.append(array[path_points[i][j][0]][path_points[i][j][1]][0])
                list_y.append(array[path_points[i][j][0]][path_points[i][j][1]][1])
                list_z.append(array[path_points[i][j][0]][path_points[i][j][1]][2])
        
        mask_message.xcoord = list_x
        mask_message.ycoord = list_y
        mask_message.zcoord = list_z
        print(list_z)
    self.pub_mask_depth.publish(message)

if name=="main":
a=MaskTo3D()
while not rospy.is_shutdown():
a.rate.sleep()

Thanks

printing the ros_numpy array just after the pointcloud assignment

array=rosnp.point_cloud2.pointcloud2_to_xyz_array(self.cloud_msg,False)

I discovered that it has only 2 dimensions and the second axis has indeed only 3 elements , as the error states .
Finally I found out that the realsense pointcloud topic I was using wasn't suitable for the ros_numpy array , so I used the depth_registered pointcloud and now my script works .

Regards

I found that the right pointcloud is the depth_registered/points

solved