版权声明:本文为博主原创文章,未经博主允许不得转载。 https://blog.csdn.net/flyfish1986/article/details/86021068
机器人 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