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