def point_cloud_parser(self, msg):
"""Each publication, we do a lookup for where the drone currently is, in x,y,z, r,p,y. """
self.point_cloud_last_timestamp = msg.header
self.point_cloud = pc2.read_points_list(msg, skip_nans=True)
try:
trans = self.tfBuffer.lookup_transform('world', prefix,
rospy.Time(0))
q = (trans.transform.rotation.x, trans.transform.rotation.y,
trans.transform.rotation.z, trans.transform.rotation.w)
euler = euler_from_quaternion(q, axes='sxyz')
x = trans.transform.translation.x
y = trans.transform.translation.y
z = trans.transform.translation.z
roll = euler[0]
pitch = euler[1]
yaw = euler[2]
self.pos = [x, y, z, roll, pitch, yaw]
#rospy.loginfo("pos: {}\n\n\n".format(self.pos))
except:
rospy.logdebug("tf lookup -- {} not found".format(prefix))
def callback(data):
global laserscan, pplheader, ppl, max_x, lidartime, stoppublishing, rawlidardata, tf_listener
base_frame = 'world'
camera_frame = 'SimpleFlight/odom_local_ned'
print('tf frames', tf_listener.frameExists(base_frame),
tf_listener.frameExists(camera_frame))
return
#rospy.loginfo(rospy.get_caller_id() + " frameid %s width %d data in bytes %d", data.header.frame_id, data.width, len(data.data))
#important! check if data point is 1024. if not, the data order is not normal and mess up the conversion start angle, which result in a rotation offset in scan.
#this is resolved now. incomplete data size are segmented according to their starting angles
if not data.width == 1024:
#print(' return on data len ', data.width)
#return
pass
curr_time = rospy.get_rostime()
if curr_time.secs == lidartime.secs and curr_time.nsecs - lidartime.nsecs < 20000000:
return
pass
lidartime = curr_time
#print('lidar msg time offset ', data.header.stamp.secs-curr_time.secs, data.header.stamp.nsecs-curr_time.nsecs)
# proceed to process pc2, save the current pc2 in global variable for async process. otherwise the conversion time too much and cause strange buffer delay.
points_list = []
#for point in pc2.read_points(data, skip_nans=True):
# points_list.append([point[0], point[1], point[2]])
#print(len(points_list), points_list[0])
rawlidardata = data
ppl = pc2.read_points_list(data, skip_nans=True)
pplheader = data.header
max_x = ppl_max_x(ppl[0:64])
def _on_point_cloud(self, msg): # type: (PointCloud2) -> None
point_list = point_cloud2.read_points_list(msg, field_names=('x', 'y', 'z'), skip_nans=True)
n_point = len(point_list)
# The point cloud in camera frame
np_cloud_camera = np.zeros(shape=(n_point, 3))
for i in range(n_point):
point_i = point_list[i]
np_cloud_camera[i, 0] = point_i.x
np_cloud_camera[i, 1] = point_i.y
np_cloud_camera[i, 2] = point_i.z
# Transform into world frame
camera2world = self.get_camera2world()
np_cloud_world = camera2world[0:3, 0:3].dot(np_cloud_camera.T)
np_cloud_world = np_cloud_world.T
for i in range(3):
np_cloud_world[:, i] += camera2world[i, 3]
# Set the color
color = self._meshcat_vis._make_meshcat_color(n_point, 254, 254, 254)
# Update the frame
self._vis_lock.acquire()
self._meshcat_vis.add_pointcloud(np_cloud_world, color)
self._vis_lock.release()
def PublishPC2callback(self, data):
#data is the pc
pc = point_cloud2.read_points_list(data, skip_nans=True)
#fetch xyz
xyz = np.array(pc)[:, 0:3]
#check if it is inside hull
isInHull = in_hull(xyz, self.roipoly)
#pick indexes
xyz = xyz[np.where(isInHull == True)]
#setup pointfield
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
]
#print points
header = data.header
pc2 = point_cloud2.create_cloud(header, fields, xyz)
#pctemp = data
#while not rospy.is_shutdown():
pc2.header.stamp = rospy.Time.now()
self.pub.publish(pc2)
print("Published")
def convert_bag_to_ply(bag, topic, out_dir):
field_names = ["x", "y", "z", "intensity"]
max_intensity = 1000 # HACK: expose parameter
bag_msgs = bag.read_messages(topics=[topic])
msg_count = bag.get_message_count(topic)
for idx, (_, msg, _) in tqdm(enumerate(bag_msgs), total=msg_count):
points_xyz = []
points_i = []
points = pc2.read_points_list(cloud=msg,
field_names=field_names,
skip_nans=True)
for point_xyzi in points:
x, y, z, i = point_xyzi
points_xyz.append([x, y, z])
points_i.append([i, i, i])
points_xyz = np.asarray(points_xyz)
points_i = (np.asarray(points_i) / max_intensity).clip(0.0, 1.0)
assert points_xyz.shape == points_i.shape, "Dimension Missmatch"
filename = out_dir + str(idx).zfill(6) + ".ply"
o3d_cloud = o3d.geometry.PointCloud()
o3d_cloud.points = o3d.utility.Vector3dVector(points_xyz)
o3d_cloud.colors = o3d.utility.Vector3dVector(points_i)
o3d.io.write_point_cloud(filename, o3d_cloud)
def scan_cb(msg):
# convert the message of type LaserScan to a PointCloud2
pc2_msg = lp.projectLaser(msg)
pc2_msg.header.frame_id = "/camera"
# 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.0
num = 0
for point in point_generator:
if not math.isnan(point[2]):
sum += point[2]
num += 1
# we can calculate the average z value for example
print(str(sum / num))
# or a list of the individual points which is less efficient
point_list = pc2.read_points_list(pc2_msg)
# we can access the point list with an index, each element is a namedtuple
# we can access the elements by name, the generator does not yield namedtuples!
# if we convert it to a list and back this possibility is lost
print(point_list[len(point_list) / 2].x)
def callback(self, msg):
assert isinstance(msg, PointCloud2)
print(msg.header.seq)
# gen=point_cloud2.read_points(msg,field_names=("x","y","z"))
points = point_cloud2.read_points_list(msg,
field_names=("x", "y", "z"))
# print(points)
self.rate.sleep()
def main(argv):
#start node
rospy.init_node('echoes_act_III', anonymous=True)
#read pointcloud boundaries
pchull = rospy.wait_for_message("/boxcast", PointCloud2)
pc = point_cloud2.read_points_list(pchull, skip_nans=False)
x = []
y = []
z = []
camname = argv[0]
for point in pc:
x.append(point[0])
y.append(point[1])
z.append(point[2])
roi = np.vstack([x, y, z])
tfer = tf.TransformListener()
print(tfer.frameExists(camname))
print(tfer.frameExists(pchull.header.frame_id))
tfer.waitForTransform(camname, pchull.header.frame_id, rospy.Time(0),
rospy.Duration(40.0))
#gets transformation
trans, rot = tfer.lookupTransform(camname, pchull.header.frame_id,
rospy.Time(0))
print(trans, rot)
H = tf.transformations.quaternion_matrix(rot)
#H = mmnip.Rt2Homo(H[0:3,0:3],trans)
R = H[0:3, 0:3]
t = np.array(trans)
#transform
roi = mmnip.Transform(roi, R, t)
#initialize callback receiver
pc2cropper = Pc2Crop(roi.T, camname)
#pctopic="/camera/depth_registered/points"
pctopic = "/depth/points"
rospy.Subscriber(camname + pctopic, PointCloud2,
pc2cropper.PublishPC2callback)
try:
rospy.spin()
except KeyboardInterrupt:
print("shut")
def parseData(self, data):
if not self.field_indices:
self.field_indices = self.findFieldIndices(data.fields)
points = np.array(point_cloud2.read_points_list(data, skip_nans=True))
points = points[:, self.field_indices]
if self.relative_timestamp:
points[:, 3] += data.header.stamp.to_sec()
return points
def FilterC(self):
pontos = pc2.read_points_list(self.old_cloud,
field_names=("x", "y", "z"),
skip_nans=True)
new_pontos = list()
d_max = 10
d_trig = 1.5
for k in range(270):
encontrou = False
for p in pontos:
if ((180 * atan2(p[1], p[0]) / pi) - (k - 135))**2 < 0.01:
Point = namedtuple("Point", ("x", "y", "z"))
l = (p[0], p[1], 0)
new_p = Point._make(l)
new_pontos.append(new_p)
encontrou = True
break
if not encontrou:
Point = namedtuple("Point", ("x", "y", "z"))
l = (d_max * cos(pi * (k - 135) / 180),
d_max * sin(pi * (k - 135) / 180), 0)
new_p = Point._make(l)
new_pontos.append(new_p)
disc = list()
# p = new_pontos[0]
# if sqrt(p[0]**2 + p[1]**2) < d_max:
# disc.append(p)
d_max = 9
for k1 in range(1, 269):
k2 = k1 + 1
p1 = new_pontos[k1]
p2 = new_pontos[k2]
d_p1 = sqrt(p1[0]**2 + p1[1]**2)
d_p2 = sqrt(p2[0]**2 + p2[1]**2)
if d_p1 == d_max and d_p2 < d_max:
disc.append(p2)
elif abs(d_p1 - d_p2) >= d_trig:
if d_p1 < d_p2:
disc.append(p1)
else:
disc.append(p2)
# p = new_pontos[269]
# if sqrt(p[0]**2 + p[1]**2) < d_max:
# disc.append(p)
print(len(disc))
return (pc2.create_cloud_xyz32(self.old_cloud.header, new_pontos),
pc2.create_cloud_xyz32(self.old_cloud.header, disc))
def callback(data):
global laserscan, pplheader, ppl, max_x, lidarnexttime, stoppublishing
rospy.loginfo(
rospy.get_caller_id() + " frameid %s width %d data in bytes %d",
data.header.frame_id, data.width, len(data.data))
points_list = []
ppl = pc2.read_points_list(data, skip_nans=True)
pplheader = data.header
max_x = ppl_max_x(ppl[0:64])
print('pc2 ', len(ppl), ppl[0])
return
def updateObstacles(self, cloud):
# Transform to odom frame:
try:
trans = self.tfb.lookup_transform(self.odom_frame, cloud.header.frame_id, rospy.Time(0))
except:
rospy.logwarn("Error looking up transform!")
return
cloud = tf2sm.do_transform_cloud(cloud, trans)
# Extract obstacle locations:
self.obstacles = pc2.read_points_list(cloud, field_names=["x", "y"], skip_nans=True)
def scanConverter(msg):
pc2_msg = lp.projectLaser(msg)
pc_pub.publish(pc2_msg)
point_generator = pc2.read_points(pc2_msg)
sum = 1.0
num = 0
for Point in point_generator:
if not math.isnan(Point[2]):
sum += Point[2]
num += 1
print(str(sum/num))
piont_list = pc2.read_points_list(pc2_msg)
print(piont_list[len(piont_list)/2].x)
def pointcloud2_callback(msg):
global processing_service
rospy.loginfo("pointcloud2_callback called")
try:
rospy.wait_for_service('/mesh_from_pointclouds', timeout=3)
mesh_from_pointcloud = rospy.ServiceProxy('/mesh_from_pointclouds',
MeshFromPointCloud2)
points = pc2.read_points_list(msg,
field_names=("x", "y", "z"),
skip_nans=True)
cloud = pcl.PointCloud(np.array(points, dtype=np.float32))
clip_distance = 20
passthrough = cloud.make_passthrough_filter()
passthrough.set_filter_field_name('x')
passthrough.set_filter_limits(-clip_distance, clip_distance)
cloud_filtered = passthrough.filter()
passthrough = cloud_filtered.make_passthrough_filter()
passthrough.set_filter_field_name('y')
passthrough.set_filter_limits(-clip_distance, clip_distance)
cloud_filtered = passthrough.filter()
passthrough = cloud_filtered.make_passthrough_filter()
passthrough.set_filter_field_name('z')
passthrough.set_filter_limits(-clip_distance, clip_distance)
cloud_filtered = passthrough.filter()
vg = cloud_filtered.make_voxel_grid_filter()
vg.set_leaf_size(0.01, 0.01, 0.01)
cloud_filtered = vg.filter()
filtered_msg = pcl_to_ros(cloud_filtered, frame_id=msg.header.frame_id)
time1 = time.time()
mesh_src_point = Point(0.0, 0.0, 0.0)
resp1 = mesh_from_pointcloud(filtered_msg, mesh_src_point)
time2 = time.time()
rospy.loginfo("pointcloud processed result: %s", resp1)
rospy.loginfo("service executed in %f seconds", (time2 - time1))
except rospy.ServiceException as e:
rospy.logerr("Service call failed: %s", e)
processing_service = False
except Exception as e:
rospy.logerr("Exception: %s", e)
processing_service = False
traceback.print_exc()
def FilterC(self):
pontos = pc2.read_points_list(self.old_cloud,
field_names=("x", "y", "z"),
skip_nans=True)
new_pontos = list()
for p in pontos:
# print('p = (', p[0], p[1], p[2],')')
if p[2] >= -0.1 and p[2] <= 0.1 and sqrt(p[0]**2 + p[1]**2 +
p[2]**2) > 0.5:
rp = (p[0], p[1])
wp = self.world_frame(rp)
if not self.inTC(wp):
new_pontos.append(p)
return pc2.create_cloud(self.old_cloud.header, self.old_cloud.fields,
new_pontos)
def detect_cart_legs(self):
# convert the message of type LaserScan to a PointCloud2
pc2_msg = self.laser_projection.projectLaser(
self.cart_isolated_laser_msg)
point_list = pc2.read_points_list(pc2_msg)
dataset = []
legs = [[np.nan, np.nan], [np.nan, np.nan], [np.nan, np.nan],
[np.nan, np.nan]]
for i in point_list:
dataset.append([i.x, i.y])
# K-mean clustering the dataset to get center of each leg:
if len(dataset) >= 4:
kmeans = KMeans(n_clusters=4, random_state=0).fit(dataset)
legs = kmeans.cluster_centers_
print(legs)
def check_leg_position(ref, source):
check = 0
polygon = shape_polygon(ref)
for i in source:
point = shape_point(i[0], i[1])
if polygon.contains(point):
check += 1
if check >= len(ref):
return True
else:
return False
# Manually check position of the cart:
if check_leg_position(self.ref_cart_legs, legs) and check_leg_position(
self.ref_cart_legs, dataset):
self.the_cart = True
self.cart_footprint_estimator.header.stamp = rospy.Time.now()
leg1 = Point(legs[0][0], legs[0][1], 0.0)
leg2 = Point(legs[3][0], legs[3][1], 0.0)
leg3 = Point(legs[1][0], legs[1][1], 0.0)
leg4 = Point(legs[2][0], legs[2][1], 0.0)
self.cart_footprint_estimator.polygon.points = [
leg1, leg2, leg3, leg4
]
self.visual_cart_footprint_estimator.publish(
self.cart_footprint_estimator)
# self.close_grippers()
else:
self.the_cart = False
rospy.loginfo("Cart detected: " + str(self.the_cart))
def FilterC(self):
pontos = pc2.read_points_list(self.old_cloud,
field_names=("x", "y", "z"),
skip_nans=True)
new_pontos = list()
for p in pontos:
rp = (p[0], p[1])
wp = self.world_frame(rp)
Point = namedtuple("Point", ("x", "y", "z"))
l = (wp[0], wp[1], p[2])
new_p = Point._make(l)
new_pontos.append(new_p)
return pc2.create_cloud_xyz32(self.old_cloud.header, new_pontos)
def clip_pointcloud_msg(msg, src, clip_distance=8):
"""
Clip pointcloud from source position given an offset
This reduces the size of the pointcloud to a reazonable size to
perform the reconstruction
:param msg:
:param x:
:param y:
:param z:
:param offset:
:return:
"""
points = pc2.read_points_list(msg,
field_names=("x", "y", "z"),
skip_nans=True)
cloud = pcl.PointCloud(np.array(points, dtype=np.float32))
passthrough = cloud.make_passthrough_filter()
passthrough.set_filter_field_name('x')
passthrough.set_filter_limits(
-clip_distance + src.pose.pose.position.x,
clip_distance + src.pose.pose.position.x)
cloud_filtered = passthrough.filter()
passthrough = cloud_filtered.make_passthrough_filter()
passthrough.set_filter_field_name('y')
passthrough.set_filter_limits(
-clip_distance + src.pose.pose.position.y,
clip_distance + src.pose.pose.position.y)
cloud_filtered = passthrough.filter()
passthrough = cloud_filtered.make_passthrough_filter()
passthrough.set_filter_field_name('z')
passthrough.set_filter_limits(
-clip_distance + src.pose.pose.position.z,
clip_distance + src.pose.pose.position.z)
cloud_filtered = passthrough.filter()
vg = cloud_filtered.make_voxel_grid_filter()
vg.set_leaf_size(0.1, 0.1, 0.1)
cloud_filtered = vg.filter()
filtered_msg = RepublishPointCloud.pcl_to_ros(
cloud_filtered, frame_id=msg.header.frame_id)
return filtered_msg
def callback_disc(self, data):
pontos = pc2.read_points_list(data,
field_names=("x", "y", "z"),
skip_nans=True)
self.disc_pontos = list()
self.rp_disc_pontos = list()
for p in pontos:
rp = (p[0], p[1])
wp = self.world_frame(rp)
if True: #not self.inTC(wp):
if self.pos_y >= 0:
if self.pos_x > wp[0] - 0.1:
self.disc_pontos.append(wp)
self.rp_disc_pontos.append(rp)
else:
if self.pos_x < wp[0] + 0.1:
self.disc_pontos.append(wp)
self.rp_disc_pontos.append(rp)
def scan(self, cloud):
xyz_generator = pc2.read_points_list(cloud,
skip_nans=True,
field_names=("x", "y", "z",
"intensity"))
x = []
y = []
z = []
intensity = []
for xyz in xyz_generator:
x.append(xyz.x)
y.append(xyz.y)
z.append(xyz.z)
intensity.append(xyz.intensity)
self.x = x
self.y = y
self.z = z
self.intensity = intensity
def callback(data):
global laserscan, pplheader, ppl, p_angles, p_segs, p_segindex, max_x, lidarnexttime, stoppublishing, pc2pub, pc2data
# rospy.loginfo(rospy.get_caller_id() + " frameid %s width %d data in bytes %d", data.header.frame_id, data.width, len(data.data))
points_list = []
ppl = pc2.read_points_list(data, skip_nans=True)
p_angles = ppl_angles(ppl)
p_segs, p_segindex = ppl_16seg(p_angles, data.data)
pplheader = data.header
#max_x =ppl_max_x(ppl[0:64])
#print('pc2 ', len(ppl), ppl[0])
segid = rospy.get_param("/segid")
data.width = len(p_segs[segid]) / 12
data.row_step = data.width * 12
data.data = p_segs[segid]
pc2data = data
#data.data=data.data[0:data.row_step]
return
def scan_cb(msg):
global point_list
# избавляемся от inf
without_inf = list()
for k in range(0, len(msg.ranges)):
if np.isinf(msg.ranges[k]) or msg.ranges[k] > max_dist_lidar:
without_inf.append(max_dist_lidar)
else:
without_inf.append(msg.ranges[k])
# инвертируем сонар без inf
msg.ranges = without_inf
# получаем курс в градусах
course = int(np.rad2deg(current_course))
# поворачиваем массив расстояний с лидара(360 знач) на курс в градусах
msg.ranges = msg.ranges[-course:] + msg.ranges[:-course]
# convert the message of type LaserScan to a PointCloud2
pc2_msg = lp.projectLaser(msg)
# convert it to a pointlist
point_list = pc2.read_points_list(pc2_msg)
def scan_cb(msg):
# convert the message of type LaserScan to a PointCloud2
points_list = []
for point in msg.points:
points_list.append([point.x, point.y, point.z])
array = np.asarray(points_list)
ev = [
np.where([
np.logical_and((x_min <= point[0] <= x_max),
(y_min <= point[1] <= y_max)) for point in array
])
]
print(ev)
# or a list of the individual points which is less efficient
point_list = pc2.read_points_list(pc2_msg)
# we can access the point list with an index, each element is a namedtuple
# we can access the elements by name, the generator does not yield namedtuples!
# if we convert it to a list and back this possibility is lost
print(point_list[len(point_list) / 2].x)
range_list = []
x_list = []
y_list = []
z_list = []
remission_list = []
range_std_list = []
x_std_list = []
y_std_list = []
z_std_list = []
remission_std_list = []
for topic, msg, t in bag.read_messages(topics=topics):
if (topic == str('/lslidar_point_cloud')):
point_list = pcl2.read_points_list(msg, skip_nans=True)
point_array = np.array(point_list, dtype=np.float32)
range_temp = []
x_temp = []
y_temp = []
z_temp = []
remission_temp = []
for line in point_array:
range_temp.append(
math.sqrt(line[0] * line[0] + line[1] * line[1] +
line[2] * line[2]))
x_temp.append(line[0])
y_temp.append(line[1])
z_temp.append(line[2])
def process_data(self):
if not self.current_map_pointcloud_msg:
rospy.logerr("No current_map_pointcloud_msg received")
return False
if not self.cmd_vel_msg:
rospy.logerr("No cmd_vel_msg received")
return False
if not self.last_odom_msg:
self.last_odom_msg = self.odom_msg
return False
# else:
# last_point = (self.last_odom_msg.pose.pose.position.x, self.last_odom_msg.pose.pose.position.y)
# curr_point = (self.odom_msg.pose.pose.position.x, self.odom_msg.pose.pose.position.y)
# odom_diff_xy = abs(last_point[0] - curr_point[0]) + abs(last_point[1] - curr_point[1])
#
# _, _, last_y = tf.transformations.euler_from_quaternion([self.last_odom_msg.pose.pose.orientation.x,
# self.last_odom_msg.pose.pose.orientation.y,
# self.last_odom_msg.pose.pose.orientation.z,
# self.last_odom_msg.pose.pose.orientation.w])
#
# _, _, curr_y = tf.transformations.euler_from_quaternion([self.odom_msg.pose.pose.orientation.x,
# self.odom_msg.pose.pose.orientation.y,
# self.odom_msg.pose.pose.orientation.z,
# self.odom_msg.pose.pose.orientation.w])
#
# odom_diff_yaw = abs(last_y - curr_y)
# if odom_diff_xy < 0.1 and odom_diff_yaw < 0.6:
# rospy.logwarn("No movement detected on XY (%s) and yaw (%s)", odom_diff_xy, odom_diff_yaw)
# rospy.logwarn("last_y (%s) curr_y (%s)", last_y, curr_y)
# return
# self.last_odom_msg = self.odom_msg
if abs(self.cmd_vel_msg.linear.x) < 0.05 and abs(self.cmd_vel_msg.angular.z) < 0.05:
rospy.logwarn("No movement (time:%.2f) x:%.2f z:%.2f odom_dist:%.2f rmse:%.2f",
self.current_timestep,
self.cmd_vel_msg.linear.x,
self.cmd_vel_msg.angular.z,
self.odom_dist,
self.rmse)
return False
last_point = (self.last_odom_msg.pose.pose.position.x,
self.last_odom_msg.pose.pose.position.y,
self.last_odom_msg.pose.pose.position.z)
curr_point = (self.odom_msg.pose.pose.position.x,
self.odom_msg.pose.pose.position.y,
self.odom_msg.pose.pose.position.z)
local_odom_dist = math.sqrt((last_point[0] - curr_point[0]) ** 2 +
(last_point[1] - curr_point[1]) ** 2 +
(last_point[2] - curr_point[2]) ** 2)
self.odom_dist += local_odom_dist
self.last_odom_msg = self.odom_msg
points = pc2.read_points_list(self.current_map_pointcloud_msg, field_names=("x", "y", "z"), skip_nans=True)
cloud = pcl.PointCloud(np.array(points, dtype=np.float32))
cloud = ExplorationMetrics.voxelize_cloud(cloud, self.leaf_size)
self.rmse = ExplorationMetrics.similarity(self.ground_truth_pointcloud, cloud)
self.pointcloud_plot_data.append({'timestep': self.current_timestep,
'rmse': self.rmse,
'odom_dist': local_odom_dist,
'robot_action_number': self.robot_action_number})
rospy.loginfo("timestep:%s action_num:%s odom_dist:%.2f rmse:%.2f", self.current_timestep,
self.robot_action_number, self.odom_dist, self.rmse)
self.current_timestep += 1
self.last_time_updated = time.time()
return True
def process_lidar_msg():
global lidar_msg
if not lidar_msg:
return
mlab.figure(1, bgcolor=(0, 0, 0))
mlab.clf()
points = pc2.read_points_list(lidar_msg,
field_names=("x", "y", "z"),
skip_nans=True)
print "size points:", len(points)
points = [
p for p in points if p[2] < -0.39
or p[2] > -0.35 and math.sqrt(p[0]**2 + p[1]**2 + p[2]**2) < 3.0
]
print "size points after Z basic cleanout:", len(points)
X = np.array(points)
pts2 = mlab.points3d(X[:, 0],
X[:, 1],
X[:, 2],
color=(1.0, 1.0, 1.0),
scale_factor=0.01,
scale_mode='none',
resolution=20)
border_kdtree = spatial.KDTree(points)
border_tresh = 0.4
db = DBSCAN(eps=0.20, min_samples=2).fit(points)
core_samples_mask = np.zeros_like(db.labels_, dtype=bool)
core_samples_mask[db.core_sample_indices_] = True
labels = db.labels_
# Number of clusters in labels, ignoring noise if present.
n_clusters_ = len(set(labels)) - (1 if -1 in labels else 0)
n_noise_ = list(labels).count(-1)
rospy.loginfo('Estimated number of clusters: %d, noise_points: %s',
n_clusters_, n_noise_)
unique_labels = set(labels)
colors = plt.cm.get_cmap('gist_rainbow', len(unique_labels))
X = np.array(points)
for idx, label in enumerate(unique_labels):
if label == -1:
# Black used for noise.
# col = [0, 0, 0, 1]
continue
else:
col = colors(idx)
# print "col", col
class_member_mask = (labels == label)
xyz = X[class_member_mask & core_samples_mask]
# print "xyz:", xyz
z_std = np.std(xyz[:, 2])
print "np.std(xyz[:, 2]):", z_std
#normaldefinition_3D_real(xyz, 16)
# mesh_filepath = None
# try:
# fields = [
# sensor_msgs.msg.PointField('x', 0, sensor_msgs.msg.PointField.FLOAT32, 1),
# sensor_msgs.msg.PointField('y', 4, sensor_msgs.msg.PointField.FLOAT32, 1),
# sensor_msgs.msg.PointField('z', 8, sensor_msgs.msg.PointField.FLOAT32, 1)
# ]
#
# pcloud = pc2.create_cloud(lidar_msg.header, fields, xyz.tolist())
# rospy.wait_for_service('/mesh_from_pointclouds', timeout=3)
# mesh_from_pointcloud = rospy.ServiceProxy('/mesh_from_pointclouds', MeshFromPointCloud2)
# resp1 = mesh_from_pointcloud(pcloud)
# mesh_filepath = resp1.path
# rospy.loginfo("pointcloud processed result: %s", resp1)
# except rospy.ServiceException as e:
# rospy.logerr("Service call failed: %s", e)
# except Exception as e:
# rospy.logerr("Exception: %s", e)
#
# if mesh_filepath is None:
# rospy.logerr("mesh_filepath is None, cannot continue with the planning")
# rate_slow.sleep()
# continue
# fxyz = np.array([centroids[v] for v in intersecting_frontiers])
pts2 = mlab.points3d(xyz[:, 0],
xyz[:, 1],
xyz[:, 2],
color=(col[0], col[1], col[2]),
scale_factor=0.05,
scale_mode='none',
resolution=20)
# if this cluster is not having multiple Z levels discard it
if z_std < 0.1:
continue
centroid = get_centroid_of_pts(xyz)
# print "centroid:", centroid
pts3 = mlab.points3d(centroid[:, 0],
centroid[:, 1],
centroid[:, 2],
color=(col[0], col[1], col[2]),
scale_factor=0.5,
scale_mode='none',
resolution=20)
mlab.show()
def labelData(self):
# Detected and idxs values to False and [], to make sure we are not using information from a previous labelling
self.labels['detected'] = False
self.labels['idxs'] = []
# Labelling process dependent of the sensor type
if self.msg_type_str == 'LaserScan': # 2D LIDARS -------------------------------------
# For 2D LIDARS the process is the following: First cluster all the range data into clusters. Then,
# associate one of the clusters with the calibration pattern by selecting the cluster which is closest to
# the rviz interactive marker.
clusters = [] # initialize cluster list to empty
cluster_counter = 0 # init counter
points = [] # init points
# Compute cartesian coordinates
xs, ys = interactive_calibration.utilities.laser_scan_msg_to_xy(self.msg)
# Clustering:
first_iteration = True
for idx, r in enumerate(self.msg.ranges):
# Skip if either this point or the previous have range smaller than minimum_range_value
if r < self.minimum_range_value or self.msg.ranges[idx - 1] < self.minimum_range_value:
continue
if first_iteration: # if first iteration, create a new cluster
clusters.append(LaserScanCluster(cluster_counter, idx))
first_iteration = False
else: # check if new point belongs to current cluster, create new cluster if not
x = xs[clusters[-1].idxs[-1]] # x coordinate of last point of last cluster
y = ys[clusters[-1].idxs[-1]] # y coordinate of last point of last cluster
distance = math.sqrt((xs[idx] - x) ** 2 + (ys[idx] - y) ** 2)
if distance > self.threshold: # if distance larger than threshold, create new cluster
cluster_counter += 1
clusters.append(LaserScanCluster(cluster_counter, idx))
else: # same cluster, push this point into the same cluster
clusters[-1].pushIdx(idx)
# Association stage: find out which cluster is closer to the marker
x_marker, y_marker = self.marker.pose.position.x, self.marker.pose.position.y # interactive marker pose
idx_closest_cluster = 0
min_dist = sys.maxint
for cluster_idx, cluster in enumerate(clusters): # cycle all clusters
for idx in cluster.idxs: # cycle each point in the cluster
x, y = xs[idx], ys[idx]
dist = math.sqrt((x_marker - x) ** 2 + (y_marker - y) ** 2)
if dist < min_dist:
idx_closest_cluster = cluster_idx
min_dist = dist
closest_cluster = clusters[idx_closest_cluster]
# Find the coordinate of the middle point in the closest cluster and bring the marker to that point
x_sum, y_sum = 0, 0
for idx in closest_cluster.idxs:
x_sum += xs[idx]
y_sum += ys[idx]
self.marker.pose.position.x = x_sum / float(len(closest_cluster.idxs))
self.marker.pose.position.y = y_sum / float(len(closest_cluster.idxs))
self.marker.pose.position.z = 0
self.menu_handler.reApply(self.server)
self.server.applyChanges()
# Update the dictionary with the labels
self.labels['detected'] = True
percentage_points_to_remove = 0.0 # remove x% of data from each side
number_of_idxs = len(clusters[idx_closest_cluster].idxs)
idxs_to_remove = int(percentage_points_to_remove * float(number_of_idxs))
clusters[idx_closest_cluster].idxs_filtered = clusters[idx_closest_cluster].idxs[
idxs_to_remove:number_of_idxs - idxs_to_remove]
self.labels['idxs'] = clusters[idx_closest_cluster].idxs_filtered
# Create and publish point cloud message with the colored clusters (just for debugging)
cmap = cm.prism(np.linspace(0, 1, len(clusters)))
points = []
z, a = 0, 255
for cluster in clusters:
for idx in cluster.idxs:
x, y = xs[idx], ys[idx]
r, g, b = int(cmap[cluster.cluster_count, 0] * 255.0), \
int(cmap[cluster.cluster_count, 1] * 255.0), \
int(cmap[cluster.cluster_count, 2] * 255.0)
rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0]
pt = [x, y, z, rgb]
points.append(pt)
fields = [PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1), PointField('rgba', 12, PointField.UINT32, 1)]
header = Header()
header.frame_id = self.parent
header.stamp = self.msg.header.stamp
pc_msg = point_cloud2.create_cloud(header, fields, points)
self.publisher_clusters.publish(pc_msg)
# Create and publish point cloud message containing only the selected calibration pattern points
points = []
for idx in clusters[idx_closest_cluster].idxs_filtered:
x_marker, y_marker, z_marker = xs[idx], ys[idx], 0
r = int(0 * 255.0)
g = int(0 * 255.0)
b = int(1 * 255.0)
a = 255
rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0]
pt = [x_marker, y_marker, z_marker, rgb]
points.append(pt)
pc_msg = point_cloud2.create_cloud(header, fields, points)
self.publisher_selected_points.publish(pc_msg)
elif self.msg_type_str == 'Image': # Cameras -------------------------------------------
# Convert to opencv image and save image to disk
image = self.bridge.imgmsg_to_cv2(self.msg, "bgr8")
result = self.pattern.detect(image)
if result['detected']:
c = []
for corner in result['keypoints']:
c.append({'x': float(corner[0][0]), 'y': float(corner[0][1])})
x = int(round(c[0]['x']))
y = int(round(c[0]['y']))
cv2.line(image, (x, y), (x, y), (0, 255, 255), 20)
# Update the dictionary with the labels
self.labels['detected'] = True
self.labels['idxs'] = c
# For visual debugging
self.pattern.drawKeypoints(image, result)
msg_out = self.bridge.cv2_to_imgmsg(image, encoding="passthrough")
msg_out.header.stamp = self.msg.header.stamp
msg_out.header.frame_id = self.msg.header.frame_id
self.publisher_labelled_image.publish(msg_out)
elif self.msg_type_str == 'PointCloud2': # RGB-D pointcloud -------------------------------------------
# print("Found point cloud!")
# Get 3D coords
points = pc2.read_points_list(self.msg, skip_nans=False, field_names=("x", "y", "z"))
# Get the marker position
x_marker, y_marker, z_marker = self.marker.pose.position.x, self.marker.pose.position.y, self.marker.pose.position.z # interactive marker pose
cam_model = PinholeCameraModel()
# Wait for camera info message
camera_info = rospy.wait_for_message('/top_center_rgbd_camera/depth/camera_info', CameraInfo)
cam_model.fromCameraInfo(camera_info)
# Project points
seed_point = cam_model.project3dToPixel((x_marker, y_marker, z_marker))
seed_point = (int(math.floor(seed_point[0])), int(math.floor(seed_point[1])))
# Wait for depth image message
imgmsg = rospy.wait_for_message('/top_center_rgbd_camera/depth/image_rect', Image)
# img = self.bridge.imgmsg_to_cv2(imgmsg, desired_encoding="8UC1")
img = self.bridge.imgmsg_to_cv2(imgmsg, desired_encoding="passthrough")
img_float = img.astype(np.float32)
img_float = img_float / 1000
# print('img type = ' + str(img.dtype))
# print('img_float type = ' + str(img_float.dtype))
# print('img_float shape = ' + str(img_float.shape))
h, w = img.shape
mask = np.zeros((h + 2, w + 2, 1), np.uint8)
# mask[seed_point[1] - 2:seed_point[1] + 2, seed_point[0] - 2:seed_point[0] + 2] = 255
img_float2 = deepcopy(img_float)
cv2.floodFill(img_float2, mask, seed_point, 128, 0.1, 0.1,
8 | (128 << 8) | cv2.FLOODFILL_MASK_ONLY) # | cv2.FLOODFILL_FIXED_RANGE)
# Switch coords of seed point
# mask[seed_point[1]-2:seed_point[1]+2, seed_point[0]-2:seed_point[0]+2] = 255
tmpmask = mask[1:h + 1, 1:w + 1]
# calculate moments of binary image
M = cv2.moments(tmpmask)
if M["m00"] != 0:
# calculate x,y coordinate of center
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
mask[cY-2:cY+2, cX-2:cX+2] = 255
cv2.imshow("mask", mask)
cv2.waitKey(20)
# msg_out = self.bridge.cv2_to_imgmsg(showcenter, encoding="passthrough")
# msg_out.header.stamp = self.msg.header.stamp
# msg_out.header.frame_id = self.msg.header.frame_id
# self.publisher_labelled_depth_image.publish(msg_out)
coords = points[cY * 640 + cX]
if not math.isnan(coords[0]):
self.marker.pose.position.x = coords[0]
self.marker.pose.position.y = coords[1]
self.marker.pose.position.z = coords[2]
self.menu_handler.reApply(self.server)
self.server.applyChanges()
def octomap_cb(msg):
global point_list
# конвертируем PointCloud2 в List
# del point_list[:]
point_list = pc2.read_points_list(msg)
def scandata(msg):
global point_list
pointcloud_msg = lasergeometry.LaserProjection().projectLaser(msg)
point_list = point.read_points_list(pointcloud_msg)
def process_lidar_msg(n_bins=72, z_std_thresh=0.1):
global lidar_msg
if not lidar_msg:
return
rospy.loginfo("process_lidar_msg")
points = pc2.read_points_list(lidar_msg,
field_names=("x", "y", "z"),
skip_nans=True)
print "size points:", len(points)
#points = [p for p in points if p[2] < -0.39 or p[2] > -0.35 and math.sqrt(p[0] ** 2 + p[1] ** 2 + p[2] ** 2) < 3.0]
#print "size points after Z basic cleanout:", len(points)
#points = [p for p in points if math.sqrt(p[0] ** 2 + p[1] ** 2 + p[2] ** 2) < 3.0]
#print "size points after distance cleanout:", len(points)
cloud = pcl.PointCloud(np.array(points, dtype=np.float32))
clip_distance = 2.5
passthrough = cloud.make_passthrough_filter()
passthrough.set_filter_field_name('x')
passthrough.set_filter_limits(-clip_distance, clip_distance)
cloud_filtered = passthrough.filter()
passthrough = cloud_filtered.make_passthrough_filter()
passthrough.set_filter_field_name('y')
passthrough.set_filter_limits(-clip_distance, clip_distance)
cloud_filtered = passthrough.filter()
passthrough = cloud_filtered.make_passthrough_filter()
passthrough.set_filter_field_name('z')
passthrough.set_filter_limits(-clip_distance, clip_distance)
cloud_filtered = passthrough.filter()
vg = cloud_filtered.make_voxel_grid_filter()
vg.set_leaf_size(0.01, 0.01, 0.01)
cloud_filtered = vg.filter()
# divide the pointcloud in bins
bin_size = 360 / float(n_bins)
colors = get_color_list(n_bins)
np_p = cloud_filtered.to_array()
bin_idx = -1
#viewer.InitCameraParameters()
marker_array = MarkerArray()
closest_p_dist = float("inf")
closest_p = None
for i in xrange((n_bins / 2)):
for sign in [1, -1]:
bin_idx += 1
bin_start = (i * bin_size) * sign
bin_end = ((i + 1) * bin_size) * sign
if sign > 0:
fov = [bin_start, bin_end]
else:
fov = [bin_end, bin_start]
cond = hv_in_range(x=np_p[:, 0],
y=np_p[:, 1],
z=np_p[:, 2],
fov=fov,
fov_type='h')
np_p_ranged = np_p[cond]
z_std = np.std(np_p_ranged[:, 2])
if z_std < z_std_thresh:
cloud_cluster = pcl.PointCloud()
cloud_cluster.from_array(np_p_ranged)
pccolor = pcl.pcl_visualization.PointCloudColorHandleringCustom(
cloud_cluster, 255, 0, 0)
viewer.AddPointCloud_ColorHandler(
cloud_cluster, pccolor, b'cluster_{}'.format(bin_idx), 0)
print "\tz_std:", z_std
continue
color = colors[bin_idx]
cloud_bin = pcl.PointCloud(np_p_ranged)
tree = cloud_bin.make_kdtree()
ec = cloud_bin.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance(0.15)
ec.set_MinClusterSize(20)
ec.set_MaxClusterSize(25000)
ec.set_SearchMethod(tree)
cluster_indices = ec.Extract()
if len(cluster_indices) <= 0:
cloud_cluster = pcl.PointCloud()
cloud_cluster.from_array(np_p_ranged)
pccolor = pcl.pcl_visualization.PointCloudColorHandleringCustom(
cloud_cluster, 255, 0, 0)
viewer.AddPointCloud_ColorHandler(
cloud_cluster, pccolor, b'cluster_{}'.format(bin_idx), 0)
print "\tlen(cluster_indices)", len(cluster_indices)
continue
max_cluster_idx = find_max_list_idx(cluster_indices)
len_max_cluster = len(cluster_indices[max_cluster_idx])
#print 'cluster_indices :', len(cluster_indices), " count."
#print 'max_cluster_idx :', max_cluster_idx, " len:", len_max_cluster
if len(cluster_indices[max_cluster_idx]) < 100:
continue
cluster_points = np.zeros((len_max_cluster, 3), dtype=np.float32)
for j, indice in enumerate(cluster_indices[max_cluster_idx]):
cluster_points[j][0] = cloud_bin[indice][0]
cluster_points[j][1] = cloud_bin[indice][1]
cluster_points[j][2] = cloud_bin[indice][2]
cloud_cluster = pcl.PointCloud()
cloud_cluster.from_array(cluster_points)
#
#
pccolor = pcl.pcl_visualization.PointCloudColorHandleringCustom(
cloud_cluster, color[0], color[1], color[2])
viewer.AddPointCloud_ColorHandler(cloud_cluster, pccolor,
b'cluster_{}'.format(bin_idx), 0)
print "z_std:", z_std, "bin_start:", bin_start, "bin_end:", bin_end, "bin_idx:", bin_idx, "color:", color
centroid = get_centroid_of_pts(cluster_points)[0]
#print "centroid:", centroid
x, y, z = centroid
f_marker = create_marker((x, y, z),
color=(0.6, 0.1, 0.0),
duration=2,
m_scale=0.25,
marker_id=bin_idx)
marker_array.markers.append(f_marker)
d = math.sqrt(x**2 + y**2 + z**2)
if d < closest_p_dist:
closest_p_dist = d
closest_p = centroid
# pccolor = pcl.pcl_visualization.PointCloudColorHandleringCustom(cloud_bin, color[0], color[1], color[2])
# viewer.AddPointCloud_ColorHandler(cloud_bin, pccolor, b'cluster_{}'.format(bin_idx), 0)
viewer.AddCube(-0.25, 0.25, -0.15, 0.15, -0.4, -0.2, 255, 255, 255,
"robot")
# color = colors[0]
# pccolor = pcl.pcl_visualization.PointCloudColorHandleringCustom(cloud_filtered, color[0], color[1], color[2])
# viewer.AddPointCloud_ColorHandler(cloud_filtered, pccolor, b'cluster_{}'.format(0), 0)
# seg = cloud_filtered.make_segmenter()
# seg.set_optimize_coefficients(True)
# seg.set_model_type(pcl.SACMODEL_PLANE)
# seg.set_method_type(pcl.SAC_RANSAC)
# seg.set_MaxIterations(100)
# seg.set_distance_threshold(0.25)
#
# indices, model = seg.segment()
# tmp = cloud_filtered.to_array()
# tmp = np.delete(tmp, indices, 0)
# cloud_filtered.from_array(tmp)
# tree = cloud_filtered.make_kdtree()
# ec = cloud_filtered.make_EuclideanClusterExtraction()
# ec.set_ClusterTolerance(0.25)
# ec.set_MinClusterSize(2)
# ec.set_MaxClusterSize(25000)
# ec.set_SearchMethod(tree)
# cluster_indices = ec.Extract()
#
# print 'cluster_indices :', len(cluster_indices), " count."
#
# colors = get_color_list(len(cluster_indices))
#
# cloud_cluster = pcl.PointCloud()
#
# for j, indices in enumerate(cluster_indices):
# # cloudsize = indices
# print 'j:', j, 'indices:', str(len(indices))
#
# points = np.zeros((len(indices), 3), dtype=np.float32)
#
# for i, indice in enumerate(indices):
# points[i][0] = cloud_filtered[indice][0]
# points[i][1] = cloud_filtered[indice][1]
# points[i][2] = cloud_filtered[indice][2]
#
# z_std = np.std(points[:, 2])
# print "z std:", z_std
#
# cloud_cluster.from_array(points)
# ss = "/tmp/cluster/cloud_cluster_" + str(j) + ".pcd"
# pcl.save(cloud_cluster, ss)
#
# color = colors[j]
# pccolor = pcl.pcl_visualization.PointCloudColorHandleringCustom(cloud, color[0], color[1], color[2])
# viewer.AddPointCloud_ColorHandler(cloud_cluster, pccolor, b'cluster_{}'.format(j), 0)
if closest_p is not None:
closest_p_marker = create_marker(
(closest_p[0], closest_p[1], closest_p[2]),
color=(0.9, 0.1, 0.0),
duration=2,
m_scale=0.5,
marker_id=0)
pub_closest_obstacle_pt.publish(closest_p_marker)
pub_obstacles_pts.publish(marker_array)
v = True
while v:
v = not (viewer.WasStopped())
viewer.SpinOnce()
#time.sleep(0.5)
#break
# for j, indices in enumerate(cluster_indices):
# viewer.RemovePointCloud(b'cluster_{}'.format(j), 0)
# for i in xrange(n_bins):
# viewer.RemovePointCloud(b'cluster_{}'.format(i), 0)
viewer.remove_all_pointclouds()
viewer.remove_all_shapes()
