def cloudCallback(self, data):
# print 'Time between cloud calls:', time.time() - self.cloudTime
# startTime = time.time()
self.pointCloud = data
self.transposeGripperToCamera()
# Determine location of spoon
spoon3D = [0.22, -0.050, 0]
spoon = np.dot(self.lGripperTransposeMatrix, np.array([spoon3D[0], spoon3D[1], spoon3D[2], 1.0]))[:3]
self.spoonX, self.spoonY = self.pinholeCamera.project3dToPixel(spoon)
lowX, highX, lowY, highY = self.boundingBox(0.05, 0.3, 0.05, 20, 100, 50)
points2D = [[x, y] for y in xrange(lowY, highY) for x in xrange(lowX, highX)]
try:
points3D = pc2.read_points(self.pointCloud, field_names=('x', 'y', 'z'), skip_nans=True, uvs=points2D)
gripperPoint = pc2.read_points(self.pointCloud, field_names=('x', 'y', 'z'), skip_nans=True, uvs=[[self.lGripX, self.lGripY]]).next()
except:
# print 'Unable to unpack from PointCloud2.', self.cameraWidth, self.cameraHeight, self.pointCloud.width, self.pointCloud.height
return
points3D = np.array([point for point in points3D])
self.clusterPoints = points3D
# # Perform dbscan clustering
# X = StandardScaler().fit_transform(points3D)
# labels = self.dbscan.fit_predict(X)
# # unique_labels = set(labels)
#
# # Find the point closest to our gripper and it's corresponding label
# index, closePoint = min(enumerate(np.linalg.norm(points3D - gripperPoint, axis=1)), key=operator.itemgetter(1))
# closeLabel = labels[index]
# while closeLabel == -1 and points3D.size > 0:
# np.delete(points3D, [index])
# np.delete(labels, [index])
# index, closePoint = min(enumerate(np.linalg.norm(points3D - gripperPoint, axis=1)), key=operator.itemgetter(1))
# closeLabel = labels[index]
# if points3D.size <= 0:
# return
# # print 'Label:', closeLabel
#
# # Find the cluster closest to our gripper
# self.clusterPoints = points3D[labels==closeLabel]
#
# if self.visual:
# # Publish depth features for spoon features
# self.publishPoints('spoonPoints', self.clusterPoints, g=1.0)
#
# # Publish depth features for non spoon features
# nonClusterPoints = points3D[labels!=closeLabel]
# self.publishPoints('nonSpoonPoints', nonClusterPoints, r=1.0)
self.updateNumber += 1
def process(self):
if self.last_scan is None or self.last_image is None:
return
last_image = self.last_image
last_scan = self.last_scan
scan_pc = pc2.read_points(last_scan)
image_pc = pc2.read_points(last_image)
scan_obstacles = []
tmp = Obstacle()
# Enumerate the point cloud from the laser scan to group points that belongs
# to the same obstacle.
for x, y, _ in scan_pc:
dist = x ** 2 + y ** 2
# If it's in range, the point is considered to be part of the current obstacle
if dist < self.max_range:
tmp.add_point(Point(x, y, dist))
else:
if not tmp.is_empty():
scan_obstacles.append(tmp)
tmp = Obstacle()
if not tmp.is_empty():
scan_obstacles.append(tmp)
if len(scan_obstacles) == 0:
self.cloud_in.publish(last_image)
else:
image_points = []
for x, y, z, _ in image_pc:
point = Point(x, y)
for obs in scan_obstacles:
if not obs.is_behind(point):
image_points.append((x, y, z))
msg = pc2.create_cloud_xyz32(last_image.header, image_points)
self.cloud_in.publish(msg)
self.cloud_in.publish(last_scan)
self.last_scan = None
self.last_image = None
def pc_cb(self, data):
# point_cloud2 library's read_points function returns a generator
# where each entry is a list of the fields specified
gen = pc2.read_points(data, field_names=("x", "y", "z"))
# Filepath is just /tmp right now
file_to_open = self.filepath + '/etu_points_raw.xyz'
f = open(file_to_open, 'w')
for xyz in gen:
# This creates the message to be added to the group
# and eventually published
point = XYZ()
point.x = xyz[0]
point.y = xyz[1]
point.z = xyz[2]
self.group.append(point)
# This is for writing directly to an XYZ file
to_write = '%(1)f %(2)f %(3)f\n' % {'1':xyz[0], '2':xyz[1], '3':xyz[2]}
f.write(to_write)
f.close()
# This takes the first published point cloud and sends it to
# the xyz_to_mesh.py script
if not self.printed:
self.printed = True
rospack = rospkg.RosPack()
package_path = rospack.get_path('uv_decontamination')
full_path = package_path + '/scripts/xyz_to_mesh.py'
call(["python", full_path])
def _deserialize_numpy(self, str):
"""
wrapper for factory-generated class that passes numpy module into deserialize
"""
self.deserialize_numpy(str, numpy) # deserialize (with numpy wherever possible)
# for Image msgs
if self._type == 'sensor_msgs/Image':
self.data = numpy.asarray(bridge.imgmsg_to_cv(self, desired_encoding=self.encoding)) # convert pixel data to numpy array
# for PointCloud2 msgs
if self._type == 'sensor_msgs/PointCloud2':
print 'Cloud is being deserialized...'
points = point_cloud2.read_points(self)
points_arr = numpy.asarray(list(points))
# Unpack RGB color info
_float2rgb_vectorized = numpy.vectorize(_float2rgb)
r, g, b = _float2rgb_vectorized(points_arr[:, 3])
r = numpy.expand_dims(r, 1).astype('uint8') # insert blank 3rd dimension (for concatenation)
g = numpy.expand_dims(g, 1).astype('uint8')
b = numpy.expand_dims(b, 1).astype('uint8')
# Concatenate and Reshape
pixels_rgb = numpy.concatenate((r, g, b), axis=1)
image_rgb = pixels_rgb.reshape(self.height, self.width, 3)
points_arr = points_arr[:, :3].reshape(self.height, self.width, 3).astype('float32')
# Build record array to separate datatypes -- int16 for XYZ, uint8 for RGB
image_xyzrgb = numpy.rec.fromarrays((points_arr, image_rgb), names=('xyz', 'rgb'))
self.data = image_xyzrgb
return self
def extractLabeledCloud(pointCloud_):
npts = 0
points = {}
normals = {}
xIdx = 0
yIdx = 1
zIdx = 2
nxIdx = 3
nyIdx = 4
nzIdx = 5
labelIdx = 6
for p in pc.read_points(pointCloud_, skip_nans=True, field_names=('x', 'y', 'z', 'normal_x', 'normal_y', 'normal_z', 'label')):
label = p[labelIdx]
if not label in points:
points[label] = []
normals[label] = []
points[label].append([p[xIdx], p[yIdx], p[zIdx]])
normals[label].append([p[nxIdx], p[nyIdx], p[nzIdx]])
npts = npts + 1
return [points, normals, npts]
def cloud_callback(self, cloud):
points = point_cloud2.read_points(cloud)
points_list = np.asarray(list(points))
points_arr = np.asarray(points_list)
# Unpack RGB color info
_float2rgb_vectorized = np.vectorize(_float2rgb)
r, g, b = _float2rgb_vectorized(points_arr[:, 3])
# Concatenate and Reshape
r = np.expand_dims(r, 1) # insert blank 3rd dimension (for concatenation)
g = np.expand_dims(g, 1)
b = np.expand_dims(b, 1)
points_rgb = np.concatenate((points_arr[:, 0:3], r, g, b), axis=1)
image_rgb = points_rgb.reshape(cloud.height, cloud.width, 6)
z = copy.deepcopy(image_rgb[:, :, 2]) # get depth values (I think)
image_np = copy.deepcopy(image_rgb[:, :, 3:].astype('uint8'))
#code.interact(local=locals())
# TWO-METER DISTANCE FILTER
z[np.isnan(z)] = 0.0
mask = np.logical_or(z > 2, z == 0)
for i in range(image_np.shape[2]):
image_np[:, :, i][mask] = 0
# Convert to Image msg
image_cv = cv.fromarray(image_np)
image_msg = self.bridge.cv_to_imgmsg(image_cv, encoding='bgr8')
self.pub.publish(image_msg)
def cloudCallback(self, data):
print 'Time between cloud calls:', time.time() - self.cloudTime
startTime = time.time()
self.pointCloud = data
self.transposeGripperToCamera()
# Determine location of spoon
spoon3D = [0.22, -0.050, 0]
self.spoon = np.dot(self.lGripperTransposeMatrix, np.array([spoon3D[0], spoon3D[1], spoon3D[2], 1.0]))[:3]
self.spoonX, self.spoonY = self.pinholeCamera.project3dToPixel(self.spoon)
lowX, highX, lowY, highY = self.boundingBox()
self.spoonImageData = self.imageData[lowY:highY, lowX:highX, :]
points2D = [[x, y] for y in xrange(lowY, highY) for x in xrange(lowX, highX)]
try:
points3D = pc2.read_points(self.pointCloud, field_names=('x', 'y', 'z'), skip_nans=True, uvs=points2D)
except:
print 'Unable to unpack from PointCloud2!', self.cameraWidth, self.cameraHeight, self.pointCloud.width, self.pointCloud.height
return
self.points3D = np.array([point for point in points3D])
self.updateNumber += 1
print 'Cloud computation time:', time.time() - startTime
self.cloudTime = time.time()
def callback(data): #print (data.data) print "-------------------------------------------" height = int(data.height/2) width = int(data.width) #print (data.header) data_out= pc2.read_points(data) #cloud = pc2.create_cloud_xyz32(data.header,data.data) int_data = next(data_out) #print size(data_out) g=[] for i in data_out: k=i if i[0] is None or i[1] is None or i[2] is None: pass #print "skipped" elif i[3]>0.00: #elif i[0] < 5.00 or i[1]<5.00 or i[2]<5.00 or i[3]<5.00: #pass #print "skip g.append(i) #print len(g),len(g[0]) print g # print i print "--------------------------------------------"
def cloudCallback(self, data):
# print 'Time between cloud calls:', time.time() - self.cloudTime
# startTime = time.time()
self.pointCloud = data
self.transposeBowlToCamera()
self.transposeGripperToCamera()
# Determine location of spoon
spoon3D = [0.22, -0.050, 0]
self.spoon = np.dot(self.lGripperTransposeMatrix, np.array([spoon3D[0], spoon3D[1], spoon3D[2], 1.0]))[:3]
lowX, highX, lowY, highY = self.boundingBox()
points2D = [[x, y] for y in xrange(lowY, highY) for x in xrange(lowX, highX)]
try:
points3D = pc2.read_points(self.pointCloud, field_names=('x', 'y', 'z'), skip_nans=True, uvs=points2D)
except:
print 'Unable to unpack from PointCloud2!', self.cameraWidth, self.cameraHeight, self.pointCloud.width, self.pointCloud.height
return
self.points3D = np.array([point for point in points3D])
# self.publishImageFeatures()
self.updateNumber += 1
def callback(self, ros_msg):
"""
This method is invoked each time a new ROS message is generated.
the message is of type CompressedImage, with data and format
"""
self.msg = ros_msg
# we don't need to be called again
self.subscriber.unregister()
if self.save_to_file:
# create directories if necessary
dest_dir = os.path.split(self.path)[0]
if not os.path.exists(dest_dir):
os.makedirs(dest_dir)
# write data to disk
if self.msg_type == CompressedImage:
f = open(self.path, 'w')
f.write(ros_msg.data)
f.close()
elif self.msg_type == PointCloud2:
rawpoints = numpy.array(list(point_cloud2.read_points(ros_msg, skip_nans=False)), dtype=numpy.float32)[:, :3]
notnanindices = ~numpy.isnan(rawpoints[:, 0])
f = open(self.path, 'wb')
pickle.dump((rawpoints[notnanindices], notnanindices, len(rawpoints)), f)
f.close()
else:
f = open(self.path, 'wb')
pickle.dump(ros_msg, f)
f.close()
self.done = True
def pointcloud_callback(self, msg):
pointcloud = pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=False, uvs=[])
# While there are points in the cloud to read...
if self.pc_count<self.pc_to_keep:
self.pc_count=self.pc_count+1
#else:self.store_pc.pop(0)
while True:
try:
# new point
point = next(pointcloud)
#print point
#convert point to a_star map coordinates
self.xyz_point = point
#add point to list of points
self.store_points.append(list(self.xyz_point))
#self.store_points.pop(0)
# When the last point has been processed
except StopIteration:
break
self.store_pc.append(list(self.store_points))
def callback(self, data):
if self.show:
print data.fields
xs = []
ys = []
zs = []
# For some reason, returns x in [0], y in [1], z in [2], rgb in [3]
for point in pc2.read_points(data,skip_nans=False,field_names=("rgb","x","y","z")):
xs.append(point[0])
ys.append(point[1])
zs.append(point[2])
print "Length: " + str(len(xs))
#add start
cloud = np.array([xs,ys,zs])
nans = np.isnan(cloud)
cloud[nans] = 0
cloud = cloud.transpose()
print cloud[0]
# cloud, _ = perception.calPointCloud(cloud)
#add end
#cloud = cloud.transpose()
cloud = cloud[::10]
print cloud
xs = cloud[:,0]
ys = cloud[:,1]
zs = cloud[:,2]
#segmentation(cloud)
# print xs
# print ys
print "length after down sample: "+str(len(zs))
if len(xs) > 0:
self.show = False
print "Saving to file: " + SAVE_LOCATION
np.savez(SAVE_LOCATION, xs, ys, zs)
print 'done saving, you can exit now with CTRL-C'
def find_centroid(self, request):
'''Computes the average point in a point cloud. '''
points = pc2.read_points(
request.cluster.pointcloud,
field_names=['x', 'y', 'z'],
skip_nans=True,
)
num_points = 0
avg_x = 0
avg_y = 0
avg_z = 0
for x, y, z in points:
num_points += 1
avg_x += x
avg_y += y
avg_z += z
if num_points > 0:
avg_x /= num_points
avg_y /= num_points
avg_z /= num_points
rospy.loginfo('Centroid: ({}, {}, {})'.format(avg_x, avg_y, avg_z))
centroid = PointStamped(
point=Point(x=avg_x, y=avg_y, z=avg_z),
header=Header(
frame_id=request.cluster.header.frame_id,
stamp=rospy.Time.now(),
)
)
return FindCentroidResponse(centroid=centroid)
def pointCloudCallback(msg: PointCloud2):
''' Process point cloud data (generated from the laser) '''
# Store robot position in map grid coordinates
robot_map_coord = np.array(
[round((MAP_WIDTH/2+robot_pose.x)/MAP_RESOLUTION), # Col
round((MAP_HEIGHT/2-robot_pose.y)/MAP_RESOLUTION)], # Row
dtype=int)
for p in point_cloud2.read_points(msg, field_names=("x", "y", "z"),
skip_nans=True):
# Get obtained value in the map grid (columm and row) - must be uint8.
obstacle_map = np.array(
[round((MAP_WIDTH/2+p[0])/MAP_RESOLUTION), # Col / x
round((MAP_HEIGHT/2-p[1])/MAP_RESOLUTION)], # Row / y
dtype=np.int)
# Update map using the line iterator for free space
it = createLineIterator(robot_map_coord, # Start point
obstacle_map, # End point
occ_map)
for pt in it[:-1]:
occ_map.itemset((pt[1], pt[0]), clipValue(pt[2]+CELL_DELTA_UP,
MIN_CELL_VALUE,
MAX_CELL_VALUE))
# Update map using the line iterator for occupied space, if
# applicable
occ_map.itemset((it[-1][1], it[-1][0]),
clipValue(it[-1][2]-CELL_DELTA_DOWN,
MIN_CELL_VALUE,
MAX_CELL_VALUE))
def get_nearest_cloud(self, msg):
points = list()
# Get all the points in the visible cloud (may be prefiltered by other nodes)
for point in point_cloud2.read_points(msg, skip_nans=True):
points.append(point[:3])
# Convert to a numpy array
points_arr = np.float32([p for p in points]).reshape(-1, 1, 3)
# Compute the COG
cog = np.mean(points_arr, 0)
# Abort if we get an NaN in any component
if np.isnan(np.sum(cog)):
return
# Store the COG in a ROS Point object
cog_point = Point()
cog_point.x = cog[0][0]
cog_point.y = cog[0][1]
cog_point.z = cog[0][2]
# Give the COG a unit orientation and store as a PoseStamped message
target = PoseStamped()
target.header.stamp = rospy.Time.now()
target.header.frame_id = msg.header.frame_id
target.pose.position = cog_point
target.pose.orientation.w = 1.0
# Publish the PoseStamped message on the /target_pose topic
self.target_pub.publish(target)
def k_means(self, msg):
## Perform K Means on the data
#
# @param msg A sensor_msgs/PointCloud2 message with num_means clusters
starttime = time.clock()
points = pc2.read_points(msg, field_names=["x","y","z","rgb"], skip_nans=True)
data = []
for point in points:
data += [point[0:3]]
data = numpy.array(data)
kmeans = KM(n_clusters = self.num_means)
kmeans.fit(data)
print kmeans.cluster_centers_
centers = sorted(kmeans.cluster_centers_, key=lambda p: p[0])
markers = MarkerArray()
for id_num, center in enumerate(centers):
centerMarker = self.makeMarker(msg.header, id_num, center)
markers.markers.append(centerMarker)
self.pub.publish(markers)
endtime = time.clock()
if self.debug:
print >> sys.stderr, "KMeans time: " + str(endtime - starttime)
def callback(self, data):
"""Computes the average point in a point cloud and saves timing info.
"""
points = pc2.read_points(data, field_names=['x', 'y', 'z'],
skip_nans=True)
num_points = 0
avg_x = 0
avg_y = 0
avg_z = 0
for x, y, z in points:
num_points += 1
avg_x += x
avg_y += y
avg_z += z
if num_points > 0:
avg_x /= num_points
avg_y /= num_points
avg_z /= num_points
current_time = rospy.Time.now()
self._time_taken += current_time - self._prev_time
self._prev_time = current_time
self._num_calls += 1
rospy.loginfo('x: {}, y: {}, z: {}, time/point: {}s'.format(
avg_x, avg_y, avg_z, self._time_taken.to_sec() / self._num_calls))
def callbackScan(laserScan):
global iniX
global distX
global espacoVazio
global estadoCont
point_cloud = laser_projector.projectLaser(laserScan)
minY = 0
maxY = 0
# calcula os pontos mínimo e máximo de detecção do laser
for p in pc2.read_points(point_cloud, field_names = ("x", "y", "z"), skip_nans=True):
if p[1] < minY:
minY = p[1]
if p[1] > maxY:
maxY = p[1]
# Se o laser não detectar obstáculo, inicia a contagem do espaço vazio
if (minY+maxY)/2 < -0.1 and estadoCont == 0:
estadoCont = 1
iniX = posX
# Se o laser detectar obstáculo, finaliza a contagem do espaço vazio
elif (minY+maxY)/2 > 0.1 and estadoCont == 1:
estadoCont = 0
espacoVazio = distX
# Se o estadoCont == 1, há um espaço vazio sendo mensurado
elif estadoCont == 1:
distX = posX - iniX
rospy.loginfo("oriZ: %.1f dist: %.1f espaco: %.1f" %(oriZ, distX, espacoVazio))
def get_nearest_cloud(self, msg):
points = list()
points_xy = list()
# Get all the points in the visible cloud (may be prefiltered by other nodes)
for point in point_cloud2.read_points(msg, skip_nans=True):
points.append(point[:3])
points_xy.append(point[:2])
# Convert to a numpy array
points_arr = np.float32([p for p in points]).reshape(-1, 1, 3)
# Compute the COG
cog = np.mean(points_arr, 0)
# Convert to a Point
cog_point = Point()
cog_point.x = cog[0][0]
cog_point.y = cog[0][1]
cog_point.z = cog[0][2]
#cog_point.z = 0.35
# Abort if we get an NaN in any component
if np.isnan(np.sum(cog)):
return
# If we have enough points, find the best fit ellipse around them
try:
if len(points_xy) > 6:
points_xy_arr = np.float32([p for p in points_xy]).reshape(-1, 1, 2)
track_box = cv2.fitEllipse(points_xy_arr)
else:
# Otherwise, find the best fitting rectangle
track_box = cv2.boundingRect(points_xy_arr)
angle = pi - radians(track_box[2])
except:
return
#print angle
# Convert the rotation angle to a quaternion
q_angle = quaternion_from_euler(0, angle, 0, axes='sxyz')
q = Quaternion(*q_angle)
q.x = 0.707
q.y = 0
q.z = 0.707
q.w = 0
# Publish the COG and orientation
target = PoseStamped()
target.header.stamp = rospy.Time.now()
target.header.frame_id = msg.header.frame_id
target.pose.position = cog_point
target.pose.orientation = q
# Publish the movement command
self.target_pub.publish(target)
def callback2(msg):
global count2
if count2 == 0:
points = list(pc2.read_points(msg, skip_nans=True))
pickle.dump(points, open('kinect2_pc2_read', 'wb'))
pickle.dump(msg, open('kinect2_pc2', 'wb'))
count2 += 1
print 'Done2'
def callback_points(self, data):
"""
Function to handle the arrival of pointcloud data
"""
cloud = list(pc2.read_points(data, skip_nans=True, field_names=("x", "y")))
cloud = np.resize(cloud, [640, 480, 2])
print cloud[100][100]
def do_transform_cloud(cloud, transform):
t_kdl = transform_to_kdl(transform)
points_out = []
for p_in in read_points(cloud):
p_out = t_kdl * PyKDL.Vector(p_in[0], p_in[1], p_in[2])
points_out.append(p_out)
res = create_cloud(transform.header, cloud.fields, points_out)
return res
def handle_pointcloud(msg):
results = []
points = pc2.read_points(msg)
for x, y, z in points:
results.extend([[x + a, y + b, z + c] for a, b, c in deltas])
pub.publish(pc2.create_cloud_xyz32(msg.header, results))
def isCloudValid(cloud):
"""
Returns whether a cloud is valid to be processed by this perception module
"""
for point in pc2.read_points(cloud,skip_nans=False,field_names=("rgb","x","y","z")):
return True
break
return False
def _cat_newcloud(self):
data = yield self._plane_subscriber.get_next_message()
if self.pointcloud is None:
self.pointcloud = data
else:
gen = list(
pc2.read_points(
data, skip_nans=True, field_names=('x', 'y', 'z')))
pc_gen = list(
pc2.read_points(
self.pointcloud,
skip_nans=True,
field_names=('x', 'y', 'z')))
concat = np.asarray(gen + pc_gen, np.float32)
print 'SONAR_POINTCLOUD - current size: {}'.format(concat.shape)
self.pointcloud = mil_ros_tools.numpy_to_pointcloud2(concat)
yield
def execute(self, userdata):
rospy.loginfo(
'Capturing item descriptor in bin {}'.format(userdata.bin_id))
rospy.loginfo(userdata.keys)
self._tts.publish('Sensing bin {}'.format(userdata.bin_id))
rospy.sleep(5)
self._tuck_arms.wait_for_service()
self._tuck_arms(tuck_left=False, tuck_right=False)
# Crop shelf.
crop_request = CropShelfRequest(cellID=userdata.bin_id)
self._crop_shelf.wait_for_service()
crop_response = self._crop_shelf(crop_request)
# Segment items.
segment_request = SegmentItemsRequest(cloud=crop_response.cloud, items=['a', 'b'])
self._segment_items.wait_for_service()
segment_response = self._segment_items(segment_request)
clusters = segment_response.clusters.clusters
userdata.clusters = clusters
rospy.loginfo('[CaptureItemDescriptor] Found {} clusters.'.format(
len(clusters)))
for i, cluster in enumerate(clusters):
points = pc2.read_points(cluster.pointcloud, skip_nans=True)
point_list = [Point(x=x, y=y, z=z) for x, y, z, rgb in points]
if len(point_list) == 0:
rospy.logwarn('Skipping cluster of size 0')
continue
viz.publish_cluster(self._markers, point_list,
'bin_{}'.format(userdata.bin_id),
'bin_{}_items'.format(userdata.bin_id), i)
self._get_item_descriptor.wait_for_service()
descriptor = self._get_item_descriptor(cluster).descriptor
rospy.loginfo('Color histogram ({} bins):\n{}'.format(
descriptor.histogram.num_bins, descriptor.histogram.histogram))
bounding_box = descriptor.planar_bounding_box
bbox_pose = bounding_box.pose
bbox_dimensions = bounding_box.dimensions
rospy.loginfo('Bounding box centroid: {}'.format(bbox_pose))
rospy.loginfo('Bounding box dimensions: {}'.format(bbox_dimensions))
viz.publish_bounding_box(
self._markers, bbox_pose, bbox_dimensions.x, bbox_dimensions.y,
bbox_dimensions.z, 0.33, 0.69, 0.31, 0.25, 1234 + i)
viz.publish_pose(self._markers, bbox_pose, 1, 0, 0, 1, 1234 + i)
action = None
while action is None:
action = raw_input('Capture [a]nother or [d]one?: ')
if action == 'a':
return outcomes.CAPTURE_ITEM_NEXT
elif action == 'd':
return outcomes.CAPTURE_ITEM_DONE
else:
action = None
def callbackScan(scan):
rospy.loginfo("Got scan, projecting")
point_cloud = laser_projector.projectLaser(scan)
for p in pc2.read_points(point_cloud, field_names = ("x", "y", "z"), skip_nans=True):
distY = p[1]
if distY < 0:
distY = 0
#print " x : %f y: %f z: %f" %(p[0],p[1],p[2])
rospy.loginfo("x: %f y: %f" %(posX, distY))
def callbackScan(laserScan, tipo):
global iniX
global distX
global espacoVazio
global estadoCont
global obstEsq
global obstTraz
global obstFrente
point_cloud = laser_projector.projectLaser(laserScan)
somaX = 0
somaY = 0
pontos = 0
# calcula os pontos mínimo e máximo de detecção do laser
for p in pc2.read_points(point_cloud, field_names = ("x", "y", "z"), skip_nans=True):
x = p[0]
y = p[1]
if x < 0:
x = 0
if y < 0:
y = 0
somaX = somaX + x
somaY = somaY + y
pontos = pontos + 1
if tipo == 'esq' and pontos > 0:
mediaX = (somaX)/pontos
mediaY = (somaY)/pontos
obstEsq = mediaX-mediaY
# Se o laser não detectar obstáculo, inicia a contagem do espaço vazio
if obstEsq > 0.15 and estadoCont == 0:
estadoCont = 1
iniX = posX
# Se o laser detectar obstáculo, finaliza a contagem do espaço vazio
elif obstEsq < 0.15 and estadoCont == 1:
estadoCont = 0
espacoVazio = distX
# Se o estadoCont == 1, há um espaço vazio sendo mensurado
elif estadoCont == 1:
distX = posX - iniX
#rospy.loginfo("obst-%s: %.1f oriZ: %.1f dist: %.1f esp: %.1f" %(tipo, obstEsq, oriZ, distX, espacoVazio))
elif (tipo == 'tr1' or tipo == 'tr2') and pontos > 0:
mediaX = (somaX)/pontos
mediaY = (somaY)/pontos
obstTraz = mediaX-mediaY
#rospy.loginfo("obst-%s: %.1f oriZ: %.1f esp: %.1f" %(tipo, obstTraz, oriZ, espacoVazio))
elif tipo == 'fre' and pontos > 0:
mediaX = (somaX)/pontos
mediaY = (somaY)/pontos
obstFrente = mediaX-mediaY
def _3_cloud_callback(message):
try:
data_out = pc2.read_points(message, field_names=None, skip_nans=True, uvs=[])
i=0
iteration1 = next(data_out) #format x,y,z,rgba
while iteration1 != None:
iteration1 = next(data_out)
i=i+1
except StopIteration:
print "(Cloud 2bottom)"
def octomap_update_callback(self, msg): # as pointcloud2.
obs_set = set()
obs_set_raw = set()
for p in pc2.read_points(msg,
field_names=("x", "y", "z"),
skip_nans=True):
#print " x : %f y: %f z: %f" % (p[0], p[1], p[2])
point = self.dg.continuous_to_discrete((p[0], p[1], p[2]))
#print ('point:',point)
obs_set.add(point)
obs_set.add(p)
acquired = self.obstacle_set_mutex.acquire(True) # blocking.
if acquired:
#print('octomap updated!')
self.driver.set_obstacle_set(obs_set)
self.obs_set_raw = obs_set_raw
self.obstacle_set_mutex.release()
self.is_obstacle_set_updated = True
return
else:
print('Lock not acquired!')
def callback_kinect(self,data) :
data_out = pc2.read_points(data, field_names=("x","y","z","intensity"), skip_nans=False)
#print(data_out)
i=0
j=0
self.id+=1
for p in data_out:
if(p[0]>-10 and p[0]<10 and p[1]>-10 and p[1]<10):
# while(i<450):
# i+=1
# j=0
# while(j<900):
# j=j+1
# self.image[:,:,0][i][j]=int(100*p[0])
# self.image[:,:,1][i][j]=int(100*p[1])
# self.image[:,:,2][i][j]=int(100*(p[2]+2))
self.image_gs[int(abs(30*(p[0]+10)))][int(abs(30*(p[1]+10)))]=255
cv2.imwrite("ima"+str(self.id)+".jpg",self.image_gs)
#self.image_gs=cv2.flip(self.image_gs,0)
# cv2.imshow("image",self.image_gs)
# cv2.waitKey(1)
self.image_gs=np.zeros((600,600))
def callback(data):
data_out = pc2.read_points(data, skip_nans=True)
loop = True
while loop:
try:
int_data = next(data_out)
s = struct.pack('>f', int_data[3])
i = struct.unpack('>l', s)[0]
pack = ctypes.c_uint32(i).value
r = (pack & 0x00FF0000) >> 16
g = (pack & 0x0000FF00) >> 8
b = (pack & 0x000000FF)
print("%d %d %d" % r, g, b)
except Exception as e:
rospy.loginfo(e.message)
loop = False
data.header.stamp = rospy.Time()
data.header.frame_id = data.header.frame_id
def detection_cb(msg, items, point_cloud_data, alvar_marker):
global kinect2_img, tf_buffer, estimator
if msg.class_id in items:
# global pose_pub
[u, v] = [int(msg.pose.x_center), int(msg.pose.y_center)]
# try:
location_xyz = list(
pc2.read_points(point_cloud_data.data, ('x', 'y', 'z'),
skip_nans=True,
uvs=[[u, v]])) # location type --> [(touple)]
if location_xyz:
converted_location = list_2_stampPose(location_xyz[0])
transform = tf_buffer.lookup_transform(
'base_footprint',
converted_location.header.frame_id, #source frame
rospy.Time(0), #get the tf at first available time
rospy.Duration(1.0)) #wait for 1 second
pose_transformed = tf2_geometry_msgs.do_transform_pose(
converted_location, transform)
alvar_marker.update(pose_transformed, msg.score)
estimator.update(pose_transformed)
publish_image(converted_location, msg)
def callback_pointcloud(data):
assert isinstance(data, PointCloud2)
gen = point_cloud2.read_points(data)
point_cnt = 0
in_vicinity = 0
for p in gen:
point_cnt += 1
if(sqrt(p[0]**2+p[1]**2) < 2):
in_vicinity += p[3]
#print p # type depends on your data type, first three entries are probably x,y,z_name__ == '__main__':
#print point_cnt
if(in_vicinity > 300):
print "STOP"
v.mode = dronekit.VehicleMode("HOLD")
else:
print "FLY"
def updatemap(self, pcl, range_max, pose):
robot_origin=int(pose[0])+int(math.ceil(self.width/self.resolution)*pose[1])
for p in pc2.read_points(pcl, field_names = ('x', 'y', 'z'), skip_nans = True):
rad = math.sqrt(p[0]*p[0] + p[1]*p[1])
px = int(rad*cos(pose[2])/self.resolution)
py = int(rad*sin(pose[2])/self.resolution)
l = bresenham.bresenham([0,0],[px,py])
for j in range(len(l.path)):
lpx= int(self.map_origin[0]+pose[0]/self.resolution +l.path[j][0])
lpy=int(self.map_origin[1]+pose[1]/self.resolution +l.path[j][1])
if (0<=lpx<self.width/self.
resolution and 0<=lpy<self.height/self.resolution):
index= int(lpx + math.floor(self.width/self.resolution)*(lpy))
if(j<len(l.path)-1):
self.localmap[index] = 0
else:
if rad<self.max_scan_range*range_max:
self.localmap[index] = 100
def merge_sensors(self):
# print('type', type(data))
# print("\n\n")
# tic = time.clock()
n_points = self.data_lidar.height*self.data_lidar.width
print("Number of data points {}".format(n_points))
dim = 2
self.points_lidar_2d = np.zeros((dim, n_points))
self.points_lidar_z = np.zeros((n_points))
it_count = 0
for pp in pc2.read_points(self.data_lidar, skip_nans=True, field_names=("x", "y", "z")):
# Cut off points which are to high
self.points_lidar_2d[:, it_count] = [pp[0], pp[1]]
self.points_lidar_z[it_count] = pp[2]
# print("points are x={}, y={}, z={}".format(pp[0], pp[1], pp[2]))
it_count += 1
# print('it count', it_count)
# print('n_points', n_points)
self.points_lidar_2d += self.points_lidar_2d + np.tile(self.trans_lidar[:2], (n_points, 1)).T
self.points_lidar_z += self.points_lidar_z + np.tile(self.trans_lidar[2], (n_points)).T
# TODO - only evaluate if no collision
ind_good = np.logical_and(self.points_lidar_z > 0, self.points_lidar_z < self.robot_clearance_height)
# Describe points in robot center-frame
radius_lidar = np.sqrt(self.points_lidar_2d[0, ind_good]*self.points_lidar_2d[0, ind_good] +
self.points_lidar_2d[1, ind_good]*self.points_lidar_2d[1, ind_good])
ind_good[ind_good] = (radius_lidar > self.robot_radius)
self.points_lidar_2d = self.points_lidar_2d[:, ind_good]
angle_lidar = np.arctan2(self.points_lidar_2d[1, :], self.points_lidar_2d[0, :])
def on_new_point_cloud(data):
global im
pc = pc2.read_points(data,
skip_nans=True,
field_names=("x", "y", "z", "intensity"))
#print pc.type
#print data.type
cloud_points = []
for p in pc:
cloud_points.append(p)
npc = np.array(cloud_points)
#lidar_to_2d_front_view(npc, v_res=VRES, h_res=HRES, v_fov=VFOV, val="depth", y_fudge=Y_FUDGE)
#lidar_to_2d_front_view(npc, v_res=VRES, h_res=HRES, v_fov=VFOV, val="height", y_fudge=Y_FUDGE)
#lidar_to_2d_front_view(npc, v_res=VRES, h_res=HRES, v_fov=VFOV, val="reflectance", y_fudge=Y_FUDGE)
#im = birds_eye_point_cloud(npc, side_range=(-10, 10), fwd_range=(-10, 10), res=0.1)
im = point_cloud_to_panorama(npc,
v_res=VRES,
h_res=HRES,
v_fov=VFOV,
y_fudge=5,
d_range=(0, 100))
def callback(self, raw_data, republisher):
current_time = time.time()
if self.__period < current_time - self.__previous_time:
self.__previous_time += (1 + int(
(current_time - self.__previous_time) /
self.__period)) * self.__period
points = []
for point in pc2.read_points(raw_data):
points.append(list(point[0:3]))
points_map_pcl = pcl.PointCloud(points)
pc_filter = points_map_pcl.make_voxel_grid_filter()
pc_filter.set_leaf_size(*self.__leaf_size)
points_map_pcl = pc_filter.filter()
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = self.__frame_id
downsampled_data = pc2.create_cloud_xyz32(header,
points_map_pcl.to_list())
republisher.publish(downsampled_data)
def scan_cb(msg):
# convert the message of type LaserScan to a PointCloud2
pc2_msg = lp.projectLaser(msg)
# now we can do something with the PointCloud2 for example:
# publish it
pc_pub.publish(pc2_msg)
# convert it to a generator of the individual points
point_generator = pc2.read_points(pc2_msg)
# we can access a generator in a loop
sum = 0
num = 0
for point in point_generator:
distance = ((point[0])**2 + (point[1] - 2)**2)**0.5
#print distance
if distance < 2:
sum += 1
# we can calculate the average z value for example
print(sum)
def octomap_update_callback(self, msg): # as pointcloud2.
obs_set = set()
for p in pc2.read_points(msg,
field_names=("x", "y", "z"),
skip_nans=True):
# print " x : %f y: %f z: %f" % (p[0], p[1], p[2])
point = self.dg.continuous_to_discrete((p[0], p[1], p[2]))
# print("corresponding discrete value: ", point)
obs_set.add(point)
# save points set
if (self.save_pts):
save_points3D(obs_set)
acquired = self.obstacle_set_mutex.acquire(True) # blocking.
if acquired:
# print('octomap updated!')
self.driver.set_obstacle_set(obs_set)
self.obstacle_set_mutex.release()
return
else:
print('Lock not acquired!')
def find_xyz(self, data):
# do processing here
gen = pc2.read_points(data,
field_names=("x", "y", "z"),
skip_nans=False,
uvs=[[self.rgb_h, self.rgb_v]])
target_xyz_cam = list(gen)
# do conversion to global coordinate here
listener = tf.TransformListener()
listener.waitForTransform("map", "head_rgbd_sensor_rgb_frame",
rospy.Time(0), rospy.Duration(4.0))
rgbd_point = PointStamped()
rgbd_point.header.frame_id = "head_rgbd_sensor_rgb_frame"
rgbd_point.header.stamp = rospy.Time(0)
rgbd_point.point.x = target_xyz_cam[0][0]
rgbd_point.point.y = target_xyz_cam[0][1]
rgbd_point.point.z = target_xyz_cam[0][2]
self.map_point = listener.transformPoint("map", rgbd_point)
self.found_3d = True
sub_once.unregister()
def pointcloudCallBack(PointCloud):
# self.lock.acquire()
gen = pc2.read_points(PointCloud, skip_nans=True)
int_data = list(gen)
x_bar = 0
y_bar = 0
z_bar = 0
num_points = 0
for x in int_data:
test = x[3]
# cast float32 to int so that bitwise operations are possible
s = struct.pack('>f', test)
i = struct.unpack('>l', s)[0]
# you can get back the float value by the inverse operations
pack = ctypes.c_uint32(i).value
# r,g,b values in the 0-255 range
r = (pack & 0x00FF0000) >> 16
g = (pack & 0x0000FF00) >> 8
b = (pack & 0x000000FF)
rgbList = [r, g, b]
# print rgbList
if isColorCorrect(rgbList, 'red'):
num_points += 1
# x,y,z can be retrieved from the x[0],x[1],x[2]
x_bar += x[1]
y_bar += x[2]
z_bar += x[3]
if (num_points != 0):
x_bar /= num_points
y_bar /= num_points
z_bar /= num_points
print('Marker Position')
print(num_points)
print([x_bar, y_bar, z_bar])
posi = Float64MultiArray(data=[x_bar, y_bar, z_bar])
manipulation_pub = rospy.Publisher('/manipulation_target',
Float64MultiArray,
queue_size=10)
manipulation_pub.publish(posi)
def pcl_callback(pcl_msg):
cloud = ros_to_pcl(pcl_msg)
cloud = pipeline.passth(cloud)
cloud = pipeline.passth(cloud, axis='y', amin=-0.45, amax=0.45)
cloud = pipeline.denoise(cloud)
filtered = pipeline.voxel(cloud)
table_points = pipeline.plane_points(filtered)
table_cloud = filtered.extract(table_points, negative=False)
obj_cloud = filtered.extract(table_points, negative=True)
pcl_objects_pub.publish(pcl_to_ros(obj_cloud))
pcl_table_pub.publish(pcl_to_ros(table_cloud))
cluster_ix = pipeline.cluster_ix(obj_cloud)
pcl_cluster_pub.publish(
pcl_to_ros(pipeline.color_clusters(obj_cloud, cluster_ix)))
detected_objects = []
for i, ixs in enumerate(cluster_ix):
pcl_data = obj_cloud.extract(ixs)
ros_data = pcl_to_ros(pcl_data)
feature = np.concatenate(
(features.compute_color_histograms(ros_data), ))
prediction = clf.predict(scaler.transform(feature.reshape(1, -1)))
label = encoder.inverse_transform(prediction)[0]
label_pos = list(next(pc2.read_points(ros_data))[:3])
label_pos[2] += .4
object_markers_pub.publish(make_label(label, label_pos, i))
do = DetectedObject()
do.label = label
do.cloud = ros_data
detected_objects.append(do)
detected_objects_pub.publish(detected_objects)
def callback(self, ros_point_cloud):
xyz = np.array([[0, 0, 0]])
rgb = np.array([[0, 0, 0]])
#self.lock.acquire()
gen = pc2.read_points(ros_point_cloud, skip_nans=True)
int_data = list(gen)
for x in int_data:
test = x[3]
# cast float32 to int so that bitwise operations are possible
s = struct.pack('>f', test)
i = struct.unpack('>l', s)[0]
# you can get back the float value by the inverse operations
pack = ctypes.c_uint32(i).value
r = (pack & 0x00FF0000) >> 16
g = (pack & 0x0000FF00) >> 8
b = (pack & 0x000000FF)
# prints r,g,b values in the 0-255 range
# x,y,z can be retrieved from the x[0],x[1],x[2]
xyz = np.append(xyz, [[x[0], x[1], x[2]]], axis=0)
rgb = np.append(rgb, [[r, g, b]], axis=0)
self.eval_one_epoch(self.sess, self.ops, xyz, rgb)
def chatterCallback_PCL(self, data):
x_vec=[]
y_vec=[]
z_vec=[]
# xaxis, yaxis, zaxis = (1, 0, 0), (0, 1, 0), (0, 0, 1)
# I = tf.transformations.identity_matrix()
# Rx = tf.transformations.rotation_matrix(-0.0150, xaxis)
# Ry = tf.transformations.rotation_matrix(0.0786, yaxis)
# Rz = tf.transformations.rotation_matrix(0.0121, zaxis)
# R = concatenate_matrices(Rx, Ry, Rz)
for data in pc2.read_points(data, skip_nans=True):
data2=data
x_vec.append(data2[0])
y_vec.append(data2[1])
z_vec.append(data2[2])
with open("calib_pcl.txt", mode='w') as f: # I add the mode='w'
print('Saving')
for i in range(len(x_vec)):
f.write("%f,"%float(x_vec[i]))
f.write("%f,"%float(y_vec[i]))
f.write("%f,\n"%float(z_vec[i]))
def __callback(self, cameraMsg, rgbMsg, depthMsg, cloudMsg):
frame_id = rgbMsg.header.frame_id
stamp = rgbMsg.header.stamp
rgb = convert.image2matrix(rgbMsg, encoding='bgr8')
depth = convert.image2matrix(depthMsg,
encoding='passthrough').astype(np.float32)
cloud = np.array(
list(pc2.read_points(cloudMsg,
field_names=["x", "y", "z"]))).reshape(
cloudMsg.height, cloudMsg.width, -1)
cameraMsg.K = np.array(cameraMsg.K, np.float32).reshape(3, 3)
cameraMsg.D = np.array(cameraMsg.D, np.float32)
data = {}
data["camera"] = cameraMsg
data["rgb"] = rgb
data["depth"] = depth
data["cloud"] = cloud
result = self._estimator.process(data)
if result is None:
markerPoseImage = convert.matrix2image(rgb,
frame_id,
stamp,
encoding='bgr8')
self._pubImage.publish(markerPoseImage)
return
frame, rvec, tvec = result
markerPoseImage = convert.matrix2image(frame,
frame_id,
stamp,
encoding='bgr8')
self._pubImage.publish(markerPoseImage)
markerPose = convert.axis2pose(rvec, tvec, frame_id, stamp)
self._pubMarker.publish(markerPose)
def callback_cloud(self, cloud_data):
# creating local world objects list we will publish them
world_objects = []
# iterating through every object in the class's list and find their actual world position by
# using point cloud and camera's world position
for obj in self.objects:
data_generator = pc2.read_points(cloud_data, ('x', 'y', 'z'),
False,
[(obj.center_x, obj.center_y)])
for point in data_generator:
w_obj = Object(obj.id, self.cam_x + point[0],
self.cam_y - point[1],
(self.cam_z - point[2]) / 2, obj.center_x,
obj.center_y, obj.width, obj.angle, obj.color,
obj.shape)
world_objects.append(w_obj)
#print('x: {}, y: {}, z: {}, center x: {}, center y: {}'.format(self.cam_x + point[0], self.cam_y - point[1], (self.cam_z - point[2]) / 2, obj.center_x, obj.center_y))
# create correct message type to publish to topic /objects
objs = ObjectStates(world_objects, len(world_objects))
self.object_pub.publish(objs)
def _deserialize_numpy(self, str):
"""
wrapper for factory-generated class that passes numpy module into deserialize
"""
self.deserialize_numpy(str,
numpy) # deserialize (with numpy wherever possible)
# for Image msgs
if self._type == 'sensor_msgs/Image':
self.data = numpy.asarray(
bridge.imgmsg_to_cv(self, desired_encoding=self.encoding)
) # convert pixel data to numpy array
# for PointCloud2 msgs
if self._type == 'sensor_msgs/PointCloud2':
print 'Cloud is being deserialized...'
points = point_cloud2.read_points(self)
points_arr = numpy.asarray(list(points))
# Unpack RGB color info
_float2rgb_vectorized = numpy.vectorize(_float2rgb)
r, g, b = _float2rgb_vectorized(points_arr[:, 3])
r = numpy.expand_dims(r, 1).astype(
'uint8') # insert blank 3rd dimension (for concatenation)
g = numpy.expand_dims(g, 1).astype('uint8')
b = numpy.expand_dims(b, 1).astype('uint8')
# Concatenate and Reshape
pixels_rgb = numpy.concatenate((r, g, b), axis=1)
image_rgb = pixels_rgb.reshape(self.height, self.width, 3)
points_arr = points_arr[:, :3].reshape(self.height, self.width,
3).astype('float32')
# Build record array to separate datatypes -- int16 for XYZ, uint8 for RGB
image_xyzrgb = numpy.rec.fromarrays((points_arr, image_rgb),
names=('xyz', 'rgb'))
self.data = image_xyzrgb
return self
def callback(data):
"""
Callback function to take pointcloud data outputted by *.bag test file
and perform manipulations on it, then publish the manipulated data.
"""
cm = CloudManipulations()
pointcloud = pc2.read_points(data,
skip_nans=True,
field_names=["x", "y", "z"])
for p in pointcloud:
x = p[0]
y = p[1]
z = p[2]
# Apply some linear translation (offset)
p = cm.apply_offset(p, offset)
# Rotate by some degrees on an axis
if (axis.lower() == "x"):
rotated = cm.rotate_x(rotation, False)
elif (axis.lower() == "y"):
rotated = cm.rotate_y(rotation, False)
elif (axis.lower() == "z"):
rotated = cm.rotate_z(rotation, False)
else:
print("Invalid axis ", axis.lower())
return None
# Apply the offset for rotation
tm = cm.apply_rotational_offset(rotated, x, y, z)
# Convert to point cloud
pc = cm.mat_to_point_cloud(tm, 'velodyne_edited')
# Publish the pointcloud
rospy.loginfo(pc)
pub.publish(pc)
def transform(self, point_cloud):
if not self.key:
return
print(22222222)
self.res = []
if self.target_found: #已检测到目标
# rospy.loginfo('Data acquiring')
# pc2.read_points(cloud, skip_nans=True, field_names=("x", "y", "z"))
for i in self.points:
# pc = pc2.read_points(point_cloud, field_names=("x", "y", "z"), skip_nans=True,uvs=[[self.target_x, self.target_y]])
pc = pc2.read_points(point_cloud,
field_names=("x", "y", "z"),
skip_nans=True,
uvs=[i])
int_data = list(pc) #[(x,y,z)]
# while not int_data:
# i = [j+1 for j in i]
# pc = pc2.read_points(point_cloud, field_names=("x", "y", "z"), skip_nans=True, uvs=[i])
# int_data = list(pc) # [(x,y,z)]
if int_data:
print("Camera_Position:", int_data)
self.camera_position[:3] = list(int_data[0])
self.camera_position[:3] = self.camera_position[:3] * 1000
self.workpiece_position = np.dot(
self.martix,
self.camera_position) #A*(x,y,z) = (X,Y,Z) (注意单位换算)
print(self.workpiece_position)
self.workpiece_position[
0] = self.workpiece_position[0] - 17
self.workpiece_position[2] = self.workpiece_position[2] + 1
self.final_position = self.workpiece_position[0:3]
# print("****Final****:",self.final_position)
self.res.append(self.final_position)
else:
print("坐标转换失败！")
self.target_found = False
print("###########", self.res)
def process_image_and_points(self):
if ((self.process_locked) or (not self.image_received)
or (not self.points_received)):
rp.loginfo("- Process locked")
return
# lock process
self.process_locked = True
rp.loginfo("- Process started")
# lock data receivers
self.receiver_locked = True
image = copy.deepcopy(self.image_data)
points = copy.deepcopy(self.points_data)
# unlock data receivers
self.image_received = False
self.points_received = False
self.receiver_locked = False
# process image
try:
bayer_image = self.bridge.imgmsg_to_cv2(image, "bayer_grbg8")
rgb_img = cv2.cvtColor(bayer_image, cv2.COLOR_BAYER_GR2RGB)
except CvBridgeError as e:
print(e)
rp.loginfo(rp.get_caller_id() + "-- Image converted, shape: %s",
str(rgb_img.shape))
# cv2.imwrite('/home/parkjaeil0108/challenge/Didi-Release-2/Data/1/test.jpg', rgb_img)
# process points
x_pos = []
y_pos = []
z_pos = []
for p in pc2.read_points(points, skip_nans=True):
x_pos.append(p[0])
y_pos.append(p[1])
z_pos.append(p[2])
rp.loginfo("-- %d Point converted, p0: %.2f %.2f %.2f", len(x_pos),
x_pos[0], y_pos[0], z_pos[0])
# unlock process
rp.loginfo("- Process finished")
self.process_locked = False
def callback_kinect(self, data):
data_out = pc2.read_points(data,
field_names=("x", "y", "z", "intensity"),
skip_nans=False)
#print(data_out)
marker_array = MarkerArray()
id = 0
for p in data_out:
if (p[0] > -5 and p[0] < 5 and p[1] > 0.1):
marker = Marker()
marker.header.frame_id = "base_link"
marker.type = visualization_msgs.msg.Marker.CUBE
marker.id = id
marker.header.stamp = rospy.Time.now()
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.action = visualization_msgs.msg.Marker.ADD
marker.color.a = 1.0
red = self.red[int(30 * (p[2] + 2.3))][int(45 * (p[0] + 5))]
blue = self.blue[int(30 * (p[2] + 2.3))][int(45 * (p[0] + 5))]
green = self.green[int(30 * (p[2] + 2.3))][int(45 *
(p[0] + 5))]
#print(str(int(40*(p[0]+5)))+" "+str(int(70*(p[2]+2.3))))
#self.reconstruct[450-int(90*(p[2]+1.9))][int(100*(p[0]+5))]=255
marker.color.r = float(red) / 255
marker.color.g = float(green) / 255
marker.color.b = float(blue) / 255
marker.pose.position.x = p[0]
marker.pose.position.y = p[1]
marker.pose.position.z = p[2] + 1.9
if (id > 10000):
id = 0
marker_array = MarkerArray()
pass
marker_array.markers.append(marker)
id += 1
self.pub.publish(marker_array)
def transform_points_robot(points_msg):
start_time = time.time()
del points_transformed[:]
#print "delete level", len(points_transformed)
interval = start_time - init_time
#print "Time from start: ", interval
for p in point_cloud2.read_points(points_msg, skip_nans=True):
z_translated = round(p[2], 3)
if (z_translated < 0.00):
continue
x_translated = round(p[0], 3) + robot0_pose[0]
y_translated = round(p[1], 3) + robot0_pose[1]
rotation_angle = math.radians(robot0_pose[2])
x_transformed = x_translated * math.cos(
rotation_angle) + y_translated * math.sin(rotation_angle)
y_transformed = y_translated * math.cos(
rotation_angle) - x_translated * math.sin(rotation_angle)
pt = [x_transformed, y_transformed, z_translated]
points_transformed.append(pt)
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1)
]
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = "world"
pc2 = point_cloud2.create_cloud(header, fields, points_transformed)
pub.publish(pc2)
duration = time.time() - start_time
print len(points_transformed), interval, duration
def pointcloudToPlanningScene(msg):
global pointCloud
global completed
msg = pointCloud
if not completed:
try:
trans = tf_buffer.lookup_transform("map", msg.header.frame_id,
rospy.Time(0),
rospy.Duration(4))
except tf2.LookupException as ex:
rospy.logwarn(ex)
return
except tf2.ExtrapolationException as ex:
rospy.logwarn(ex)
return
cloud_out = do_transform_cloud(msg, trans)
rospy.sleep(2)
scene.remove_world_object()
rospy.sleep(2)
data = pc2.read_points(cloud_out,
field_names=("x", "y", "z", "rgb"),
skip_nans=True)
counter = 0
limitCounter = 0
limit = 5
for value in data:
if limitCounter == limit:
limitCounter = 0
p = PointStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = value[0]
p.pose.position.y = value[1]
p.pose.position.z = value[2]
scene.add_box("point" + str(counter), p, (0.01, 0.01, 0.01))
counter = counter + 1
completed = True
limitCounter = limitCounter + 1
print("completed scene")
def callback_intel_realsense(data):
global mag, dir, dist_arr
h_comp = 0.0
v_comp = 0.0
for i, point in zip(
range(11),
pc2.read_points(data,
field_names=("x", "y", "z"),
skip_nans=False,
uvs=[[1, 240], [64, 240], [128, 240], [192, 240],
[256, 240], [320, 240], [384,
240], [448, 240],
[512, 240], [576, 240], [639, 240]])):
pt_x = point[0]
pt_y = point[1]
pt_z = point[2]
if ((isnan(pt_x) or isnan(pt_y)) or (isnan(pt_z))):
dist_arr[i] = 5.0
else:
dist_arr[i] = sqrt(pt_x**2 + pt_y**2 + pt_z**2)
h_comp += dist_arr[i] * cos(18 * i * pi / 180)
v_comp += dist_arr[i] * sin(18 * i * pi / 180)
mag = sqrt(h_comp**2 + v_comp**2)
dir = atan2(v_comp, h_comp) * 180 / pi
if dir < 0:
dir += 360
dir = int(dir)
mag = np.round(mag)
mag /= 32.0 # proportionality constant
print("MAGNITUDE = ", mag, "DIRECTION = ", dir)
def __init__(self):
super(MarkerDetectionNode, self).__init__()
# configure
self.dynamic_reconfigure = Server(MarkerPoseEstimationConfig,
self.dynamic_reconfigure_callback)
# # generate marker
# image_dir = rospy.get_param("~image_dir")
# for i in range(50):
# marker_filename = os.path.join(image_dir, "marker_" + str(i) + ".png")
# self.generate_marker(marker_filename, i, 600)
# connect to the camera
self.bridge = CvBridge()
camera_name = rospy.get_param("~camera_name")
camera_type = rospy.get_param("~camera_type")
self.camera = CameraClient(camera_name, camera_type)
while not rospy.is_shutdown():
# get frame
cloud, texture = self.camera.get_frame(publish=True)
cv_image = self.bridge.imgmsg_to_cv2(texture, "bgr8")
# convert point cloud2 message to points
i = 0
points = np.zeros((cv_image.shape[1] * cv_image.shape[0], 3))
for p in pc2.read_points(cloud,
field_names=("x", "y", "z"),
skip_nans=False):
points[i, 0] = p[0]
points[i, 1] = p[1]
points[i, 2] = p[2]
i = i + 1
self.detect_marker(points, cv_image, target_marker_id=0)
rospy.spin()
cv2.destroyAllWindows()
def __extractPointCloudData(data): iterData = pc2.read_points(data) points = [] height = sqrt((data.width/float(16))*9) width = 16 * ( height/float(9) ) height = int(height) width = int(width) maxDist = 0 z = 2 print "PointCloud data received." for i in range(width): intermediate = [] for j in range(height): point = next(iterData) if(point[z]>maxDist): maxDist=point[z] intermediate.append(point) points.append(intermediate) points = np.array(points) points = np.swapaxes(points, 0, 1) print "New point cloud available" return (maxDist, points)
def callback(scan, image):
rospy.loginfo("image timestamp: %d ns" % image.header.stamp.to_nsec())
rospy.loginfo("scan timestamp: %d ns" % scan.header.stamp.to_nsec())
diff = abs(image.header.stamp.to_nsec() - scan.header.stamp.to_nsec())
rospy.loginfo("diff: %d ns" % diff)
img = bridge.imgmsg_to_cv2(image)
cloud = lp.projectLaser(scan)
points = pc2.read_points(cloud)
objPoints = np.array(map(extract, points))
Z = get_z(q, objPoints, K)
objPoints = objPoints[Z > 0]
if lens == 'pinhole':
img_points, _ = cv2.projectPoints(objPoints, rvec, tvec, K, D)
elif lens == 'fisheye':
objPoints = np.reshape(objPoints, (1,objPoints.shape[0],objPoints.shape[1]))
img_points, _ = cv2.fisheye.projectPoints(objPoints, rvec, tvec, K, D)
img_points = np.squeeze(img_points)
for i in range(len(img_points)):
try:
cv2.circle(img, (int(round(img_points[i][0])),int(round(img_points[i][1]))), laser_point_radius, (0,255,0), 1)
except OverflowError:
continue
pub.publish(bridge.cv2_to_imgmsg(img))
def getObjectPointCloud(req):
global cachedPCmsg, cachedPCmsgFlag
print "Returning pointclouds"
bin_num = req.bin_num # bin_num is of the format like 'bin_A'
#obj_id = req.obj_id #(string)
pc = PointCloud()
pts = []
with mutex:
if cachedPCmsgFlag:
source_pc2_kinect = cachedPCmsg # does it do deep copy?
else:
rospy.logerr("No pointcloud received yet, check your kinect")
# convert PointCloud2 to PointCloud manually
for point in point_cloud2.read_points(source_pc2_kinect, skip_nans=False):
pts.append(list(point)[0:3])
filtered_pc_map = filterPointCloud(bin_num, pts, source_pc2_kinect.header)
return GetObjectPointCloudResponse(filtered_pc_map)
def _cloud_cb(self, cloud):
points = np.array(list(read_points(cloud)))
if points.shape[0] == 0:
return
pos = points[:,0:3]
cor = np.reshape(points[:,-1], (points.shape[0], 1))
# Get 4x4 matrix which transforms point cloud co-ords to odometry frame
try:
points_to_map = self.tf.asMatrix('/lasths', cloud.header)
except tf.ExtrapolationException:
return
transformed_points = points_to_map.dot(np.vstack((pos.T, np.ones((1, pos.shape[0])))))
transformed_points = transformed_points[:3,:].T
self.seq += 1
header = Header()
header.seq = self.seq
header.stamp = rospy.Time.now()
header.frame_id = '/lasths'
self.cloud = np.vstack((self.cloud, np.hstack((transformed_points, cor))))
if self.seq % 30 == 0:
print "plup!"
self.cloud = np.zeros((0, 4))
output_cloud = create_cloud(header, cloud.fields, self.cloud)
self.cloud_pub.publish(output_cloud)
