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