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 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 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 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 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 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(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)
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)
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)
br.sendTransform((0, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(), 'map', 'world')
