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 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 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 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(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)