def get_all_markers(self, frame_id="map"): if len(self.points) > 0: wx, wy, wz = getDims(self.points[0], self.points[1]) else: wx, wy, wz = 0, 0, 0 marker = Marker() clr_ = create_color(1.0, 0, 0, 1.0) pts = [ center(mn, mx) for mn, mx in zip(self.points[0::2], self.points[1::2]) ] colors = [clr_ for x in pts] marker.type = marker.CUBE_LIST marker.action = marker.ADD marker.scale.x = abs(wx) marker.scale.y = abs(wy) marker.scale.z = abs(wz) marker.color.a = 1.0 marker.color.g = 1.0 marker.pose.position.x = 0 marker.pose.position.y = 0 marker.pose.position.z = 0 marker.pose.orientation.w = 1.0 marker.points = pts marker.colors = colors marker.frame_locked = True marker.header.frame_id = frame_id return [marker]
def draw_curve( self, pose_array, id=None, rgba=(0, 1, 0, 1), width=.01, ns='default_ns', duration=0, type=Marker.LINE_STRIP, ): if id is None: id = self.get_unused_id() marker = Marker(type=type, action=Marker.ADD) marker.header = pose_array.header marker.points = [pose.position for pose in pose_array.poses] marker.lifetime = rospy.Duration(0) if isinstance(rgba, list): assert len(rgba) == len(pose_array.poses) marker.colors = [stdm.ColorRGBA(*c) for c in rgba] else: marker.color = stdm.ColorRGBA(*rgba) marker.scale = gm.Vector3(width, width, width) marker.id = id marker.ns = ns self.pub.publish(marker) self.ids.add(id) return MarkerHandle(marker, self.pub)
def _regenerateMesh(self): self.mesh = mesh_creation.create_cut_cylinder(radius=self.radius, height=self.height, cutting_planes=[ ([0., 0., 0.], [1., 0., 0.]) ], sections=10) mesh_marker = Marker() mesh_marker.type = Marker.TRIANGLE_LIST mesh_marker.color.r = self.color[0] mesh_marker.color.g = self.color[1] mesh_marker.color.b = self.color[2] if len(self.color) > 3: mesh_marker.color.a = self.color[3] else: mesh_marker.color.a = 1. tris = self.mesh.faces.ravel() verts = self.mesh.vertices[tris, :] mesh_marker.points = ros_utils.arrayToPointMsgs(verts.T) mesh_marker.colors = [ std_msgs.msg.ColorRGBA(self.color[0], self.color[1], self.color[2], self.color[3]) ] * tris.shape[0] if self.mesh_control is not None: self.im_marker.controls.remove(self.mesh_control) self.mesh_control = InteractiveMarkerControl() self.mesh_control.always_visible = True self.mesh_control.markers.append(mesh_marker) self.im_marker.controls.append(self.mesh_control)
def make_wire_marker(mat, scale = 1): marker = Marker() marker.header.frame_id = "base" marker.header.stamp = rospy.Time.now() marker.ns = "z_wire_phantom" marker.id = 0 marker.action = Marker.MODIFY marker.type = Marker.LINE_LIST marker.pose.position.x = mat[0,3] * scale marker.pose.position.y = mat[1,3] * scale marker.pose.position.z = mat[2,3] * scale q = t3d.quaternions.mat2quat(mat[0:3,0:3]) marker.pose.orientation.x = q[1] marker.pose.orientation.y = q[2] marker.pose.orientation.z = q[3] marker.pose.orientation.w = q[0] marker.color.a = 1.0 marker.color.r = 0.5 marker.color.g = 0.5 marker.color.b = 1.0 marker.scale.x = 0.001 * scale wires = _wires * scale marker.points = [Point(p[0], p[1], p[2]) for p in wires] marker.colors = [marker.color for c in range(len(marker.points))] return marker
def publish_wall(self, xy_valid): ps = self.wall_finder_(xy_valid) if ps is None: return rospy.loginfo_throttle(1.0, '# Walls Detected : {}'.format(len(ps))) #r, t = rt ### points #po = np.stack([r * np.cos(t), r * np.sin(t)],axis=-1).reshape(-1,1,2) # (n,1,2) ### vectors #u = np.stack([np.sin(t), -np.cos(t)], axis=-1).reshape(-1,1,2) #ps = po + ((3.0*u) * np.reshape([-1.0, 1.0], (1,2,1)) ) ps = np.asarray(ps, dtype=np.float32) col = ColorRGBA(1.0, 0.0, 1.0, 1.0) msg = Marker() msg.lifetime = rospy.Duration(1.0) msg.header.frame_id = 'base_link' msg.header.stamp = self.scan_t_ #rospy.Time.now() msg.type = msg.LINE_LIST msg.action = msg.ADD msg.points = [] msg.pose.position = Point() msg.pose.orientation = Quaternion(0, 0, 0, 1) for (p0, p1) in ps: msg.points.extend( [Point(x=p0[0], y=p0[1]), Point(x=p1[0], y=p1[1])]) msg.scale.x = msg.scale.y = msg.scale.z = 0.1 msg.colors = [col for _ in msg.points] self.wall_pub_.publish(msg)
def publish_quad(self, range, angle): """ Publish four points front,back,left,right to the robot """ msg = Marker() msg.lifetime = rospy.Duration(1.0) msg.header.frame_id = 'base_link' fwd = range[np.argmin(np.abs(U.adiff(angle, 0.0 * np.pi)))] fwd = Point(x=fwd) lft = range[np.argmin(np.abs(U.adiff(angle, 0.5 * np.pi)))] lidx = np.argmin(np.abs(U.adiff(angle, 0.5 * np.pi))) print 'lft', lft, lidx lft = Point(y=lft) bwd = range[np.argmin(np.abs(U.adiff(angle, 1.0 * np.pi)))] bwd = Point(x=-bwd) rgt = range[np.argmin(np.abs(U.adiff(angle, -0.5 * np.pi)))] rgt = Point(y=-rgt) col = ColorRGBA(1.0, 1.0, 1.0, 1.0) msg.type = msg.POINTS msg.action = msg.ADD msg.points = [fwd, lft, bwd, rgt] msg.colors = [col, col, col, col] msg.scale.x = msg.scale.y = msg.scale.z = 0.2 self.viz_pub_.publish(msg)
def make_pose_marker(mat, scale = 1): marker = Marker() marker.header.frame_id = "base" marker.header.stamp = rospy.Time.now() marker.ns = "z_wire" marker.id = 0 marker.action = Marker.MODIFY marker.type = Marker.LINE_LIST marker.pose.position.x = mat[0,3] * scale marker.pose.position.y = mat[1,3] * scale marker.pose.position.z = mat[2,3] * scale q = t3d.quaternions.mat2quat(mat[0:3,0:3]) marker.pose.orientation.x = q[1] marker.pose.orientation.y = q[2] marker.pose.orientation.z = q[3] marker.pose.orientation.w = q[0] marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.scale.x = 0.001 * scale marker.points = [Point() for p in range(6)] marker.points[1].x = 0.005 * scale marker.points[3].y = 0.005 * scale marker.points[5].z = 0.005 * scale marker.colors = [ColorRGBA(1,0,0,1), ColorRGBA(1,0,0,1), ColorRGBA(0,1,0,1), ColorRGBA(0,1,0,1), ColorRGBA(0,0,1,1), ColorRGBA(0,0,1,1)] return marker
def add_leader_plot(self, msg, height=0.25): print("Adding leader.") leader_marker = Marker() leader_marker.id = self.current_id leader_marker.action = Marker.ADD leader_marker.header.frame_id = 'map' leader_marker.type = Marker.LINE_STRIP leader_marker.ns = "points" leader_marker.scale.x = 0.25 leader_marker.colors = [] leader_marker.points = [] leader_marker.points.append( Point(msg.leader.initial[0], msg.leader.initial[1], height)) leader_marker.points.append( Point(msg.leader.line_start[0], msg.leader.line_start[1], height)) leader_marker.points.append( Point(msg.leader.line_end[0], msg.leader.line_end[1], height)) leader_marker.points.append( Point(msg.leader.goal[0], msg.leader.goal[1], height)) leader_marker.colors.append(ColorRGBA(1.0, 0.0, 0.0, 1.0)) leader_marker.colors.append(ColorRGBA(1.0, 0.0, 0.0, 1.0)) leader_marker.colors.append(ColorRGBA(1.0, 0.0, 0.0, 1.0)) leader_marker.colors.append(ColorRGBA(1.0, 0.0, 0.0, 1.0)) leader_marker.lifetime = rospy.Duration(self.refresh_rate) self.current_id = self.current_id + 1 self.Markers.markers.append(leader_marker)
def write_dict_to_topic(self): if not self.detected_items: return msg_list = TrackedObjectList() for key in self.detected_items: msg = TrackedObject() msg.name = key msg.x = self.detected_items[key].x msg.y = self.detected_items[key].y msg.th = self.detected_items[key].th msg.robot_pose = self.detected_items[key].pose msg.confidence = self.detected_items[key].confidence msg_list.ob_msgs.append(msg) self.pub.publish(msg_list) # Publish Markers point_list = [] color_list = [] for key in self.detected_items: point_list.append( Point(self.detected_items[key].x, self.detected_items[key].y, 0.5)) if key[0:len(key) - 1] == "stop_sign": color_list.append(ColorRGBA(1.0, 0, 0, 1.0)) else: color_list.append(ColorRGBA(0, 1.0, 0.1, 1.0)) m = Marker() m.type = Marker.CUBE_LIST m.action = Marker.ADD m.header.frame_id = "map" m.scale.x = 0.07 m.scale.y = 0.07 m.scale.z = 1.5 m.points = point_list m.colors = color_list self.pub_marker.publish(m)
def copy_marker_attributes(sample): marker = Marker() marker.header.frame_id = sample.header.frame_id marker.header.stamp = sample.header.stamp marker.id = sample.id marker.type = sample.type marker.action = sample.action marker.scale.x = sample.scale.x marker.scale.y = sample.scale.y marker.scale.z = sample.scale.z marker.pose.position.x = sample.pose.position.x marker.pose.position.y = sample.pose.position.y marker.pose.position.z = sample.pose.position.z marker.pose.orientation.x = sample.pose.orientation.x marker.pose.orientation.y = sample.pose.orientation.y marker.pose.orientation.z = sample.pose.orientation.z marker.pose.orientation.w = sample.pose.orientation.w marker.color.a = sample.color.a marker.color.r = sample.color.r marker.color.g = sample.color.g marker.color.b = sample.color.b marker.text = sample.text marker.mesh_resource = sample.mesh_resource marker.mesh_use_embedded_materials = sample.mesh_use_embedded_materials marker.points = sample.points marker.colors = sample.colors return marker
def create_point_list(cls, ref_pose, points, colors, size=0.01, namespace='point_list'): """ Creates a point list referenced to a pose Args: ref_pose (PoseStamped): This is used for the header and the ref_points points (list): List of Point objects with the positions relative to ref_pose colors (list): The colors for the points size (float): size of the points in meters """ marker = Marker() marker.type = Marker.POINTS marker.header = ref_pose.header marker.pose = ref_pose.pose marker.ns = namespace marker.action = Marker.ADD marker.scale.x = size marker.scale.y = size if isinstance(colors[0], list): # colors defined for every element marker.colors = [cls.rgba_from_list(color) for color in colors] else: marker.color = cls.rgba_from_list(colors) marker.points = points return marker
def add_follower_plot(self, msg, height=0.25): print("Adding follower") follower_marker = Marker() follower_marker.id = self.current_id follower_marker.action = Marker.ADD follower_marker.header.frame_id = 'map' follower_marker.type = Marker.LINE_STRIP follower_marker.ns = "points" follower_marker.scale.x = 0.25 follower_marker.colors = [] follower_marker.points = [] follower_marker.points.append( Point(msg.follower.initial[0], msg.follower.initial[1], height)) follower_marker.points.append( Point(msg.follower.line_start[0], msg.follower.line_start[1], height)) follower_marker.points.append( Point(msg.follower.line_end[0], msg.follower.line_end[1], height)) follower_marker.points.append( Point(msg.follower.goal[0], msg.follower.goal[1], height)) follower_marker.colors.append(ColorRGBA(0.0, 0.0, 1.0, 1.0)) follower_marker.colors.append(ColorRGBA(0.0, 0.0, 1.0, 1.0)) follower_marker.colors.append(ColorRGBA(0.0, 0.0, 1.0, 1.0)) follower_marker.colors.append(ColorRGBA(0.0, 0.0, 1.0, 1.0)) follower_marker.lifetime = rospy.Duration(self.refresh_rate) self.Markers.markers.append(follower_marker) self.current_id = self.current_id + 1
def publish(self, suc, info): # area ... # self.ar_msg_.header.stamp = rospy.Time.now() # self.ar_msg_.header.frame_id = 'base_link' # self.ar_msg_.polygon = Polygon([Point32(x,y,0) for (x,y) in self.ar_]) # self.ar_pub_.publish(self.ar_msg_) # detections ... if not suc: return p0, p1 = info p0x,p0y = p0 # np.mean(c0,axis=0) # centroid p1x,p1y = p1 # np.mean(c1,axis=0) # centroid p0 = Point(x=p0x,y=p0y) p1 = Point(x=p1x,y=p1y) col = ColorRGBA(1.0,1.0,1.0,1.0) msg = Marker() msg.lifetime = rospy.Duration(1.0) msg.header.frame_id = 'odom' msg.type = msg.POINTS msg.action = msg.ADD msg.points = [p0, p1] msg.colors = [col,col] msg.scale.x = msg.scale.y = msg.scale.z = 0.2 self.viz_pub_.publish(msg)
def _draw_sphere_list(self, uid, namespace, scale, points, color=None, colors=None): # type: (int, str, Vector3, list, ColorRGBA, list) -> None marker = Marker() # Header marker.header.frame_id = "base_link" marker.header.stamp = rospy.Time() # Body marker.ns = namespace marker.id = uid marker.type = Marker.SPHERE_LIST marker.action = Marker.ADD marker.scale = scale marker.pose.orientation = Quaternion(0, 0, 0, 1) marker.points = points if color is not None: marker.color = color if colors is not None: marker.colors = colors self._visualization_publisher.publish(marker)
def draw_risk_grid(self, risk_grid): if not rospy.is_shutdown(): p_list = list() c_list = list() x_gen = xrange(0, self.x_dim, 1) y_gen = xrange(0, self.y_dim, 1) for i in x_gen: for j in y_gen: risk = risk_grid[i, j] pnt = Point(i, j, 0) r, g, b, a = self.sm.to_rgba(risk) clr = ColorRGBA(r, g, b, a) p_list.append(pnt) c_list.append(clr) marker = Marker() marker.header.frame_id = "/my_frame" marker.lifetime = rospy.Duration(10000000) marker.type = marker.POINTS marker.scale.x = 1 marker.scale.y = 1 marker.action = marker.ADD marker.points = p_list marker.colors = c_list marker.id = -1 self.pub.publish(marker) return self
def line(self, frame_id, p, r, t=[0.0, 0.0], key=None): """ line: t r + p This will be drawn for t[0] .. t[1] """ r = np.array(r) p = np.array(p) m = Marker() m.header.frame_id = frame_id m.ns = key or 'lines' m.id = 0 if key else len(self.lines) m.type = Marker.LINE_STRIP m.action = Marker.MODIFY m.pose = Pose(Point(0,0,0), Quaternion(0,0,0,1)) m.scale = Vector3(0.005, 0.005, 0.005) m.color = ColorRGBA(0,0.8,0.8,1) m.points = [Point(*(p+r*t)) for t in np.linspace(t[0], t[1], 10)] m.colors = [m.color] * 10 key = key or element_key(m) with self.mod_lock: self.lines[key] = m return key
def publish_marker(points, header, marker_id): marker = Marker() marker.header = header marker.ns = 'sensfloor' marker.type = Marker.POINTS marker.lifetime = RVIZ_MARKER_LIFETIME marker.pose.orientation.w = 1 marker.scale.x = RVIZ_MARKER_SCALE marker.scale.y = RVIZ_MARKER_SCALE marker.id = marker_id marker.points = map(lambda x: Point(x=x.x, y=x.y), points) marker.colors = map( lambda x: ColorRGBA( r=max( 0.01, min(1.0, float(x.c - NEUTRAL_SENSOR_VALUE) / SENSOR_COLOUR_RANGE)), g=0.0, b=0.0, a=max( 0.01, min(1.0, float(x.c - NEUTRAL_SENSOR_VALUE) / SENSOR_COLOUR_RANGE))), points) # print marker.colors markerPub.publish(marker)
def make_Marker(slef, markerP, id): marker = Marker() marker.type = Marker.LINE_STRIP marker.header.frame_id = "map" marker.header.stamp = rospy.Time.now() marker.id = id marker.action = marker.ADD marker.pose.position.x = 0 marker.pose.position.y = 0 marker.pose.position.z = 0 marker.pose.orientation.x = 0.0 marker.pose.orientation.y = 0.0 marker.pose.orientation.z = 0.0 marker.pose.orientation.w = 1.0 marker.scale.x = 0.1 marker.scale.y = 0 marker.scale.z = 0 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 1.0 marker.points = markerP marker.colors = [ColorRGBA(1, 0, 0, 1) for i in markerP] return marker
def marker(self, position=None, orientation=None, points=None, colors=None, scale=1.0): mark = Marker() mark.header.stamp = rospy.Time.now() mark.header.frame_id = self.frame_id mark.ns = self.ns mark.type = self.type mark.id = self.counter mark.action = self.action mark.scale.x = self.scale[0] mark.scale.y = self.scale[1] mark.scale.z = self.scale[2] mark.color.r = self.color[0] mark.color.g = self.color[1] mark.color.b = self.color[2] mark.color.a = self.color[3] mark.lifetime = rospy.Duration(self.lifetime) if points is not None: mark.points = [] for point in points: mark.points.append(get_point(point, scale)) if colors is not None: mark.colors = colors if position is not None or orientation is not None: mark.pose.position = get_point(position, scale) mark.pose.orientation = get_quat(orientation) self.counter+=1 return mark
def markers_for_skeleton(self, skeleton, index=0, sphere_color=None, line_color=None): sphere_color = ColorRGBA(1, 0, 0, 1) if not sphere_color else sphere_color line_color = ColorRGBA(0, 0, 1, 1) if not line_color else line_color joints_marker = Marker() joints_marker.id = index # joints_marker.lifetime = rospy.Duration(30) joints_marker.type = Marker.SPHERE_LIST joints_marker.scale.x, joints_marker.scale.y, joints_marker.scale.z = 0.05, 0.05, 0.05 joints_marker.action = Marker.ADD joints_marker.ns = "skeleton_spheres" joints_marker.header.frame_id = skeleton.bodyparts.values( )[0].header.frame_id # Just take the first one joints_marker.header.stamp = rospy.Time.now() joints_marker.points = [ joint_ps.point for joint_name, joint_ps in skeleton.items() ] joints_marker.colors = [sphere_color for _, _ in skeleton.items()] rospy.loginfo("Added {} joints for skeleton {}".format( len(joints_marker.points), skeleton)) links_marker = Marker() links_marker.id = index # links_marker.lifetime = rospy.Duration(30) links_marker.type = Marker.LINE_LIST links_marker.ns = "skeleton_lines" links_marker.scale.x = 0.02 links_marker.action = Marker.ADD links_marker.header.frame_id = skeleton.bodyparts.values( )[0].header.frame_id # Just take the first one links_marker.header.stamp = rospy.Time.now() links_marker.points = list(skeleton.generate_links()) links_marker.colors = [ line_color for _ in links_marker.points ] # TODO: not in sync, whould iterate over pairs of points here rospy.loginfo("Added {} links for skeleton {}".format( len(links_marker.points) / 2, skeleton)) # /2 because lines are pairs of points return [joints_marker, links_marker]
def resizeWithResolution(self, map, resolution): marker_container = Marker() marker_container.id = 2 marker_container.type = Marker.CUBE_LIST marker_container.points = [] marker_container.colors = [] marker_container.header.frame_id = "map" marker_container.ns = "wall" marker_container.scale.x = (0.5 / float(10)) * resolution marker_container.scale.y = (0.5 / float(10)) * resolution marker_container.header.stamp = rospy.Time.now() marker_container.pose.orientation.w = 1 resizedMapArray = [[0 for x in range(self.map_height / resolution)] for x in range(self.map_width / resolution)] i = 0 j = 0 while i < len(map[0]): j = 0 while j < len(map): if (i == 0): new_i = 0 else: new_i = i / resolution if (j == 0): new_j = 0 else: new_j = j / resolution # if(j>=0 and j<self.map_width/resolution and i>=0 and i<self.map_height/resolution): if self.isObstacle(map, i, j, resolution): resizedMapArray[new_i][new_j] = self.MAP_OBSTACLE_VALUE current_point = Point() current_color = ColorRGBA() current_color.a = 0.5 current_color.r = 0 current_color.g = 1 current_color.b = 1 current_point.z = 0.20 / float(10) current_point.x = ((new_j / float(2)) / (float(10) / resolution)) + 0.2 current_point.y = ((new_i / float(2)) / (float(10) / resolution)) + 0.2 marker_container.points.append(current_point) marker_container.points.append(current_color) else: # print 'i:'+str(i)+"--> obstacle" # print 'j:'+str(j) resizedMapArray[new_i][new_j] = 0 j = j + resolution i = i + resolution self.pub_marker.publish(marker_container) return resizedMapArray
def test(): rospy.init_node('intersect_plane_test') marker_pub = rospy.Publisher('table_marker', Marker) pose_pub = rospy.Publisher('pose', PoseStamped) int_pub = rospy.Publisher('intersected_points', PointCloud2) tfl = tf.TransformListener() # Test table table = Table() table.pose.header.frame_id = 'base_link' table.pose.pose.orientation.x, table.pose.pose.orientation.y, table.pose.pose.orientation.z, table.pose.pose.orientation.w = (0,0,0,1) table.x_min = -0.5 table.x_max = 0.5 table.y_min = -0.5 table.y_max = 0.5 # A marker for that table marker = Marker() marker.header.frame_id = table.pose.header.frame_id marker.id = 1 marker.type = Marker.LINE_STRIP marker.action = 0 marker.pose = table.pose.pose marker.scale.x, marker.scale.y, marker.scale.z = (0.005,0.005,0.005) marker.color.r, marker.color.g, marker.color.b, marker.color.a = (0.0,1.0,1.0,1.0) marker.frame_locked = False marker.ns = 'table' marker.points = [ Point(table.x_min,table.y_min, table.pose.pose.position.z), Point(table.x_min,table.y_max, table.pose.pose.position.z), Point(table.x_max,table.y_max, table.pose.pose.position.z), Point(table.x_max,table.y_min, table.pose.pose.position.z), Point(table.x_min,table.y_min, table.pose.pose.position.z), ] marker.colors = [] marker.text = '' # marker.mesh_resource = '' marker.mesh_use_embedded_materials = False marker.header.stamp = rospy.Time.now() # A test pose pose = PoseStamped() pose.header = table.pose.header pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = (0,0,0.5) pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = quaternion_from_euler(0,-pi/5,pi/5) intersection = cast_ray(pose, table, tfl) cloud = xyz_array_to_pointcloud2(np.array([[intersection.point.x, intersection.point.y, intersection.point.z]])) cloud.header = pose.header while not rospy.is_shutdown(): marker_pub.publish(marker) pose_pub.publish(pose) int_pub.publish(cloud) rospy.loginfo('published') rospy.sleep(0.1)
def publish_marker(self): if self.points is not None or not self.locked: del self.actualPoints[:] for x in self.points: if not self.dict.has_key((x.x, x.y)) and x.c > 137: self.isEvent = True self.dict.update({(x.x, x.y): x.c}) if x.c <= 137 and self.dict.has_key((x.x, x.y)): self.dict.pop((x.x, x.y), None) if self.isEvent is True: event_time = int(round(time.time() * 1000)) #print "Event-Sensfloor: " + str(event_time) logger.debug(event_time) footpressEvent.publish( SensfloorEventMsg(x=x.x, y=x.y, c=x.c, ms=event_time)) self.isEvent = False for key, value in self.dict.iteritems(): self.actualPoints.append( CapacitancePoint(x=key[0], y=key[1], c=value)) marker = Marker() marker.header = self.header marker.ns = 'sensfloor' marker.type = Marker.POINTS marker.lifetime = RVIZ_MARKER_LIFETIME marker.pose.orientation.w = 1 marker.scale.x = RVIZ_MARKER_SCALE marker.scale.y = RVIZ_MARKER_SCALE marker.id = 0 marker.points = map(lambda x: Point(x=x.x, y=x.y), self.actualPoints) marker.colors = map( lambda x: ColorRGBA(r=max( 0.01, min( 1.0, float(x.c - NEUTRAL_SENSOR_VALUE) / SENSOR_COLOUR_RANGE )), g=0.0, b=0.0, a=max( 0.01, min( 1.0, float(x.c - NEUTRAL_SENSOR_VALUE) / SENSOR_COLOUR_RANGE))), self.actualPoints) markerPub.publish(marker) self.locked = True
def create_trajectory_marker(self, traj): # create an interactive marker for our server int_marker = InteractiveMarker() int_marker.header.frame_id = "/map" int_marker.name = traj.uuid int_marker.description = traj.uuid pose = Pose() pose.position.x = traj.pose[0]['position']['x'] pose.position.y = traj.pose[0]['position']['y'] int_marker.pose = pose line_marker = Marker() line_marker.type = Marker.LINE_STRIP line_marker.scale.x = 0.05 # random.seed(traj.uuid) # val = random.random() # line_marker.color.r = r_func(val) # line_marker.color.g = g_func(val) # line_marker.color.b = b_func(val) # line_marker.color.a = 1.0 line_marker.points = [] MOD = 1 for i, point in enumerate(traj.pose): if i % MOD == 0: x = point['position']['x'] y = point['position']['y'] p = Point() p.x = x - int_marker.pose.position.x p.y = y - int_marker.pose.position.y line_marker.points.append(p) line_marker.colors = [] for i, vel in enumerate(traj.vel): if i % MOD == 0: color = ColorRGBA() val = vel / traj.max_vel color.r = r_func(val) color.g = g_func(val) color.b = b_func(val) color.a = 1.0 line_marker.colors.append(color) # create a control which will move the box # this control does not contain any markers, # which will cause RViz to insert two arrows control = InteractiveMarkerControl() control.markers.append(line_marker) int_marker.controls.append(control) return int_marker
def plane(self, frame_id, p, n, r=1.0, key=None, draw_triad=False): """ plane: n . ( X - p ) = 0 This will be drawn as a disc of radius r about p. """ n = np.array(n) p = np.array(p) m = Marker() m.header.frame_id = frame_id m.ns = key or 'planes' m.id = 0 if key else len(self.planes) m.type = Marker.TRIANGLE_LIST m.action = Marker.MODIFY # Compute plane rotation nz = n / np.linalg.norm(n) nx = np.cross(nz, np.array([0,0,1])); nx = nx / np.linalg.norm(nx) ny = np.cross(nz, nx); ny = ny / np.linalg.norm(ny) R = kdl.Rotation( kdl.Vector(*nx), kdl.Vector(*ny), kdl.Vector(*nz)) # define the pose of the plane marker m.pose = Pose(Point(*(p)), Quaternion(*R.GetQuaternion())) m.scale = Vector3(1, 1, 1) # flatten the triangle list for triangle in self.mesh_t: m.points.extend([ Point(r*self.mesh_p[t][0],r*self.mesh_p[t][1],0) for t in triangle]) m.color = ColorRGBA(0.8,0,0.8,0.9) m.colors = [m.color] * len(m.points) if draw_triad: self.line(frame_id, p, nx, t=[0.0,r],key=key+'-x') self.line(frame_id, p, ny, t=[0.0,r],key=key+'-y') self.line(frame_id, p, nz, t=[0.0,r],key=key+'-z') self.point(frame_id, p, 0.005, key=key+'-center-point') key = key or element_key(m) with self.mod_lock: self.planes[key] = m return key
def makeViz(self, reachSets): pointSets = [[tuple(p.p) for p in rs.set] for rs in reachSets.sets] triangleSets = [tripy.earclip(ps) for ps in pointSets] header = Header() header.stamp = rospy.Time.now() header.frame_id = reachSets.header.frame_id origin = Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1)) lineMarkers = MarkerArray() lineMarkerArray = [] for ii in range(len(pointSets)): m = Marker() m.header = header m.id = self.outline_marker_id + ii m.action = 0 m.pose = origin m.type = 4 #LINE_STRIP m.color = self.colors[ii % len(self.colors)] m.points = [Point(p[0], p[1], 0) for p in pointSets[ii]] m.scale = Vector3(.05, 0, 0) lineMarkerArray.append(m) lineMarkers.markers = lineMarkerArray self.outlinePub.publish(lineMarkers) triPoints = [xy for tri in triangleSets for xy in tri] triMarker = Marker() triMarker.header = header triMarker.id = self.tri_marker_id triMarker.type = 11 #TRIANGLE_LIST triMarker.action = 0 triMarker.pose = origin triMarker.color = ColorRGBA(1, 1, 1, 1) triMarker.scale = Vector3(1, 1, 1) triMarker.points = [ Point(p[0], p[1], 0) for tri in triPoints for p in tri ] #expand color array to cover all verts for all tris in each set with same color triFrequency = [len(ps) for ps in pointSets] triColors = [ self.colors[ii % len(self.colors)] for ii in range(len(pointSets)) ] triMarker.colors = [ c for cidx in range(len(triColors)) for c in repeat(triColors[cidx], triFrequency[cidx]) ] self.trisPub.publish(triMarker)
def _create_marker_container(self): marker_container = Marker() marker_container.id = 1 marker_container.type = Marker.CUBE_LIST marker_container.points = [] marker_container.colors = [] marker_container.header.frame_id = "map" marker_container.header.stamp = rospy.Time.now() marker_container.scale.x = (0.5 / float(10)) * self.RESOLUTION marker_container.scale.y = (0.5 / float(10)) * self.RESOLUTION marker_container.pose.orientation.w = 1 return marker_container
def create_plan_marker(self, plan, plan_values): # create an interactive marker for our server int_marker = InteractiveMarker() int_marker.header.frame_id = "/map" int_marker.name = plan.ID int_marker.description = plan.ID pose = Pose() #pose.position.x = traj.pose[0]['position']['x'] #pose.position.y = traj.pose[0]['position']['y'] int_marker.pose = pose line_marker = Marker() line_marker.type = Marker.LINE_STRIP line_marker.scale.x = 0.1 # random.seed(float(plan.ID)) # val = random.random() # line_marker.color.r = r_func(val) # line_marker.color.g = g_func(val) # line_marker.color.b = b_func(val) # line_marker.color.a = 1.0 line_marker.points = [] for view in plan.views: x = view.get_ptu_pose().position.x y = view.get_ptu_pose().position.y z = 0.0 # float(plan.ID) / 10 p = Point() p.x = x - int_marker.pose.position.x p.y = y - int_marker.pose.position.y p.z = z - int_marker.pose.position.z line_marker.points.append(p) line_marker.colors = [] for i, view in enumerate(plan.views): color = ColorRGBA() val = float(i) / len(plan.views) color.r = r_func(val) color.g = g_func(val) color.b = b_func(val) color.a = 1.0 line_marker.colors.append(color) # create a control which will move the box # this control does not contain any markers, # which will cause RViz to insert two arrows control = InteractiveMarkerControl() control.markers.append(line_marker) int_marker.controls.append(control) return int_marker
def transformStampedArrayToLabeledLineStripMarker(cls, tsdata_lst, fmt="%Y-%m-%d %H:%M:%S", zoffset=0.05, label_downsample=1, discrete=False): "[[transformStamped, meta],...] -> LineStrip / String" res = [] # make line strip points = [] colors = [] t_first = tsdata_lst[0][0] prev_t = t_first.transform.translation for ts, _ in tsdata_lst: t = ts.transform.translation dist = distanceOfVector3(prev_t, t) * 0.65 rgb = colorsys.hsv_to_rgb(dist, 1.0, 1.0) points.append(Point(x=t.x, y=t.y, z=t.z)) colors.append(ColorRGBA(rgb[0],rgb[1],rgb[2],1.0)) prev_t = t h = Header() h.stamp = rospy.Time(0) #rospy.Time.now() h.frame_id = "eng2" #t_first.child_frame_id if discrete: m_type = Marker.POINTS else: m_type = Marker.LINE_STRIP m = Marker(type=m_type, action=Marker.ADD, header=h, id=cls.marker_id) cls.marker_id += 1 m.scale.x = 0.1 m.scale.y = 0.1 m.points = points m.colors = colors m.ns = "labeled_line" m.lifetime = rospy.Time(3000) res.append(m) for t, t_meta in tsdata_lst[::label_downsample]: text = Marker(type=Marker.TEXT_VIEW_FACING, action=Marker.ADD, header=h, id=cls.marker_id) cls.marker_id += 1 text.scale.z = 0.1 text.pose = T.poseFromTransform(t.transform) text.pose.position.z += zoffset text.color = ColorRGBA(0.0,0.0,1.0,1.0) text.text = t_meta["inserted_at"].strftime(fmt) text.ns = "labeled_line_text" text.lifetime = rospy.Time(3000) res.append(text) return res
def make_match_marker(key, match, time, lifetime): marker = Marker(ns="Match:" + key, id=match.id, type=3, text=key, pose=match.pose, lifetime=lifetime) marker.header.frame_id = "map" marker.header.stamp = time marker.scale.z = match.estimated_height * 0.5 marker.scale.x = 0.1 marker.scale.y = 0.1 marker.color.a = 1.0 marker.color.r = 0.25 marker.color.g = 0.75 marker.color.b = 0.25 marker.colors = [marker.color] return marker
def echoes_callback(echo): echo['data'] = echo['data'][np.bitwise_and( echo['data']['flags'], 0x01).astype(np.bool)] #keep valid echoes only indices, flags, distances, amplitudes = [ echo['data'][x] for x in ['indices', 'flags', 'distances', 'amplitudes'] ] stamp = rospy.Time.now() if pub_raw.get_num_connections() > 0: pub_raw.publish( ros_numpy.msgify(PointCloud2, echo['data'], stamp, frame_id)) if pub_cloud.get_num_connections() > 0: struct_cloud = np.empty(indices.size, dtype=[('x', np.float32), ('y', np.float32), ('z', np.float32), ('intensity', np.float32)]) points = clouds.to_point_cloud(indices, distances, directions, np.float32) struct_cloud['x'] = points[:, 0] struct_cloud['y'] = points[:, 1] struct_cloud['z'] = points[:, 2] struct_cloud['intensity'] = amplitudes pub_cloud.publish( ros_numpy.msgify(PointCloud2, struct_cloud, stamp, frame_id)) if pub_triangles.get_num_connections() > 0: points, quad_amplitudes, quad_indices = clouds.to_quad_cloud( indices, distances, amplitudes, quad_directions, specs.v, specs.h, np.float32) stl_points = points[quad_indices] stl_amplitudes = quad_amplitudes[quad_indices] marker = Marker() marker.header.stamp = stamp marker.header.frame_id = frame_id marker.type = Marker.TRIANGLE_LIST marker.scale = Vector3(1, 1, 1) marker.points = [Point(x[0], x[1], x[2]) for x in stl_points] #TODO: optimize me marker.colors = [ColorRGBA(a, a, a, 1) for a in stl_amplitudes] #TODO: optimize me pub_triangles.publish(marker)
def cls_viz(self, cls): col = ColorRGBA(0.0, 0.0, 1.0, 1.0) msg = Marker() msg.lifetime = rospy.Duration(1.0) msg.header.frame_id = 'base_link' msg.type = msg.POINTS msg.action = msg.ADD msg.points = [Point(x=x, y=y) for (x, y) in cls] msg.colors = [col for _ in msg.points] msg.scale.x = msg.scale.y = msg.scale.z = 0.1 self.cls_viz_pub_.publish(msg)
def calculate_error(self, opt_all_vec): opt_param_vec, full_pose_arr = self.split_all(opt_all_vec) full_param_vec = self.calculate_full_param_vec(opt_param_vec) # Update the primitives with the new set of parameters self._robot_params.inflate(full_param_vec) # Update all the blocks' configs for multisensor in self._multisensors: multisensor.update_config(self._robot_params) r_list = [] for multisensor, cb_pose_vec in zip(self._multisensors, list(full_pose_arr)): # Process cb pose cb_points = SingleTransform(cb_pose_vec).transform * self._robot_params.checkerboards[multisensor.checkerboard].generate_points() if (self._use_cov): r_list.append(multisensor.compute_residual_scaled(cb_points)) else: r_list.append(multisensor.compute_residual(cb_points)) cb_points_msgs = [ geometry_msgs.msg.Point(cur_pt[0,0], cur_pt[0,1], cur_pt[0,2]) for cur_pt in cb_points.T] cb_colors_msgs = [ ColorRGBA(1,0,1,1) for cur_pt in cb_points.T] cb_colors_msgs[0] = ColorRGBA(0,1,0,1) cb_colors_msgs[1] = ColorRGBA(0,1,0,1) m = Marker() m.header.frame_id = self._robot_params.base_link m.pose.orientation.w = 1; m.ns = "points_3d" m.id = id m.type = Marker.SPHERE_LIST m.action = Marker.MODIFY m.points = cb_points_msgs m.colors = cb_colors_msgs m.scale.x = 0.02 m.scale.y = 0.02 m.scale.z = 0.02 self.marker_pub.publish(m) id += 1 r_vec = concatenate(r_list) rms_error = numpy.sqrt( numpy.mean(r_vec**2) ) print "\t\t\t\t\tRMS error: %.3f \r" % rms_error, sys.stdout.flush() return array(r_vec)
def init_marker(self, centres): marker = Marker() marker.header = self.header marker.ns = 'sensfloor_cluster' marker.type = Marker.POINTS marker.lifetime = RVIZ_MARKER_LIFETIME marker.pose.orientation.w = 1 marker.scale.x = RVIZ_MARKER_SCALE marker.scale.y = RVIZ_MARKER_SCALE marker.id = 0 marker.points = map(lambda x: Point(x=x[0], y=x[1] - 1), centres) marker.colors = map(lambda x: ColorRGBA(r=0, g=100, b=0, a=1), centres) marker.text = str(centres) return marker
def lineMarker(self, p, d): marker = Marker() header = Header() header.stamp = rospy.Time.now() header.frame_id = self.base_frame marker.header = header marker.ns = "rays" marker.id = self.recent_next marker.type = Marker.LINE_LIST marker.action = Marker.ADD marker.scale = Vector3(*[0.02 for i in range(3)]) marker.points = toPoints(np.column_stack([p, (p+2*d/np.linalg.norm(d))])) color = self.recent_color marker.color = color marker.colors = [color for i in range(len(marker.points))] return marker
def publish(self, event): """GVG를 출력한다""" gvg_node = Marker() # GVG 노드 마커를 생성한다. gvg_node.header.stamp = rospy.Time.now() gvg_node.header.frame_id = 'map' gvg_node.id = 0 gvg_node.type = Marker.POINTS gvg_node.action = Marker.ADD gvg_node.scale.x = 0.5 * self.map.info.resolution gvg_node.scale.y = 0.5 * self.map.info.resolution gvg_node.points = [] gvg_node.colors = [] for n in self.graph.nodes: c = ColorRGBA() c.a = 0.5 c.r = 1 gvg_node.colors.append(c) p = Point() p.x = self.graph.nodes[n]['pos'][0] p.y = self.graph.nodes[n]['pos'][1] p.z = 0.01 gvg_node.points.append(p) gvg_edge = Marker() # GVG 엣지 마커를 생성한다. gvg_edge.header.stamp = rospy.Time.now() gvg_edge.header.frame_id = 'map' gvg_edge.id = 1 gvg_edge.type = Marker.LINE_LIST gvg_edge.action = Marker.ADD gvg_edge.scale.x = 0.2 * self.map.info.resolution gvg_edge.points = [] gvg_edge.color.a = 0.7 for e in self.graph.edges: p1 = Point() p1.x = self.graph.nodes[e[0]]['pos'][0] p1.y = self.graph.nodes[e[0]]['pos'][1] p1.z = 0.01 gvg_edge.points.append(p1) p2 = Point() p2.x = self.graph.nodes[e[1]]['pos'][0] p2.y = self.graph.nodes[e[1]]['pos'][1] p2.z = 0.01 gvg_edge.points.append(p2) self.publisher.publish([gvg_node, gvg_edge]) # GVG 마커를 출력한다.
def on_lead_obstacle(self, msg): marker = Marker() marker.header.frame_id = "middle_radar_link" if rospy_compat.use_ros_1: marker.header.stamp = rospy_compat.rospy.Time.now() marker.ns = "lead_obstacle" marker.id = 1 marker.type = Marker.POINTS marker.action = Marker.MODIFY marker.scale.x = 0.5 marker.scale.y = 0.5 marker.scale.z = 0.1 marker.color.r = 1.0 marker.color.a = 1.0 msg.point.y *= -1 marker.points = [msg.point] marker.colors = [ColorRGBA(0.,0.,1.0,1.0)] self.radar_rviz_pub.publish(marker)
def toMarker(self, points, observation_type): marker = Marker() header = Header() header.stamp = rospy.Time.now() header.frame_id = self.base_frame marker.header = header marker.ns = "object_tracker" marker.id = observation_type marker.type = Marker.SPHERE_LIST marker.action = Marker.ADD marker.scale = Vector3(*[self.sigma_observation for i in range(3)]) marker.points = toPoints(points) color = None if observation_type == ObjectTracker.RECENT: color = self.recent_color elif observation_type == ObjectTracker.ALL: color = self.all_color else: raise ValueError marker.color = color marker.colors = [color for i in range(len(marker.points))] return marker
def point(self, frame_id, p, s, key=None): """ point: p size: s""" m = Marker() m.header.frame_id = frame_id m.ns = key or 'points' m.id = 0 if key else len(self.points) m.type = Marker.POINTS m.action = Marker.MODIFY m.pose = Pose(Point(0,0,0), Quaternion(0,0,0,1)) m.scale = Vector3(s,s,1.0) m.points = [Point(*p)] m.colors = [ColorRGBA(0.8,0.8,0,1)] key = key or element_key(m) with self.mod_lock: self.points[key] = m return key
def marker_maker(): pub = rospy.Publisher("markers",Marker,queue_size=10) rospy.init_node("marker_maker") r = rospy.Rate(1) q = 0 i = 1 j = 0 while not rospy.is_shutdown(): msg = Marker() msg.ns = "markers" msg.type=Marker.SPHERE msg.id=q msg.action = 0 msg.color.r = .75 msg.color.g = 0 msg.color.b = .5 msg.color.a = 1 msg.colors = [ColorRGBA(1,1,1,1),ColorRGBA(0,0,0,0)] msg.pose.position.x = j msg.pose.position.y = j msg.pose.position.z = j msg.pose.orientation.x = 0 msg.pose.orientation.y = 0 msg.pose.orientation.z = 0 msg.pose.orientation.w = 1 msg.scale.x = 1 msg.scale.y = 1 msg.scale.z = 1 msg.lifetime = rospy.Duration() msg.header.frame_id = "my_frame" msg.header.stamp = rospy.Time.now() pub.publish(msg) q += 1 i /= 1.1 j += 1 r.sleep()
def callback(data): global imagePub global markerPub #Convert image to CV image bridge=CvBridge() cv_image = bridge.imgmsg_to_cv(data, "mono8") # Convert the image to a Numpy array since most cv2 functionsrequire Numpy arrays. searched = np.array(cv_image, dtype=np.uint8) #Create a copy for sobel comparison searchedCopy=copy.copy(searched) searchedCopy[searchedCopy==255]=0 #Take Sobel Derivatives sobel_x=np.uint8(np.absolute(cv2.Sobel(searched,cv2.CV_16S,1,0,ksize=1))) sobel_y=np.uint8(np.absolute(cv2.Sobel(searched,cv2.CV_16S,0,1,ksize=1))) sobel_xy=cv2.addWeighted(sobel_x,0.5,sobel_y,0.5,0) sobel_x_base=np.uint8(np.absolute(cv2.Sobel(searchedCopy,cv2.CV_16S,1,0,ksize=1))) sobel_y_base=np.uint8(np.absolute(cv2.Sobel(searchedCopy,cv2.CV_16S,0,1,ksize=1))) sobel_xy_base=cv2.addWeighted(sobel_x_base,0.5,sobel_y_base,0.5,0) ret,sobel_xy_thres = cv2.threshold(sobel_xy,0,255,cv2.THRESH_BINARY) ret,sobel_xy_base_thres = cv2.threshold(sobel_xy_base,0,255,cv2.THRESH_BINARY) #Subtract Comparisons for frontiers only sobelCombined=sobel_xy_base_thres-sobel_xy_thres ret,sobelCombined_thresh = cv2.threshold(sobelCombined,0,255,cv2.THRESH_BINARY) #Dialate Combined dialate=cv2.dilate(sobelCombined_thresh,np.ones((3,3),'uint8')) #Find Contour Centorids dialateCopy=copy.copy(dialate) contours,hier=cv2.findContours(dialateCopy,cv2.RETR_LIST,cv2.CHAIN_APPROX_SIMPLE) centroids = [] colors = [] for i in contours: if len(i) > 10: moments=cv2.moments(i) cx = int(moments['m10']/moments['m00']) cy = int(moments['m01']/moments['m00']) centroidPoint = Point() centroidColor = ColorRGBA() centroidPoint.x = cx/20.0-20 centroidPoint.y = cy/20.0-32.8 centroidPoint.z = 0 #centroidColor = (0,0,1,1) centroidColor.r = 0 centroidColor.g = 0 centroidColor.b = 1 centroidColor.a = 1 centroids.append(centroidPoint) colors.append(centroidColor) #code.interact(local=locals()) #Display Image in PLT Window '''plt.subplot(1,5,1),plt.imshow(searched,cmap='gray'),plt.title('Searched Space'),plt.xticks([]), plt.yticks([]) plt.subplot(1,5,2),plt.imshow(sobel_xy_thres,cmap='gray'),plt.title('Sobel All'),plt.xticks([]), plt.yticks([]) plt.subplot(1,5,3),plt.imshow(sobel_xy_base_thres,cmap='gray'),plt.title('Sobel All'),plt.xticks([]), plt.yticks([]) plt.subplot(1,5,4),plt.imshow(sobelCombined,cmap='gray'),plt.title('Frontiers Map'),plt.xticks([]), plt.yticks([]) plt.subplot(1,5,5),plt.imshow(dialate,cmap='gray'),plt.title('Dialate'),plt.xticks([]), plt.yticks([]) plt.draw() plt.pause(0.001)''' #Convert the image back to mat format and publish as ROS image frontiers = cv.fromarray(dialate) imagePub.publish(bridge.cv_to_imgmsg(frontiers, "mono8")) #Publish centorids of frontiers as marker markerMsg = Marker() markerMsg.header.frame_id = "/map" markerMsg.header.stamp = rospy.Time.now() markerMsg.ns = "" markerMsg.id = 0 markerMsg.type = 7 #Sphere List markerMsg.action = 0 #Add markerMsg.scale.x = 0.5 markerMsg.scale.y = 0.5 markerMsg.scale.z = 0.5 markerMsg.points = centroids markerMsg.colors = colors markerPub.publish(markerMsg) print 'yes'
def imageCallback(self,data): #Convert image to CV2 supported numpy array bridge=CvBridge() cv_image = bridge.imgmsg_to_cv(data, "mono8") searched = np.array(cv_image, dtype=np.uint8) #Create copy and clear searched space in copy (leaving map only) searchedCopy=copy.copy(searched) searchedCopy[searchedCopy==255]=0 #Take Sobel Derivatives of searched and map only sobel_x=np.uint8(np.absolute(cv2.Sobel(searched,cv2.CV_16S,1,0,ksize=1))) sobel_y=np.uint8(np.absolute(cv2.Sobel(searched,cv2.CV_16S,0,1,ksize=1))) sobel_xy=cv2.addWeighted(sobel_x,0.5,sobel_y,0.5,0) sobel_x_base=np.uint8(np.absolute(cv2.Sobel(searchedCopy,cv2.CV_16S,1,0,ksize=1))) sobel_y_base=np.uint8(np.absolute(cv2.Sobel(searchedCopy,cv2.CV_16S,0,1,ksize=1))) sobel_xy_base=cv2.addWeighted(sobel_x_base,0.5,sobel_y_base,0.5,0) ret,sobel_xy_thres = cv2.threshold(sobel_xy,0,255,cv2.THRESH_BINARY) ret,sobel_xy_base_thres = cv2.threshold(sobel_xy_base,0,255,cv2.THRESH_BINARY) #Subtract Sobel Derivatives (Leaves Frontiers Only) sobelCombined=sobel_xy_base_thres-sobel_xy_thres ret,sobelCombined_thresh = cv2.threshold(sobelCombined,0,255,cv2.THRESH_BINARY) #Dialate Frontiers To Form Continuous Contours dialate=cv2.dilate(sobelCombined_thresh,np.ones((3,3),'uint8')) #Find Contours (make copy since dialate destorys original image) dialateCopy=copy.copy(dialate) contours,hier=cv2.findContours(dialateCopy,cv2.RETR_LIST,cv2.CHAIN_APPROX_NONE) #Convert the image back to mat format for publishing as ROS image frontiers = cv.fromarray(dialate) #Create List Data for Marker Message centroids = [] colors = [] #Filter Frontier Contour by number of pixels for i in contours: if len(i) > 50: moments=cv2.moments(i) cx = int(moments['m10']/moments['m00']) cy = int(moments['m01']/moments['m00']) centroidPoint = Point() centroidColor = ColorRGBA() centroidPoint.x = cx*self.mapRes+self.mapOrigin.position.x centroidPoint.y = cy*self.mapRes+self.mapOrigin.position.y centroidPoint.z = 0; centroidColor.r = 0 centroidColor.g = 0 centroidColor.b = 1 centroidColor.a = 1 centroids.append(centroidPoint) colors.append(centroidColor) #Pack Marker Message markerMsg = Marker() markerMsg.header.frame_id = "/map" markerMsg.header.stamp = rospy.Time.now() markerMsg.ns = "" markerMsg.id = 0 markerMsg.type = 7 #Sphere List Type markerMsg.action = 0 #Add Mode markerMsg.scale.x = 0.5 markerMsg.scale.y = 0.5 markerMsg.scale.z = 0.5 markerMsg.points = centroids markerMsg.colors = colors #Publish Marker and Image messages self.imagePub.publish(bridge.cv_to_imgmsg(frontiers, "mono8")) self.markerPub.publish(markerMsg)