def sub_bb(msg):
rospy.loginfo("sub_bb cb")
bb = msg.boxes[0]
x1 = bb.pose.position.x - bb.dimensions.x
x2 = bb.pose.position.x + bb.dimensions.x
y1 = bb.pose.position.y - bb.dimensions.y
y2 = bb.pose.position.y + bb.dimensions.y
z1 = bb.pose.position.z - bb.dimensions.z
z2 = bb.pose.position.z + bb.dimensions.z
ps = PolygonStamped()
if face == "xy":
ps.polygon.points.append(Point32(x1, y1, z1))
ps.polygon.points.append(Point32(x1, y2, z1))
ps.polygon.points.append(Point32(x2, y2, z1))
ps.polygon.points.append(Point32(x2, y1, z1))
elif face == "yz":
ps.polygon.points.append(Point32(x1, y1, z1))
ps.polygon.points.append(Point32(x1, y1, z2))
ps.polygon.points.append(Point32(x1, y2, z2))
ps.polygon.points.append(Point32(x1, y2, z1))
elif face == "zx":
ps.polygon.points.append(Point32(x1, y1, z1))
ps.polygon.points.append(Point32(x1, y1, z2))
ps.polygon.points.append(Point32(x2, y1, z2))
ps.polygon.points.append(Point32(x2, y1, z1))
else:
rospy.loginfo("Face Error!")
ps.header = msg.header
pa = PolygonArray()
pa.header = msg.header
pa.polygons = [ps]
rospy.loginfo('pa:%s', pa)
pub.publish(pa)
def dynamic_custom_polygon_demo(self, zones_list):
#rospy.init_node("polygon_array_dynamic")
# /polygon_array_dynamic/output
pub = rospy.Publisher("/polygon_array_dynamic/output",
PolygonArray,
queue_size=1)
r = rospy.Rate(5)
count = 0
msg = PolygonArray()
header = Header()
header.frame_id = "world"
header.stamp = rospy.Time.now()
msg.header = header
msg.polygons = []
msg.labels = []
msg.likelihood = []
for i in zones_list:
#print(i)
msg.polygons.append(self.DynamicPolygon(header, i))
msg.labels.append(count)
msg.likelihood.append(np.random.ranf())
count += 1
rospy.loginfo("Publishing box")
pub.publish(msg)
r.sleep()
def generatePolygons(header, model_index):
polygon = PolygonArray()
polygon.header = header
polygon.polygons = [PolygonStamped()]
polygon.polygons[0].header = header
coef = ModelCoefficientsArray()
coef.coefficients = [ModelCoefficients()]
coef.header = header
if model_index in [0, 1, 2, 3]:
# Rectangle
polygon.polygons[0].polygon.points = [
Point32(x=1.0, y=1.0),
Point32(x=-1.0, y=1.0),
Point32(x=-1.0, y=-1.0),
Point32(x=1.0, y=-1.0)
]
coef.coefficients[0].header = header
coef.coefficients[0].values = [0, 0, 1, 0]
elif model_index == 4:
polygon.polygons[0].polygon.points = [
Point32(x=0.0, y=-0.5, z=0.0),
Point32(x=0.0, y=-0.5, z=2.0),
Point32(x=0.0, y=0.5, z=2.0),
Point32(x=0.0, y=0.5, z=0.0)
]
# polygon.polygons[0].polygon.points.reverse()
coef.coefficients[0].header = header
coef.coefficients[0].values = [-1, 0, 0, 0]
return (polygon, coef)
def sample_demo():
"""
Draws a star, circle, rectangle and square
"""
rospy.init_node("polygon_array_sample")
pub = rospy.Publisher("~output", PolygonArray, queue_size=1)
r = rospy.Rate(10)
while not rospy.is_shutdown():
msg = PolygonArray()
header = Header()
header.frame_id = "world"
header.stamp = rospy.Time.now()
msg.header = header
msg.polygons = [
SquarePolygon(header),
RectanglePolygon(header),
CirclePolygon(header),
StarPolygon(header)
]
# rospy.loginfo("number of point to enter :"+ str(msg.polygons))
msg.labels = [0, 1, 2, 3]
msg.likelihood = [
np.random.ranf(),
np.random.ranf(),
np.random.ranf(),
np.random.ranf()
]
pub.publish(msg)
r.sleep()
def timer_cb(msg1, msg2, msg3):
global polygons
# print(msg1.header)
# info_msg_polygon=msg2
info_msg_polygon = PolygonArray()
# info_msg_coefficient=msg3
info_msg_coefficient = ModelCoefficientsArray()
planes_indices = msg1.cluster_indices
# print('len(planes_indices)', len(planes_indices))
ind = 0
longest_ind = 0
longest_length = 0
if (len(planes_indices) >= 1):
for ind, plane in enumerate(planes_indices):
length = len(plane.indices)
if (length > longest_length):
longest_ind = ind
longest_length = length
ind += 1
# print(longest_ind)
# # print (msg2.polygons)
# print (len(msg2.polygons))
# print (len(planes_indices))
info_msg_polygon.polygons = msg2.polygons
info_msg_polygon.header = msg2.header
info_msg_coefficient.coefficients = [msg3.coefficients[longest_ind]]
info_msg_coefficient.header = msg3.header
pub_info_polygon.publish(info_msg_polygon)
pub_info_coefficient.publish(info_msg_coefficient)
# print('len(msg2.polygons)', len(msg2.polygons))
polygons = msg2.polygons[longest_ind]
if polygons is not None:
pub_info_polygonstamped.publish(polygons)
else:
print("finding planes")
def generatePolygons(header, model_index):
polygon = PolygonArray()
polygon.header = header
polygon.polygons = [PolygonStamped()]
polygon.polygons[0].header = header;
coef = ModelCoefficientsArray()
coef.coefficients = [ModelCoefficients()]
coef.header = header
if model_index in [0, 1, 2, 3]:
# Rectangle
polygon.polygons[0].polygon.points = [Point32(x=1.0, y=1.0),
Point32(x=-1.0, y=1.0),
Point32(x=-1.0, y=-1.0),
Point32(x=1.0, y=-1.0)]
coef.coefficients[0].header = header
coef.coefficients[0].values = [0, 0, 1, 0]
elif model_index == 4:
polygon.polygons[0].polygon.points = [Point32(x=0.0, y=-0.5, z=0.0),
Point32(x=0.0, y=-0.5, z=2.0),
Point32(x=0.0, y=0.5, z=2.0),
Point32(x=0.0, y=0.5, z=0.0)]
# polygon.polygons[0].polygon.points.reverse()
coef.coefficients[0].header = header
coef.coefficients[0].values = [-1, 0, 0, 0]
return (polygon, coef)
def _convert(self, msg):
polys_msg = PolygonArray()
polys_msg.header = msg.header
for rect in msg.rects:
poly_msg = PolygonStamped()
poly_msg.header = msg.header
pt0 = Point32(x=rect.x, y=rect.y)
pt1 = Point32(x=rect.x + rect.width, y=rect.y + rect.height)
poly_msg.polygon.points.append(pt0)
poly_msg.polygon.points.append(pt1)
polys_msg.polygons.append(poly_msg)
self.pub.publish(polys_msg)
def _convert(self, msg):
polys_msg = PolygonArray()
polys_msg.header = msg.header
for rect in msg.rects:
poly_msg = PolygonStamped()
poly_msg.header = msg.header
pt0 = Point32(x=rect.x, y=rect.y)
pt1 = Point32(x=rect.x + rect.width, y=rect.y + rect.height)
poly_msg.polygon.points.append(pt0)
poly_msg.polygon.points.append(pt1)
polys_msg.polygons.append(poly_msg)
self.pub.publish(polys_msg)
def callback(msg):
output_msg = PolygonArray()
output_msg.header = msg.header
output_msg.polygons = msg.polygons
output_msg.labels = msg.labels
output_msg.likelihood = msg.likelihood
# fill empty
output_msg.header.stamp = rospy.Time(0)
for i in range(len(output_msg.polygons)):
output_msg.polygons[i].header = output_msg.header
pub.publish(output_msg)
def generate_plane(self, header,
xmin=-0.0, xmax=1.0,
ymin=-1.0, ymax=1.0):
msg = PolygonArray()
msg.header = header
p = PolygonStamped()
p.header = header
p.polygon.points = [Point32(x=xmin, y=ymin),
Point32(x=xmax, y=ymin),
Point32(x=xmax, y=ymax),
Point32(x=xmin, y=ymax)]
msg.polygons.append(p)
return msg
def dynamic_custom_polygon_demo():
rospy.init_node("polygon_array_dynamic")
# /polygon_array_dynamic/output
pub = rospy.Publisher("~output", PolygonArray, queue_size=1)
r = rospy.Rate(5)
#while not rospy.is_shutdown():
msg = PolygonArray()
header = Header()
header.frame_id = "world"
header.stamp = rospy.Time.now()
msg.header = header
msg.polygons = [DynamicPolygon(header)]
msg.labels = [0]
msg.likelihood = [np.random.ranf()]
pub.publish(msg)
r.sleep()
def callback(self, msg, msg_coef):
if len(msg.polygons) > 0:
#self._pub.publish(msg.histograms[0])
max_index = max(xrange(len(msg.polygons)), key=lambda i: msg.likelihood[i])
res = PolygonArray()
res.header = msg.header
res.polygons = [msg.polygons[max_index]]
res.likelihood = [msg.likelihood[max_index]]
if msg.likelihood[max_index] < self._min_likelihood:
rospy.loginfo("Ignore result because of too small likelihood: {0} < {1}".format(
msg.likelihood[max_index],
self._min_likelihood))
return
self._pub.publish(res)
res_coef = ModelCoefficientsArray()
res_coef.header = msg.header
res_coef.coefficients = [msg_coef.coefficients[max_index]]
self._pub_coef.publish(res_coef)
def callback(polygon_msg, coeff_msg):
print "callback"
# odom->ground
max_val = -1.0
max_index = None
for i in range(len(polygon_msg.polygons)):
val = polygon_msg.likelihood
if max_val < val:
max_val = val
max_index = i
if max_index != None:
out_poly_msg = PolygonArray()
out_poly_msg.header = polygon_msg.header
out_poly_msg.polygons = [polygon_msg.polygons[max_index]]
pub_poly.publish(out_poly_msg)
out_coeff_msg = ModelCoefficientsArray()
out_coeff_msg.header = coeff_msg.header
out_coeff_msg.coefficients = [coeff_msg.coefficients[max_index]]
pub_coeff.publish(out_coeff_msg)
def callback(polygon_msg, coeff_msg):
print "callback"
# odom->ground
max_val = -1.0;
max_index = None
for i in range(len(polygon_msg.polygons)):
val = polygon_msg.likelihood
if max_val < val:
max_val = val
max_index = i
if max_index != None:
out_poly_msg = PolygonArray()
out_poly_msg.header = polygon_msg.header
out_poly_msg.polygons = [polygon_msg.polygons[max_index]]
pub_poly.publish(out_poly_msg)
out_coeff_msg = ModelCoefficientsArray()
out_coeff_msg.header = coeff_msg.header
out_coeff_msg.coefficients= [coeff_msg.coefficients[max_index]]
pub_coeff.publish(out_coeff_msg)
def callback(self, msg, msg_coef):
if len(msg.polygons) > 0:
#self._pub.publish(msg.histograms[0])
max_index = max(xrange(len(msg.polygons)),
key=lambda i: msg.likelihood[i])
res = PolygonArray()
res.header = msg.header
res.polygons = [msg.polygons[max_index]]
res.likelihood = [msg.likelihood[max_index]]
if msg.likelihood[max_index] < self._min_likelihood:
rospy.loginfo(
"Ignore result because of too small likelihood: {0} < {1}".
format(msg.likelihood[max_index], self._min_likelihood))
return
self._pub.publish(res)
res_coef = ModelCoefficientsArray()
res_coef.header = msg.header
res_coef.coefficients = [msg_coef.coefficients[max_index]]
self._pub_coef.publish(res_coef)
return p
if __name__ == "__main__":
rospy.init_node("polygon_array_sample")
pub = rospy.Publisher("~output", PolygonArray)
r = rospy.Rate(10)
br = tf.TransformBroadcaster()
while not rospy.is_shutdown():
msg = PolygonArray()
header = Header()
header.frame_id = "world"
header.stamp = rospy.Time.now()
msg.header = header
msg.polygons = [
SquarePolygon(header),
RectanglePolygon(header),
CirclePolygon(header),
StarPolygon(header)
]
msg.labels = [0, 1, 2, 3]
msg.likelihood = [
np.random.ranf(),
np.random.ranf(),
np.random.ranf(),
np.random.ranf()
]
pub.publish(msg)
def timer_cb(msg1):
plarray = PolygonArray()
plarray.header = msg1.header
plarray.polygons = [msg1]
pub_info_polygonstamped.publish(plarray)
def callback(self, point_cloud):
print("callback")
points = np.array(list(pc2.read_points(point_cloud, skip_nans=True)))
z_points = points[:, [2]]
self.max_z = max(z_points)
points_surface = points[:, [0, 1]] # choose x, y
points_surface = points_surface[np.argsort(
points_surface[:, 0])] # sort x
min_x = points_surface[0, 0]
max_x = points_surface[-1, 0]
points_surface = points_surface[np.argsort(
points_surface[:, 1])] # sort x
min_y = points_surface[0, 1]
max_y = points_surface[-1, 1]
self.image_pixel_zero_pos = [min_x, min_y]
pixel_size_x = ((max_x - min_x) / self.one_pixel_length)
pixel_size_y = ((max_y - min_y) / self.one_pixel_length)
pixel_size_x_int = int((max_x - min_x) / self.one_pixel_length)
pixel_size_y_int = int((max_y - min_y) / self.one_pixel_length)
if ((pixel_size_x - pixel_size_x_int) >= 0.5):
pixel_size_x_int = pixel_size_x_int + 1
if ((pixel_size_y - pixel_size_y_int) >= 0.5):
pixel_size_y_int = pixel_size_y_int + 1
np_image = np.zeros((pixel_size_x_int, pixel_size_y_int, 3))
print("image size:", pixel_size_x_int, pixel_size_y_int)
for i in points_surface:
x = i[0]
y = i[1]
pix_x = (x - self.image_pixel_zero_pos[0]) / self.one_pixel_length
pix_y = (y - self.image_pixel_zero_pos[1]) / self.one_pixel_length
pix_x_int = int(
(x - self.image_pixel_zero_pos[0]) / self.one_pixel_length)
pix_y_int = int(
(y - self.image_pixel_zero_pos[1]) / self.one_pixel_length)
if (pix_x - pix_x_int >= 0.5):
pix_x_int = pix_x_int + 1
if (pix_y - pix_y_int >= 0.5):
pix_y_int = pix_y_int + 1
np_image[pix_x_int - 1][pix_y_int - 1] = [255, 255, 255]
img = np_image.astype(np.uint8)
kernel = np.ones((5, 5), np.uint8)
img = cv2.morphologyEx(img, cv2.MORPH_CLOSE, kernel)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
_, bw = cv2.threshold(gray, 0, 255,
cv2.THRESH_BINARY | cv2.THRESH_OTSU)
ret = findSquares(bw, img)
img_polygons = ret[0]
img_centers = ret[1]
polygons = []
centers = []
for i in range(len(img_polygons)):
polygon = []
for j in range(len(img_polygons[0])):
pos_xy = self.pixel_to_pos(img_polygons[i][j])
polygon.append([pos_xy[0], pos_xy[1], self.max_z])
polygons.append(polygon)
pos_xy = self.pixel_to_pos(img_centers[i])
centers.append([pos_xy[0], pos_xy[1], self.max_z])
header = point_cloud.header
msg = PolygonArray()
msg.header = header
for i in range(len(polygons)):
p = PolygonStamped()
p.header = header
for j in range(len(polygons[0])):
xyz = polygons[i][j]
p.polygon.points.append(Point32(x=xyz[0], y=xyz[1], z=xyz[2]))
msg.polygons.append(p)
self.pub_polygons.publish(msg)
pub_msg = PoseArray()
pub_msg.header = point_cloud.header
for i in range(len(centers)):
pose = Pose()
pose.position.x = centers[i][0]
pose.position.y = centers[i][1]
pose.position.z = centers[i][2]
pub_msg.poses.append(pose)
self.pub_target_poses.publish(pub_msg)
p.header = header
p.polygon.points = [Point32(x= .0000, y= 1.0000 + 3.0),
Point32(x= .2245, y= .3090 + 3.0),
Point32(x= .9511, y= .3090 + 3.0),
Point32(x= .3633, y= -.1180 + 3.0),
Point32(x= .5878, y= -.8090 + 3.0),
Point32(x= .0000, y= -.3820 + 3.0),
Point32(x= -.5878, y= -.8090 + 3.0),
Point32(x= -.3633, y= -.1180 + 3.0),
Point32(x= -.9511, y= .3090 + 3.0),
Point32(x= -.2245, y= .3090 + 3.0)]
return p
if __name__ == "__main__":
rospy.init_node("polygon_array_sample")
pub = rospy.Publisher("~output", PolygonArray)
r = rospy.Rate(10)
while not rospy.is_shutdown():
msg = PolygonArray()
header = Header()
header.frame_id = "world"
header.stamp = rospy.Time.now()
msg.header = header
msg.polygons = [SquarePolygon(header),
RectanglePolygon(header),
CirclePolygon(header),
StarPolygon(header)]
msg.labels = [0, 1, 2, 3]
msg.likelihood = [np.random.ranf(), np.random.ranf(), np.random.ranf(), np.random.ranf()]
pub.publish(msg)
