机器人 Grasp Pose Detection (GPD) select_grasp.py 问题

flyfish

问题描述

python santiago_select_grasp.py 
[ERROR] [1546838582.884550]: bad callback: <function cloudCallback at 0x7f08de6d4938>
Traceback (most recent call last):
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/topics.py", line 750, in _invoke_callback
    cb(msg)
  File "santiago_select_grasp.py", line 14, in cloudCallback
    cloud.append([p[0], p[1], p[2]])
AttributeError: 'numpy.ndarray' object has no attribute 'append'

Traceback (most recent call last):
  File "santiago_select_grasp.py", line 40, in <module>
    C, _, _, _ = lstsq(A, X[:,2])
  File "/usr/lib/python2.7/dist-packages/scipy/linalg/basic.py", line 1154, in lstsq
    a1 = _asarray_validated(a, check_finite=check_finite)
  File "/usr/lib/python2.7/dist-packages/scipy/_lib/_util.py", line 238, in _asarray_validated
    a = toarray(a)
  File "/home/pumpkinking/anaconda2/lib/python2.7/site-packages/numpy/lib/function_base.py", line 461, in asarray_chkfinite
    "array must not contain infs or NaNs")
ValueError: array must not contain infs or NaNs

源码更改如下

import rospy
from sensor_msgs.msg import PointCloud2
from sensor_msgs import point_cloud2
import numpy as np
cloud = [] # global variable to store the point cloud

def cloudCallback(msg):
    global cloud
    if len(cloud) == 0:
        for p in point_cloud2.read_points(msg):
         
            cloud.append([p[0], p[1], p[2]])


# Create a ROS node.
rospy.init_node('select_grasp')

# Subscribe to the ROS topic that contains the grasps.
cloud_sub = rospy.Subscriber('/camera/depth/points', PointCloud2, cloudCallback)

# Wait for point cloud to arrive.
while len(cloud) == 0:
    rospy.sleep(0.01)


# Extract the nonplanar indices. Uses a least squares fit AX = b. Plane equation: z = ax + by + c.
import numpy as np
from scipy.linalg import lstsq

cloud = np.asarray(cloud)
cloud=np.nan_to_num(cloud)
X = cloud



A = np.c_[X[:,0], X[:,1], np.ones(X.shape[0])]
C, _, _, _ = lstsq(A, X[:,2])
a, b, c, d = C[0], C[1], -1., C[2] # coefficients of the form: a*x + b*y + c*z + d = 0.
dist = ((a*X[:,0] + b*X[:,1] + d) - X[:,2])**2
err = dist.sum()
idx = np.where(dist > 0.01)


# Publish point cloud and nonplanar indices.
from gpd.msg import CloudIndexed
from std_msgs.msg import Header, Int64
from geometry_msgs.msg import Point

pub = rospy.Publisher('cloud_indexed', CloudIndexed, queue_size=1)

msg = CloudIndexed()
header = Header()
header.frame_id = "/camera_link"
header.stamp = rospy.Time.now()



msg.cloud_sources.cloud = point_cloud2.create_cloud_xyz32(header, cloud.tolist())
msg.cloud_sources.view_points.append(Point(0,0,0))
for i in xrange(cloud.shape[0]):
    msg.cloud_sources.camera_source.append(Int64(0))
for i in idx[0]:
    msg.indices.append(Int64(i))    
#s = raw_input('Hit [ENTER] to publish')
pub.publish(msg)
rospy.sleep(2)
print 'Published cloud with', len(msg.indices), 'indices'


# Select a grasp for the robot to execute.
from gpd.msg import GraspConfigList

grasps = [] # global variable to store grasps

def callback(msg):
    global grasps
    grasps = msg.grasps

# Subscribe to the ROS topic that contains the grasps.
grasps_sub = rospy.Subscriber('/detect_grasps/clustered_grasps', GraspConfigList, callback)

# Wait for grasps to arrive.
rate = rospy.Rate(1)

while not rospy.is_shutdown():    
    if len(grasps) > 0:
        rospy.loginfo('Received %d grasps.', len(grasps))
        break

grasp = grasps[0] # grasps are sorted in descending order by score
print(grasp)
print 'Selected grasp with score:', grasp.score

机器人 Grasp Pose Detection (GPD) select_grasp.py 问题

猜你喜欢