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 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 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 pathLocalToPolArr(self):
pa = PolygonArray()
pa.header.stamp = rospy.Time.now()
pa.header.frame_id = "map"
for i in range(self.N-1):
spoly = np.array([self.pathlocal.s[i], self.pathlocal.s[i+1],self.pathlocal.s[i+1],self.pathlocal.s[i]])
dpoly = np.array([self.pathlocal.dub[i], self.pathlocal.dub[i+1],self.pathlocal.dlb[i+1],self.pathlocal.dlb[i]])
Xpoly, Ypoly = ptsFrenetToCartesian(spoly,dpoly,self.pathlocal.X,self.pathlocal.Y,self.pathlocal.psi_c,self.pathlocal.s)
p = PolygonStamped()
p.header.stamp = rospy.Time.now()
p.header.frame_id = "map"
p.polygon.points = [Point32(x=Xpoly[0], y=Ypoly[0], z=0.0),
Point32(x=Xpoly[1], y=Ypoly[1], z=0.0),
Point32(x=Xpoly[2], y=Ypoly[2], z=0.0),
Point32(x=Xpoly[3], y=Ypoly[3], z=0.0)]
pa.polygons.append(p)
#pa.Color.r = 1.0
#p.color.g = 0.0
#p.color.b = 0.0
# color for mu
pa.likelihood = self.pathlocal.mu[0:-1]*(0.2/np.max(self.pathlocal.mu)) # discarding final value
#pa.color.r = 1.0
#pa.color.g = 0.0
#pa.color.b = 0.0
return pa
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 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 publishPolygons(self, event=None):
pmsg = PolygonArray()
pmsg.header.stamp = rospy.Time.now()
cmsg = ModelCoefficientsArray()
cmsg.header.stamp = pmsg.header.stamp
for i in range(10):
pmsg.polygons.append(PolygonStamped())
pmsg.polygons[i].header.stamp = pmsg.header.stamp
pmsg.polygons[i].header.frame_id = str(i)
cmsg.coefficients.append(ModelCoefficients())
cmsg.coefficients[i].header.stamp = cmsg.header.stamp
cmsg.coefficients[i].header.frame_id = str(i)
pmsg.likelihood = [1.0,2.0,3.0,4.0,5.0,4.0,3.0,2.0,1.0,0.0]
self.pub_polygon.publish(pmsg)
self.pub_coefficients.publish(cmsg)
def publishPolygons(self, event=None):
pmsg = PolygonArray()
pmsg.header.stamp = rospy.Time.now()
cmsg = ModelCoefficientsArray()
cmsg.header.stamp = pmsg.header.stamp
for i in range(10):
pmsg.polygons.append(PolygonStamped())
pmsg.polygons[i].header.stamp = pmsg.header.stamp
pmsg.polygons[i].header.frame_id = str(i)
cmsg.coefficients.append(ModelCoefficients())
cmsg.coefficients[i].header.stamp = cmsg.header.stamp
cmsg.coefficients[i].header.frame_id = str(i)
pmsg.likelihood = [1.0, 2.0, 3.0, 4.0, 5.0, 4.0, 3.0, 2.0, 1.0, 0.0]
self.pub_polygon.publish(pmsg)
self.pub_coefficients.publish(cmsg)
def parse_params(self, param, frame_id):
# validate params
assert isinstance(param, list), "polygons must be list"
has_label = any("label" in p for p in param)
has_likelihood = any("likelihood" in p for p in param)
for polygon in param:
assert "points" in polygon and isinstance(polygon["points"], list),\
"each polygon must have at least 1 'points'"
for point in polygon["points"]:
assert len(point) == 3,\
"element of 'points' must be list of 3 numbers"
if has_label:
assert "label" in polygon, "missing 'label'"
if has_likelihood:
assert "likelihood" in polygon, "missing 'likelihood'"
# parse params
msg = PolygonArray()
msg.header.frame_id = frame_id
for polygon in param:
ps = PolygonStamped()
ps.header.frame_id = frame_id
ps.polygon.points = [Point32(*v) for v in polygon["points"]]
msg.polygons.append(ps)
if has_label:
msg.labels.append(polygon["label"])
if has_likelihood:
msg.likelihood.append(polygon["likelihood"])
return msg
def makePolygonArray():
global_frame_id = rospy.myargv()[1]
ret = PolygonArray()
ret.header.stamp = rospy.Time.now()
ret.header.frame_id = global_frame_id
for i in range(-1, 5):
# i = 0, 1, 2, 3, 4, 5
x1 = i * 3 - 0.5
x2 = i * 3 + 1 - 0.5
x3 = i * 3 + 2 - 0.5
x4 = i * 3 + 3 - 0.5
points_array = [[[x2, 2, 0], [x1, 2, 0], [x1, -2, 0], [x2, -2, 0]],
[[x3, 2, 0.3], [x2, 2, 0.2], [x2, -2, 0.2],
[x3, -2, 0.3]],
[[x4, 2, 0.1], [x3, 2, 0.1], [x3, -2, 0.1],
[x4, -2, 0.1]]]
for points in points_array:
polygon = PolygonStamped()
polygon.header.frame_id = global_frame_id
polygon.header.stamp = rospy.Time.now()
for p in points:
polygon.polygon.points.append(Point32(*p))
ret.polygons.append(polygon)
return ret
def getting_cordi(A,B):
theta_increment=(2)*(math.pi)/360
re=PolygonArray()
re.header.frame_id="odom"
roar=PolygonStamped()
roar.header.frame_id="odom"
pt=[]
count=0
# print("This is A:")
# print(A)
for i in range(len(A)):
if str(A[i])!="inf":
count+=1
theta1=(i)*theta_increment
# print(A[i])
x=A[i]*math.sin(theta1)
y=A[i]*math.cos(theta1)
roar.polygon.points.append(Point32(x,y,0))
pt.append([x,y,theta1,i])
if i==len(A)-1 or str(A[i+1])=="inf":
for j in range(i,i-count,-1):
theta2=(j)*theta_increment
x=B[j]*math.sin(theta2)
y=B[j]*math.cos(theta2)
roar.polygon.points.append(Point32(x,y,0))
pt.append([x,y,theta2,j])
# print(pt)
pt=[]
count=0
# print("***************************************************************")
# print(poly.points)
re.polygons.append(roar)
roar=PolygonStamped()
roar.header.frame_id="odom"
return re
def getting_cordi(A,B,shu):
theta_increment=shu
re=PolygonArray()
re.header.frame_id="base_scan"
roar=PolygonStamped()
roar.header.frame_id="base_scan"
roar.header.stamp=rospy.Time.now()
pt=Exp_msg()
count=0
baby=Ipoly()
for i in range(len(A)):
if str(A[i])!="inf":
if count==0:
for t in range(3,1,-1):
theta1=(i-t)*theta_increment
x=A[i]*math.cos(theta1)
y=A[i]*math.sin(theta1)
roar.polygon.points.append(Point32(x,y,0))
pt.bliss.append(Cordi(x,y,0))
count+=1
theta1=(i)*theta_increment
x=A[i]*math.cos(theta1)
y=A[i]*math.sin(theta1)
roar.polygon.points.append(Point32(x,y,0))
pt.bliss.append(Cordi(x,y,0))
if i==len(A)-1 or str(A[i+1])=="inf" or abs(A[i+1]-A[i])>0.1:
for t in range(1,4):
theta1=(t+i)*theta_increment
x=A[i]*math.cos(theta1)
y=A[i]*math.sin(theta1)
roar.polygon.points.append(Point32(x,y,0))
pt.bliss.append(Cordi(x,y,0))
for t in range(3,1,-1):
theta1=(t+i)*theta_increment
x=B[i]*math.cos(theta1)
y=B[i]*math.sin(theta1)
roar.polygon.points.append(Point32(x,y,0))
pt.bliss.append(Cordi(x,y,0))
for j in range(i,i-count,-1):
theta2=(j)*theta_increment
x=B[j]*math.cos(theta2)
y=B[j]*math.sin(theta2)
roar.polygon.points.append(Point32(x,y,0))
pt.bliss.append(Cordi(x,y,0))
for t in range(-1,-4,-1):
theta2=(j+t)*theta_increment
x=B[j]*math.cos(theta2)
y=B[j]*math.sin(theta2)
roar.polygon.points.append(Point32(x,y,0))
pt.bliss.append(Cordi(x,y,0))
baby.eternal_bliss.append(pt)
pt=Exp_msg()
count=0
re.polygons.append(roar)
roar=PolygonStamped()
roar.header.frame_id="base_scan"
roar.header.stamp=rospy.Time.now()
pubf=rospy.Publisher("ol2",Ipoly,queue_size=0)
pubf.publish(baby)
return re
def getting_cordi(A, B, shu):
theta_increment = shu
re = PolygonArray()
re.header.frame_id = "odom"
roar = PolygonStamped()
roar.header.frame_id = "odom"
roar.header.stamp = rospy.Time.now()
pt = Exp_msg()
count = 0
baby = Ipoly()
for i in range(len(A)):
if str(A[i]) != "inf":
if count == 0:
for t in range(10, 1, -1):
theta1 = (i - t) * theta_increment
x = A[i] * math.cos(theta1)
y = A[i] * math.sin(theta1)
roar.polygon.points.append(Point32(x, y, 0))
pt.bliss.append(Cordi(x, y, 0))
count += 1
theta1 = (i) * theta_increment
x = A[i] * math.cos(theta1)
y = A[i] * math.sin(theta1)
roar.polygon.points.append(Point32(x, y, 0))
pt.bliss.append(Cordi(x, y, 0))
if i == len(A) - 1 or abs(A[i + 1] - A[i]) > 0.15:
# ra=A[i]/math.cos(theta_increment)
for t in range(1, 10):
theta1 = (t + i) * theta_increment
# ra=ra/math.cos(theta_increment)
x = A[i] * math.cos(theta1)
y = A[i] * math.sin(theta1)
roar.polygon.points.append(Point32(x, y, 0))
pt.bliss.append(Cordi(x, y, 0))
for t in range(10, 1, -1):
theta1 = (t + i) * theta_increment
x = (B[i]) * math.cos(theta1)
y = (B[i]) * math.sin(theta1)
roar.polygon.points.append(Point32(x, y, 0))
pt.bliss.append(Cordi(x, y, 0))
for j in range(i, i - count, -1):
theta2 = (j) * theta_increment
x = B[j] * math.cos(theta2)
y = B[j] * math.sin(theta2)
roar.polygon.points.append(Point32(x, y, 0))
pt.bliss.append(Cordi(x, y, 0))
for t in range(-1, -10, -1):
theta2 = (j + t) * theta_increment
x = B[j] * math.cos(theta2)
y = B[j] * math.sin(theta2)
roar.polygon.points.append(Point32(x, y, 0))
pt.bliss.append(Cordi(x, y, 0))
baby.eternal_bliss.append(pt)
pt = Exp_msg()
count = 0
re.polygons.append(roar)
roar = PolygonStamped()
roar.header.frame_id = "odom"
roar.header.stamp = rospy.Time.now()
return re
def Spawner(self):
polygon_array = PolygonArray()
polygon_array.header.stamp = rospy.Time.now()
polygon_array.header.frame_id = "map"
for i, polygon_poses in enumerate(
self.rviz_polygon_array_def["polygon_array_poses"]):
polygon = PolygonStamped()
polygon.header.stamp = rospy.Time.now()
polygon.header.frame_id = "map"
for pose in polygon_poses:
polygon.polygon.points.append(
Point32(pose.position.x, pose.position.y, pose.position.z))
polygon_array.labels.append(1)
polygon_array.likelihood.append(1.0)
polygon_array.polygons.append(polygon)
# polygon_array.labels = self.rviz_polygon_array_def["labels"]#[i]
# polygon_array.likelihood = self.rviz_polygon_array_def["likelihood"]#[i]
# polygon_array.labels = [1,1,1,1,1,1]
# polygon_array.likelihood = [1.0,1.0,1.0,1.0,1.0,1.0]
self.polygon_array = polygon_array
t = 0
while not rospy.is_shutdown() and t < 10:
self.polygon_array_pub.publish(self.polygon_array)
t = t + 1
time.sleep(0.05)
def getting_cordi(A,B,shu):
theta_increment=shu
re=PolygonArray()
re.header.frame_id="odom"
roar=PolygonStamped()
roar.header.frame_id="odom"
pt=[]
count=0
# print("This is A:")
# print(A)
for i in range(len(A)):
if str(A[i])!="inf":
if count==0:
for t in range(3,1,-1):
theta1=(i-t)*theta_increment
# print(A[i])
x=A[i]*math.cos(theta1)
y=A[i]*math.sin(theta1)
roar.polygon.points.append(Point32(x,y,0))
count+=1
theta1=(i)*theta_increment
# print(A[i])
x=A[i]*math.cos(theta1)
y=A[i]*math.sin(theta1)
roar.polygon.points.append(Point32(x,y,0))
pt.append([x,y,theta1,i])
if i==len(A)-1 or str(A[i+1])=="inf" or abs(A[i+1]-A[i])>0.5:
for t in range(1,5):
theta1=(t+i)*theta_increment
x=A[i]*math.cos(theta1)
y=A[i]*math.sin(theta1)
roar.polygon.points.append(Point32(x,y,0))
for t in range(3,0,-1):
theta1=(t+i)*theta_increment
x=B[i]*math.cos(theta1)
y=B[i]*math.sin(theta1)
roar.polygon.points.append(Point32(x,y,0))
for j in range(i,i-count,-1):
theta2=(j)*theta_increment
x=B[j]*math.cos(theta2)
y=B[j]*math.sin(theta2)
roar.polygon.points.append(Point32(x,y,0))
pt.append([x,y,theta2,j])
for t in range(-1,-3,-1):
theta2=(j+t)*theta_increment
x=B[j]*math.cos(theta2)
y=B[j]*math.sin(theta2)
roar.polygon.points.append(Point32(x,y,0))
# print(pt)
pt=[]
count=0
# print("***************************************************************")
# print(poly.points)
re.polygons.append(roar)
roar=PolygonStamped()
roar.header.frame_id="odom"
return re
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)
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 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 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 imu_cb(imu):
ax, ay, az = imu.linear_acceleration.x, imu.linear_acceleration.y, imu.linear_acceleration.z
# rospy.loginfo("%f %f %f" % (ax, ay, az))
if az > 0.1:
dx, dy = [-az, 0, ax], [0, -az, ay]
elif ay > 0.1:
dx, dy = [-ay, ax, 0], [0, az, -ay]
else:
dx, dy = [ay, -ax, 0], [az, 0, -az]
PStamped = PolygonStamped()
for x, y in [[-10, -10], [-10, 10], [10, 10], [10, -10]]:
PStamped.polygon.points.append(
Point32(x * dx[0] + y * dy[0], x * dx[1] + y * dy[1],
x * dx[2] + y * dy[2]))
PStamped.header = imu.header
PArrayPub.publish(PolygonArray(imu.header, [PStamped]))
normal_array = np.array([ax, ay, az, 0])
normal_array = normal_array / np.linalg.norm(x)
MStamped = ModelCoefficients(imu.header, normal_array)
MArrayPub.publish(ModelCoefficientsArray(imu.header, [MStamped]))
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, 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)
def timer_cb(msg1):
plarray = PolygonArray()
plarray.header = msg1.header
plarray.polygons = [msg1]
pub_info_polygonstamped.publish(plarray)
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)
if __name__ == '__main__':
rospy.init_node("bounding_box_to_polygon_array")
pub = rospy.Publisher("output", PolygonArray)
rospy.Subscriber("/tablecloth_bb", BoundingBoxArray, sub_bb)
r = rospy.Rate(10)
bb = BoundingBox()
pa = PolygonArray()
face = rospy.get_param("face", "xy")
rospy.spin()
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)
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(),
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)
def getting_cordi(A, B, shu):
theta_increment = shu
re = PolygonArray()
re.header.frame_id = "base_scan"
roar = PolygonStamped()
roar.header.frame_id = "base_scan"
pt = Exp_msg()
count = 0
baby = Ipoly()
for i in range(len(A)):
if str(A[i]) != "inf":
if count == 0:
for t in range(3, 1, -1):
theta1 = (i - t) * theta_increment
x = A[i] * math.cos(theta1)
y = A[i] * math.sin(theta1)
roar.polygon.points.append(Point32(x, y, 0))
pt.bliss.append(Cordi(x, y, 0))
count += 1
theta1 = (i) * theta_increment
x = A[i] * math.cos(theta1)
y = A[i] * math.sin(theta1)
roar.polygon.points.append(Point32(x, y, 0))
pt.bliss.append(Cordi(x, y, 0))
if i == len(A) - 1 or str(
A[i + 1]) == "inf" or abs(A[i + 1] - A[i]) > 0.5:
for t in range(1, 5):
theta1 = (t + i) * theta_increment
x = A[i] * math.cos(theta1)
y = A[i] * math.sin(theta1)
roar.polygon.points.append(Point32(x, y, 0))
pt.bliss.append(Cordi(x, y, 0))
for t in range(3, 0, -1):
theta1 = (t + i) * theta_increment
x = B[i] * math.cos(theta1)
y = B[i] * math.sin(theta1)
roar.polygon.points.append(Point32(x, y, 0))
pt.bliss.append(Cordi(x, y, 0))
for j in range(i, i - count, -1):
theta2 = (j) * theta_increment
x = B[j] * math.cos(theta2)
y = B[j] * math.sin(theta2)
roar.polygon.points.append(Point32(x, y, 0))
pt.bliss.append(Cordi(x, y, 0))
for t in range(-1, -3, -1):
theta2 = (j + t) * theta_increment
x = B[j] * math.cos(theta2)
y = B[j] * math.sin(theta2)
roar.polygon.points.append(Point32(x, y, 0))
pt.bliss.append(Cordi(x, y, 0))
baby.eternal_bliss.append(pt)
pt = Exp_msg()
count = 0
re.polygons.append(roar)
roar = PolygonStamped()
roar.header.frame_id = "base_scan"
pubf = rospy.Publisher("ol2", Ipoly, queue_size=0)
pubf.publish(baby)
# print "bot yaw calculator ", bot_yaw
final_polys = PolygonArray()
# print "New"
for p in re.polygons:
# print " "
temp_poly = PolygonStamped()
temp_poly.header.frame_id = "odom"
for point in p.polygon.points:
temp_point = numpy.array([[point.x], [point.y]])
# print temp_point
# print point.x, point.y
# print "bot yaw calculator ", bot_yaw
cos = math.cos(-bot_yaw)
sin = math.sin(-bot_yaw)
# print cos, sin
rot_mat = numpy.array([[cos, sin], [-sin, cos]])
# rot_mat=numpy.array([[1 , 0],[0 , 1]])
trans_mat = numpy.array([[bot_x], [bot_y]])
rot_point = numpy.matmul(rot_mat, temp_point)
# print rot_point
trans_point = rot_point + trans_mat
# print trans_point[0], trans_point[1]
# point.x = trans_point[0]
# point.y = trans_point[1]
t_point = Point32()
t_point.x = trans_point[0]
t_point.y = trans_point[1]
temp_poly.polygon.points.append(t_point)
final_polys.polygons.append(temp_poly)
final_polys.header.frame_id = "odom"
return final_polys