def add_marker(self, array, track, color, tid): m1 = Marker() m1.header.frame_id = "camera_rgb_optical_frame" m1.ns = "line" m1.id = tid m1.type = 4 #lines m1.action = 0 m1.scale.x = .002 m1.color.a = 1. m1.color.r = color[0]/255. m1.color.g = color[1]/255. m1.color.b = color[2]/255. m1.points = track array.append(m1) m2 = Marker() m2.header.frame_id = "camera_rgb_optical_frame" m2.ns = "point" m2.id = tid m2.type = 8 #points m2.action = 0 m2.scale.x = .008 m2.scale.y = .008 m2.color.a = 1. m2.color.r = color[0]/255. m2.color.g = color[1]/255. m2.color.b = color[2]/255. m2.points = [ track[-1] ] array.append(m2)
def publish_debug_info(self, footprint_point, distance, scan_point): array = MarkerArray() fp = Marker() fp.id = 1 fp.type = fp.SPHERE scale = 0.1 fp.pose.position.x,fp.pose.position.y, fp.pose.position.z = footprint_point[0], footprint_point[1], 0 fp.scale.x, fp.scale.y, fp.scale.z = scale, scale, scale fp.color.r, fp.color.g, fp.color.b, fp.color.a = (0, 0, 1, 1) fp.header.frame_id = "{}/base_link".format(self.robot.robot_name) fp.frame_locked = True fp.action = fp.ADD fp.ns = "door_opening" array.markers += [fp] sp = Marker() sp.id = 2 sp.type = sp.SPHERE scale = 0.1 sp.pose.position.x,sp.pose.position.y, sp.pose.position.z = scan_point[0], scan_point[1], 0 sp.scale.x, sp.scale.y, sp.scale.z = scale, scale, scale sp.color.r, sp.color.g, sp.color.b, sp.color.a = (1, 0, 0, 1) sp.header.frame_id = "{}/base_link".format(self.robot.robot_name) sp.frame_locked = False sp.action = sp.ADD sp.ns = "door_opening" array.markers += [sp] self.debug_vizualizer.publish(array)
def publishWaypointMarker(self, wp, delete = False): marker = Marker() marker.header.frame_id = "world" marker.header.stamp = rospy.Time.now() marker.ns = "waypoints" marker.id = self.wp_ids[wp] marker.lifetime = rospy.Duration(0) if delete: marker.action = marker.DELETE else: marker.type = marker.SPHERE marker.action = marker.ADD marker.pose.position.x = wp.x marker.pose.position.y = wp.y marker.pose.orientation.w = 1.0 marker.scale.y = 0.02 marker.scale.x = 0.02 marker.scale.z = 0.01 marker.color.a = 1.0 if wp.action == Waypoint.VIEW: marker.color.g = 1.0 else: marker.color.r = 1.0 # print marker self.vizPub.publish(marker)
def plot_3d_vel(self, p_arr, v_arr, v_scale=1.0): marker_array = MarkerArray() for i in xrange(len(p_arr)): p = p_arr[i] v = vx,vy,vz = v_arr[i] marker = Marker() marker.header.frame_id = "/openni_rgb_optical_frame" marker.type = marker.ARROW marker.action = marker.ADD marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 marker.points.append(Point(p[0],p[1],p[2])) marker.points.append(Point(p[0]+vx*v_scale,p[1]+vy*v_scale,p[2]+vz*v_scale)) marker.scale.x=0.05 marker.scale.y=0.1 marker.id = i marker_array.markers.append(marker) for i in xrange(len(p_arr),self.max_features): marker = Marker() marker.action = marker.DELETE marker.id = i marker_array.markers.append(marker) self.marker_pub.publish(marker_array)
def publishPositions(self): if not self.initialized: return nr = 0 markerArray = MarkerArray() marker = Marker() marker.header.frame_id = "/map" marker.type = marker.MESH_RESOURCE marker.action = marker.ADD marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 marker.mesh_use_embedded_materials = True marker.mesh_resource = "package://pacman_controller/meshes/pacman.dae" marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.pose.orientation = self.pacman["orientation"] marker.pose.position.x = self.pacman["x"] marker.pose.position.y = self.pacman["y"] marker.pose.position.z = 0.0 marker.id = nr markerArray.markers.append(marker) for ghost in self.ghosts: curGhost = self.ghosts[ghost] if not curGhost["initialized"]: continue nr += 1 marker = Marker() marker.header.frame_id = "/map" marker.type = marker.MESH_RESOURCE marker.action = marker.ADD marker.scale.x = 0.3 marker.scale.y = 0.3 marker.scale.z = 0.3 marker.mesh_use_embedded_materials = True if curGhost["eaten"]: marker.mesh_resource = "package://pacman_controller/meshes/dead.dae" elif self.state == State.FLEEING: marker.mesh_resource = "package://pacman_controller/meshes/ghost_catchable.dae" else: marker.mesh_resource = "package://pacman_controller/meshes/%s.dae" % ghost marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.pose.orientation = curGhost["orientation"] marker.pose.position.x = curGhost["x"] marker.pose.position.y = curGhost["y"] marker.pose.position.z = 0.0 marker.id = nr markerArray.markers.append(marker) self.pubPositions.publish(markerArray) return
def publishMap(self): markerArray = MarkerArray() marker = Marker() marker.header.frame_id = "/map" marker.type = marker.SPHERE_LIST marker.action = marker.ADD marker.scale.x = 0.05 marker.scale.y = 0.05 marker.scale.z = 0.05 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.pose.orientation.w = 1.0 marker.pose.position.x = 0.0 marker.pose.position.y = 0.0 marker.pose.position.z = 0.0 marker.id = 0 for p in self.mapPoints: if p["eaten"]: continue if p["powerup"]: continue point = Point() point.x = p["x"] point.y = p["y"] point.z = 0.15 marker.points.append(point) markerArray.markers.append(marker) marker = Marker() marker.header.frame_id = "/map" marker.type = marker.SPHERE_LIST marker.action = marker.ADD marker.scale.x = 0.3 marker.scale.y = 0.3 marker.scale.z = 0.3 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.pose.orientation.w = 1.0 marker.pose.position.x = 0.0 marker.pose.position.y = 0.0 marker.pose.position.z = 0.0 marker.id = 1 for p in self.mapPoints: if p["eaten"]: continue if not p["powerup"]: continue point = Point() point.x = p["x"] point.y = p["y"] point.z = 0.2 marker.points.append(point) markerArray.markers.append(marker) self.pubMap.publish(markerArray)
def publish_cluster(publisher, points, frame_id, namespace, cluster_id): """Publishes a marker representing a cluster. The x and y arguments specify the center of the target. Args: publisher: A visualization_msgs/Marker publisher points: A list of geometry_msgs/Point frame_id: The coordinate frame in which to interpret the points. namespace: string, a unique name for a group of clusters. cluster_id: int, a unique number for this cluster in the given namespace. """ marker = Marker() # TODO(jstn): Once the point clouds have the correct frame_id, # use them here. marker.header.frame_id = frame_id marker.header.stamp = rospy.Time().now() marker.ns = namespace marker.id = 2 * cluster_id marker.type = Marker.POINTS marker.action = Marker.ADD marker.color.r = random.random() marker.color.g = random.random() marker.color.b = random.random() marker.color.a = 0.5 marker.points = points marker.scale.x = 0.002 marker.scale.y = 0.002 marker.lifetime = rospy.Duration() center = [0, 0, 0] for point in points: center[0] += point.x center[1] += point.y center[2] += point.z center[0] /= len(points) center[1] /= len(points) center[2] /= len(points) text_marker = Marker() text_marker.header.frame_id = frame_id text_marker.header.stamp = rospy.Time().now() text_marker.ns = namespace text_marker.id = 2 * cluster_id + 1 text_marker.type = Marker.TEXT_VIEW_FACING text_marker.action = Marker.ADD text_marker.pose.position.x = center[0] - 0.1 text_marker.pose.position.y = center[1] text_marker.pose.position.z = center[2] text_marker.color.r = 1 text_marker.color.g = 1 text_marker.color.b = 1 text_marker.color.a = 1 text_marker.scale.z = 0.05 text_marker.text = '{}'.format(cluster_id) text_marker.lifetime = rospy.Duration() _publish(publisher, marker, "cluster") _publish(publisher, text_marker, "text_marker") return marker
def visualize_cluster(self, cluster, label=None): points = pc2.read_points(cluster.pointcloud, skip_nans=True) point_list = [Point(x=x, y=y-0.3, z=z) for x, y, z, rgb in points] if len(point_list) == 0: rospy.logwarn('Point list was of size 0, skipping.') return marker_id = len(self._current_markers) marker = Marker() marker.header.frame_id = 'map' marker.header.stamp = rospy.Time().now() marker.ns = 'clusters' marker.id = marker_id marker.type = Marker.POINTS marker.action = Marker.ADD marker.color.r = random.random() marker.color.g = random.random() marker.color.b = random.random() marker.color.a = 0.5 + random.random() marker.points = point_list marker.scale.x = 0.002 marker.scale.y = 0.002 marker.lifetime = rospy.Duration() self.visualize_marker(marker) if label is not None: center = [0, 0, 0] for point in point_list: center[0] += point.x center[1] += point.y center[2] += point.z center[0] /= len(point_list) center[1] /= len(point_list) center[2] /= len(point_list) text_marker = Marker() text_marker.header.frame_id = 'map' text_marker.header.stamp = rospy.Time().now() text_marker.ns = 'labels' text_marker.id = marker_id + 1 text_marker.type = Marker.TEXT_VIEW_FACING text_marker.action = Marker.ADD text_marker.pose.position.x = center[1] - 0.05 text_marker.pose.position.y = center[1] text_marker.pose.position.z = center[2] text_marker.color.r = 1 text_marker.color.g = 1 text_marker.color.b = 1 text_marker.color.a = 1 text_marker.scale.z = 0.05 text_marker.text = label text_marker.lifetime = rospy.Duration() self.visualize_marker(text_marker)
def publish(self, target_frame, timestamp): ma = MarkerArray() for id in self.marker_list: marker = Marker() marker.header.stamp = timestamp marker.header.frame_id = target_frame marker.ns = "landmark_kf" marker.id = id marker.type = Marker.CYLINDER marker.action = Marker.ADD Lkf = self.marker_list[id] marker.pose.position.x = Lkf.L[0,0] marker.pose.position.y = Lkf.L[1,0] marker.pose.position.z = 0 marker.pose.orientation.x = 0 marker.pose.orientation.y = 0 marker.pose.orientation.z = 1 marker.pose.orientation.w = 0 marker.scale.x = max(3*sqrt(Lkf.P[0,0]),0.05) marker.scale.y = max(3*sqrt(Lkf.P[1,1]),0.05) marker.scale.z = 0.5; marker.color.a = 1.0; marker.color.r = 1.0; marker.color.g = 1.0; marker.color.b = 0.0; marker.lifetime.secs=3.0; ma.markers.append(marker) marker = Marker() marker.header.stamp = timestamp marker.header.frame_id = target_frame marker.ns = "landmark_kf" marker.id = 1000+id marker.type = Marker.TEXT_VIEW_FACING marker.action = Marker.ADD Lkf = self.marker_list[id] marker.pose.position.x = Lkf.L[0,0] marker.pose.position.y = Lkf.L[1,0] marker.pose.position.z = 1.0 marker.pose.orientation.x = 0 marker.pose.orientation.y = 0 marker.pose.orientation.z = 1 marker.pose.orientation.w = 0 marker.text = str(id) marker.scale.x = 1.0 marker.scale.y = 1.0 marker.scale.z = 0.2 marker.color.a = 1.0; marker.color.r = 1.0; marker.color.g = 1.0; marker.color.b = 1.0; marker.lifetime.secs=3.0; ma.markers.append(marker) self.marker_pub.publish(ma)
def pubViz(self, ast, bst): rate = rospy.Rate(10) for i in range(self.winSize): msgs = MarkerArray() #use1について msg = Marker() #markerのプロパティ msg.header.frame_id = 'camera_link' msg.header.stamp = rospy.Time.now() msg.ns = 'j1' msg.action = 0 msg.id = 1 msg.type = 8 msg.scale.x = 0.1 msg.scale.y = 0.1 msg.color = self.carray[2] #ジョイントポイントを入れる処理 for j1 in range(self.jointSize): point = Point() point.x = self.pdata[0][ast+i][j1][0] point.y = self.pdata[0][ast+i][j1][1] point.z = self.pdata[0][ast+i][j1][2] msg.points.append(point) msg.pose.orientation.w = 1.0 msgs.markers.append(msg) msg = Marker() msg.header.frame_id = 'camera_link' msg.header.stamp = rospy.Time.now() msg.ns = 'j2' msg.action = 0 msg.id = 2 msg.type = 8 msg.scale.x = 0.1 msg.scale.y = 0.1 msg.color = self.carray[1] for j2 in range(self.jointSize): point = Point() point.x = self.pdata[1][bst+i][j2][0] point.y = self.pdata[1][bst+i][j2][1] point.z = self.pdata[1][bst+i][j2][2] msg.points.append(point) msg.pose.orientation.w = 1.0 msgs.markers.append(msg) self.mpub.publish(msgs) rate.sleep()
def publishState(self): self.pubState.publish(str(self.state)) marker = Marker() marker.header.frame_id = "/map" marker.header.stamp = rospy.get_rostime() marker.type = marker.TEXT_VIEW_FACING marker.action = marker.ADD marker.scale.x = 1.0 marker.scale.y = 0.2 marker.scale.z = 0.5 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 marker.pose.orientation.w = 1.0 marker.pose.position.x = self.infoLoc["x"] marker.pose.position.y = self.infoLoc["y"] marker.pose.position.z = 0.0 marker.id = 0 if self.state in [State.RUNNING]: if not self.sendStart: marker.text = "Start" self.sendStart = True self.timeSend = rospy.get_rostime() elif (rospy.get_rostime() - self.timeSend).to_sec() > 0.2: # marker.type = marker.SPHERE marker.action = marker.DELETE else: marker.text = "Start" if self.state == State.FLEEING: timeFleeing = TIME_FLEEING - (rospy.get_rostime() - self.timeStartFleeing).to_sec() if timeFleeing > 0: marker.text = str(timeFleeing) else: marker.action = marker.DELETE self.state = State.RUNNING if self.state in [State.GAME_OVER, State.STOPPED, State.SETUP]: marker.text = "GAME OVER!" if self.state in [State.INIT]: marker.text = "Initializing Game" if self.state in [State.PAUSED]: marker.text = "Game Paused" if self.state in [State.WON]: marker.text = "Congratulations, you have won the Game! Now get back to you real job!" self.pubInfo.publish(marker)
def pubRviz(self, pos, joints): msgs = MarkerArray() for p in range(len(pos)): msg = Marker() msg.header.frame_id = 'camera_link' msg.header.stamp = rospy.Time.now() msg.ns = 'marker' msg.action = 0 msg.id = p msg.type = 4 msg.scale.x = 0.1 msg.scale.y = 0.1 msg.color = self.carray[2] for i in range(len(pos[p])): point = Point() point.x = pos[p][i][0] point.y = pos[p][i][1] point.z = pos[p][i][2] msg.points.append(point) msg.pose.orientation.w = 1.0 msgs.markers.append(msg) for j in range(len(joints)): msg = Marker() msg.header.frame_id = 'camera_link' msg.header.stamp = rospy.Time.now() msg.ns = 'joints' msg.action = 0 msg.id = j msg.type = 8 msg.scale.x = 0.1 msg.scale.y = 0.1 msg.color = self.carray[j] #print "joints len:"+str(len(joints[j])) for i in range(len(joints[j])): point = Point() point.x = joints[j][i][0] point.y = joints[j][i][1] point.z = joints[j][i][2] msg.points.append(point) msg.pose.orientation.w = 1.0 msgs.markers.append(msg) self.mpub.publish(msgs)
def publish(anns, data): wall_list = WallList() marker_list = MarkerArray() marker_id = 1 for a, d in zip(anns, data): # Walls object = deserialize_msg(d.data, Wall) wall_list.obstacles.append(object) # Markers marker = Marker() marker.id = marker_id marker.header = a.pose.header marker.type = a.shape marker.ns = "wall_obstacles" marker.action = Marker.ADD marker.lifetime = rospy.Duration.from_sec(0) marker.pose = copy.deepcopy(a.pose.pose.pose) marker.scale = a.size marker.color = a.color marker_list.markers.append(marker) marker_id = marker_id + 1 marker_pub = rospy.Publisher('wall_marker', MarkerArray, latch=True, queue_size=1) wall_pub = rospy.Publisher('wall_pose_list', WallList, latch=True, queue_size=1) wall_pub.publish(wall_list) marker_pub.publish(marker_list) return
def addPolygonFilled(self, points): oid = self.oid+1 name = "/polygonFilled"+str(self.oid) marker = Marker() marker.id = self.oid marker.ns = "/polygonFilled" marker.header.frame_id = name marker.type = marker.TRIANGLE_LIST marker.action = marker.ADD marker.scale.x = 1 marker.scale.y = 1 marker.scale.z = 1 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 1.0 marker.color.a = 1.0 marker.pose.orientation.w = 1.0 marker.pose.position.x = 0 marker.pose.position.y = 0 marker.pose.position.z = 0 marker.points = [] for i in range(0,len(points)-2,1): pt = Point(points[0][0], points[0][1], points[0][2]) marker.points.append(pt) pt = Point(points[i+1][0], points[i+1][1], points[i+1][2]) marker.points.append(pt) pt = Point(points[i+2][0], points[i+2][1], points[i+2][2]) marker.points.append(pt) self.markerArray.markers.append(marker)
def input_handler(space): global marker_color global marker_pub global matches for str in matches: if space.aux_payload.find(str) == -1: # print "FAIL: str='%s' aux_payload='%s'" % (str, space.aux_payload) return # print space # print marker = Marker() marker.header.frame_id = "/map" marker.header.stamp = rospy.Time() marker.ns = "" marker.id = 0 marker.type = Marker.POINTS marker.action = Marker.ADD marker.pose.position.x = 0 marker.pose.position.y = 0 marker.pose.position.z = 0 marker.pose.orientation.x = 0 marker.pose.orientation.y = 0 marker.pose.orientation.z = 0 marker.pose.orientation.w = 0 marker.scale.x = .1 marker.scale.y = .1 marker.scale.z = .1 marker.color = marker_color marker.points = [] # this line cuts off anything before the string "convex_set: " in the aux_payload convex_set = space.aux_payload[ space.aux_payload.find("convex_set: ") :] # this line cuts off that string at the next newline character convex_set = convex_set.split('\n')[0] # this line splits the string into pieces delineated by spaces convex_set = convex_set.split(' ') # print convex_set for candidate in convex_set: if len(candidate) == 0 or candidate[0] != '(': continue coords = candidate[1:-1].split(',') # print coords marker.points.append(Point(float(coords[0]), float(coords[1]), float(coords[2]))) # print marker.points[-1] # print # print marker.points # print marker_pub.publish(marker)
def __init__(self): self.layout = rospy.get_param('/thruster_layout/thrusters') # Add thruster layout from ROS param set by mapper assert self.layout is not None, 'Could not load thruster layout from ROS param' ''' Create MarkerArray with an arrow marker for each thruster at index node_id. The position of the marker is as specified in the layout, but the length of the arrow will be set at each thrust callback. ''' self.markers = MarkerArray() for i in xrange(len(self.layout)): # Initialize each marker (color, scale, namespace, frame) m = Marker() m.header.frame_id = '/base_link' m.type = Marker.ARROW m.ns = 'thrusters' m.color.a = 0.8 m.scale.x = 0.01 # Shaft diameter m.scale.y = 0.05 # Head diameter m.action = Marker.DELETE m.lifetime = rospy.Duration(0.1) self.markers.markers.append(m) for key, layout in self.layout.iteritems(): # Set position and id of marker from thruster layout idx = layout['node_id'] pt = numpy_to_point(layout['position']) self.markers.markers[idx].points.append(pt) self.markers.markers[idx].points.append(pt) self.markers.markers[idx].id = idx # Create publisher for marker and subscribe to thrust self.pub = rospy.Publisher('/thrusters/markers', MarkerArray, queue_size=5) self.thrust_sub = rospy.Subscriber('/thrusters/thrust', Thrust, self.thrust_cb, queue_size=5)
def new_marker(self, ns="/debug", frame="enu", time=None, type = Marker.CUBE , position=(0,0,0), orientation=(0,0,0,1), color=(1,0,0)): marker = Marker() marker.ns = ns if time != None: marker.header.stamp = time marker.header.frame_id = frame marker.type = type marker.action = marker.ADD marker.scale.x = 1.0 marker.scale.y = 1.0 marker.scale.z = 1.0 marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] marker.color.a = 1.0 marker.id = self.last_id marker.pose.orientation.x = orientation[0] marker.pose.orientation.y = orientation[1] marker.pose.orientation.z = orientation[2] marker.pose.orientation.w = orientation[3] marker.pose.position.x = position[0] marker.pose.position.y = position[1] marker.pose.position.z = position[2] self.last_id += 1 self.markers.markers.append(marker)
def to_marker(self): """ :return: a marker representing the map. :type: Marker """ marker = Marker() marker.type = Marker.CUBE_LIST for x in range(0, len(self.field)): for y in range(0, len(self.field[0])): marker.header.frame_id = '/odom_combined' marker.ns = 'suturo_planning/map' marker.id = 23 marker.action = Marker.ADD (x_i, y_i) = self.index_to_coordinates(x, y) marker.pose.position.x = 0 marker.pose.position.y = 0 marker.pose.position.z = 0 marker.pose.orientation.w = 1 marker.scale.x = self.cell_size marker.scale.y = self.cell_size marker.scale.z = 0.01 p = Point() p.x = x_i p.y = y_i marker.points.append(p) c = self.get_cell_by_index(x, y) marker.colors.append(self.__cell_to_color(c)) marker.lifetime = rospy.Time(0) return marker
def create_marker(self, markerArray, view, pose, view_values): marker1 = Marker() marker1.id = self.marker_id self.marker_id += 1 marker1.header.frame_id = "/map" marker1.type = marker1.TRIANGLE_LIST marker1.action = marker1.ADD marker1.scale.x = 1 marker1.scale.y = 1 marker1.scale.z = 2 marker1.color.a = 0.25 vals = view_values.values() max_val = max(vals) non_zero_vals = filter(lambda a: a != 0, vals) min_val = min(non_zero_vals) print min_val, max_val, view_values[view.ID] marker1.color.r = r_func( float((view_values[view.ID] - min_val)) / float((max_val - min_val + 1))) marker1.color.g = g_func( float((view_values[view.ID] - min_val)) / float((max_val - min_val + 1))) marker1.color.b = b_func( float((view_values[view.ID] - min_val)) / float((max_val - min_val + 1))) marker1.pose.orientation = pose.orientation marker1.pose.position = pose.position marker1.points = [Point(0,0,0.01),Point(2.5,-1.39,0.01),Point(2.5,1.39,0.01)] markerArray.markers.append(marker1)
def publish_markerArray(publisher, points, rgba=(1, 0, 0, 1), shape=Marker.CUBE, duration=rospy.Duration(360), ns='ns'): #points is expected to be a list of tuples (x,y) #It's recommended that the publisher is created with latch=True _id = 0 ma = MarkerArray() for p in points: m = Marker() m.header.frame_id = '/map' m.header.stamp = rospy.get_rostime() m.ns = ns m.id = _id m.type = shape m.action = m.ADD m.pose.position.x = p[0] m.pose.position.y = p[1] m.pose.orientation.w = 1 m.scale.x = 0.5 m.scale.y = 0.5 m.scale.z = 0.5 m.color.r = rgba[0] m.color.g = rgba[1] m.color.b = rgba[2] m.color.a = rgba[3] m.lifetime = duration ma.markers.append(m) _id += 1 publisher.publish(ma)
def publish_prob(self, waypoints, objects, probs): prob_msg = MarkerArray() i = 0 n_waypoints = len(waypoints) n_objects = len(objects) scaling_factor = max(probs) for node in self._topo_map: if node.name in waypoints: marker = Marker() marker.header.frame_id = 'map' marker.id = i marker.type = Marker.CYLINDER marker.action = Marker.ADD marker.pose = node.pose prob = 1 for j in range(0, n_objects): prob = prob*probs[n_objects*i + j] prob = prob/(scaling_factor**2) print prob marker.scale.x = 1*prob marker.scale.y = 1*prob marker.scale.z = 1 marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 prob_msg.markers.append(marker) i = i + 1 self._prob_marker_pub.publish(prob_msg)
def __publish_marker_for_object(self, object, txt): marker = Marker() marker.header.frame_id = object.object.header.frame_id marker.header.stamp = rospy.Time(0) marker.ns = "scene_classifier_marker" self.marker_id = self.marker_id + 1 marker.id = self.marker_id marker.type = Marker.TEXT_VIEW_FACING marker.action = Marker.ADD marker.pose.position.x = object.c_centroid.x marker.pose.position.y = object.c_centroid.y marker.pose.position.z = object.c_centroid.z 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.5 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.text = txt marker.lifetime = rospy.Duration.from_sec(10) self.marker_publisher.publish(marker)
def pose_cb(self, pose): pose = self.listener.transformPose(self.reference_frame,pose) q = pose.pose.orientation qvec = [q.x,q.y,q.z,q.w] euler = euler_from_quaternion(qvec) self.goal_x = pose.pose.position.x self.goal_y = pose.pose.position.y self.goal_theta = euler[2] (ex,ey,etheta) = self.compute_error() if ex < -self.dist_threshold: self.goal_theta = norm_angle(etheta + pi) print "New goal: %.2f %.2f %.2f" % (self.goal_x, self.goal_y, self.goal_theta) marker = Marker() marker.header = pose.header marker.ns = "goal_marker" marker.id = 10 marker.type = Marker.ARROW marker.action = Marker.ADD marker.pose = pose.pose marker.scale.x = 0.5 marker.scale.y = 0.5 marker.scale.z = 1.0; marker.color.a = 1.0; marker.color.r = 1.0; marker.color.g = 1.0; marker.color.b = 0.0; marker.lifetime.secs=-1.0 self.marker_pub.publish(marker)
def publish_prob2(self, waypoints, objects, probs): prob_msg = MarkerArray() i = 0 idx = 0 n_waypoints = len(waypoints) n_objects = len(objects) scaling_factor = max(probs) current_probs = [0 for foo in objects] for node in self._topo_map: if node.name in waypoints: for j in range(0, n_objects): marker = Marker() marker.header.frame_id = 'map' marker.id = idx marker.type = Marker.CYLINDER marker.action = Marker.ADD marker.pose = node.pose prob = probs[n_objects*i + j] prob = prob/(scaling_factor) print "AHAHHAHBHBHBHBHBHB", prob marker.pose.position.z = marker.pose.position.z + current_probs[j] marker.scale.x = 1*prob marker.scale.y = 1*prob marker.scale.z = 1*prob current_probs[j] = current_probs[j] + prob + 0.1 marker.color.a = 1.0 marker.color.r = 1.0*prob marker.color.g = 1.0*prob marker.color.b = 1.0*prob prob_msg.markers.append(marker) idx = idx + 1 i = i + 1 self._prob_marker_pub.publish(prob_msg)
def publish_marker(self): # create marker marker = Marker() marker.header.frame_id = "/base_link" marker.header.stamp = rospy.Time.now() marker.ns = "color" marker.id = 0 marker.type = 2 # SPHERE marker.action = 0 # ADD marker.pose.position.x = 0 marker.pose.position.y = 0 marker.pose.position.z = 1.5 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.1 marker.scale.z = 0.1 marker.color.a = self.color.a #Transparency marker.color.r = self.color.r marker.color.g = self.color.g marker.color.b = self.color.b # publish marker self.pub_marker.publish(marker)
def publish(anns, data): ar_mk_list = AlvarMarkers() marker_list = MarkerArray() marker_id = 1 for a, d in zip(anns, data): # AR markers object = deserialize_msg(d.data, AlvarMarker) ar_mk_list.markers.append(object) # Visual markers marker = Marker() marker.id = marker_id marker.header = a.pose.header marker.type = a.shape marker.ns = "ar_mk_obstacles" marker.action = Marker.ADD marker.lifetime = rospy.Duration.from_sec(0) marker.pose = copy.deepcopy(a.pose.pose.pose) marker.scale = a.size marker.color = a.color marker_list.markers.append(marker) marker_id = marker_id + 1 marker_pub = rospy.Publisher('ar_mk_marker', MarkerArray, latch=True, queue_size=1) ar_mk_pub = rospy.Publisher('ar_mk_pose_list', AlvarMarkers,latch=True, queue_size=1) ar_mk_pub.publish(ar_mk_list) marker_pub.publish(marker_list) return
def drawlandmark(pub): global pt_count points_tag = Marker() points_tag.header.frame_id = "/base_link" points_tag.action = Marker.ADD points_tag.header.stamp = rospy.Time.now() points_tag.lifetime = rospy.Time(0) points_tag.scale.x = 0.1; points_tag.scale.y = 0.1; points_tag.scale.z = 0.1; points_tag.color.a = 1.0; points_tag.color.r = 1.0; points_tag.color.g = 0.0; points_tag.color.b = 0.0; points_tag.ns = "pts_line" pt_count+=1 points_tag.id = pt_count points_tag.type = Marker.POINTS for x in range(6): p1 = Point() p1.x = tagnum_x[x]/100 p1.y = tagnum_y[x]/100 p1.z = 0 points_tag.points.append(p1) pub.publish(points_tag)
def draw_box_marker( self, id, x, y, z, dimx, dimy, dimz, color=(1.0, 1.0, 1.0, 0.0), duration=0.0, frame_id="base_link" ): marker = Marker() marker.header.stamp = rospy.Time.now() marker.ns = "object_detector" marker.type = Marker.CUBE marker.action = marker.ADD marker.id = id marker.header.frame_id = "base_link" marker.pose.position.x = x marker.pose.position.y = y marker.pose.position.z = z marker.pose.position.x = x 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 = dimx marker.scale.y = dimy marker.scale.z = dimz marker.color.a = color[0] marker.color.r = color[1] marker.color.g = color[2] marker.color.b = color[3] marker.lifetime = rospy.Duration(duration) self.box_drawer.publish(marker)
def set_position_callback(self,marker,joy): print self.position_marker.ns print joy.buttons[3] == 1 print marker.ns == "PEOPLE" if (self.position_marker.ns == "NONE" and joy.buttons[3] == 1 and marker.ns == "PEOPLE"): self.position_marker = marker print "set position" ref_marker = Marker() ref_marker.header.frame_id = "base_footprint" ref_marker.header.stamp = rospy.get_rostime() ref_marker.ns = "robot" ref_marker.type = 9 ref_marker.action = 0 ref_marker.pose.position.x = self.position_marker.pose.position.x ref_marker.pose.position.y = self.position_marker.pose.position.y ref_marker.pose.position.z = self.position_marker.pose.position.z ref_marker.scale.x = .25 ref_marker.scale.y = .25 ref_marker.scale.z = .25 ref_marker.color.r = 1.0 ref_marker.color.g = 0.0 ref_marker.color.a = 1.0 ref_marker.lifetime = rospy.Duration(0) self.ref_viz_pub.publish(ref_marker) else: pass
def __setTextMarker(self, id, waypoint, colors = [1,0,0,0]): try: marker = Marker() marker.header.frame_id = '/map' marker.header.stamp = rospy.Time.now() marker.ns = 'patrol' marker.id = id + len(self.__waypoints) marker.type = marker.TEXT_VIEW_FACING marker.action = marker.ADD marker.text = str(id) marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 marker.color.a = colors[0] marker.color.r = colors[1] marker.color.b = colors[2] marker.color.g = colors[3] marker.pose.orientation.w = 1.0 marker.pose.position.x = waypoint.target_pose.pose.position.x marker.pose.position.y = waypoint.target_pose.pose.position.y marker.pose.position.z = waypoint.target_pose.pose.position.z return marker except Exception as ex: rospy.logwarn('PatrolNode.__setMarker- ', ex.message)
-0.00798111114703, -0.00240289023425, 0.973910607185, 3.55992334659e-05, 2.53591989399e-05, 0.707060385227, 0.707153172752 ] manipPose1 = [ 0.441024004555, -0.0157218288562, 0.402612554019, 0.527145535299, 0.477387854476, 0.486045011226, 0.50791600494 ] manipPose2 = [0.01, 0.3, 0.55, 0, 0, 0, 0.707] pub_line_min_dist = rospy.Publisher('visualization_marker', Marker, queue_size=1) marker = Marker() marker.header.frame_id = "/chassis" marker.type = marker.POINTS marker.action = marker.ADD marker.id = 0 # marker scale marker.scale.x = 0.05 marker.scale.y = 0.05 marker.scale.z = 0.05 # marker color marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 1.0 # marker orientaitn marker.pose.orientation.x = 0.0
results = json.load(resultsf) count = 0 tstamp = 0 markerArray = MarkerArray() for result in results: marker = Marker() marker.header.frame_id = 'map' marker.header.stamp = rospy.Time.from_sec(result['timestamp'] / 100000000.0) marker.ns = 'object' marker.id = count marker.lifetime = rospy.Duration( 0.23 ) #The lifetime of the bounding-box, you can modify it according to the power of your machine. marker.type = Marker.CUBE marker.action = Marker.ADD marker.scale.x = result['length'] marker.scale.y = result['width'] marker.scale.z = result['height'] marker.color.b = 1.0 marker.color.a = 0.5 #The alpha of the bounding-box marker.pose.position.x = result['center']['x'] marker.pose.position.y = result['center']['y'] marker.pose.position.z = result['center']['z'] marker.pose.orientation.w = result['rotation']['w'] marker.pose.orientation.x = result['rotation']['x'] marker.pose.orientation.y = result['rotation']['y'] marker.pose.orientation.z = result['rotation']['z'] marker1 = Marker() marker1.header.frame_id = 'map' marker1.header.stamp = rospy.Time.from_sec(
def recorder(): rospy.init_node('recorder', anonymous = True) rate = rospy.Rate(10) header = Header() data_path = "/Pandaset/data" bag_path = "/Pandaset/bagfiles/" dataset = DataSet(data_path) for files in dataset.sequences(with_semseg = True): #if files != "024" : # continue f = dataset[files] f.load() print("===file", files, "open===") bag_name = "data_" + files + ".bag" full_bag_path = bag_path + bag_name bag = rosbag.Bag(full_bag_path, 'w') ###lidar visualization lidar_obj = f.lidar for i, pc0 in enumerate(f.lidar): semseg = np.array(f.semseg[i]) lidar = PointCloud() lidar_set = PointSet() lidar_set.header = header lidar_set.header.frame_id = "/base_link" lidar_set.header.stamp.secs = int(lidar_obj.timestamps[i]) lidar_set.header.stamp.nsecs = int((lidar_obj.timestamps[i] - lidar.header.stamp.secs) * (10**9)) lidar.header = header lidar.header.frame_id = "/base_link" lidar.header.stamp.secs = int(lidar_obj.timestamps[i]) lidar.header.stamp.nsecs = int((lidar_obj.timestamps[i] - lidar.header.stamp.secs) * (10**9)) for index, lidar_points in enumerate(pc0.values): lidar.points.append(Point32(lidar_points[0], lidar_points[1], lidar_points[2])) lidar_set.point.append(LidarPoint(lidar_points[0] ,lidar_points[1] ,lidar_points[2] ,lidar_points[3] ,semseg[index][0])) bag.write("lidar_data", lidar, lidar.header.stamp) bag.write("lidar_info", lidar_set, lidar_set.header.stamp) rate.sleep() rospy.loginfo("===lidar_complete===") for i, cuboid in enumerate(f.cuboids): lidar_label = MarkerArray() header = Header() header.frame_id = "base_link" header.stamp.secs = int(f.timestamps[i]) header.stamp.nsecs = int((f.timestamps[i] - header.stamp.secs) * (10**9)) POSITION_X = cuboid["position.x"] POSITION_Y = cuboid["position.y"] POSITION_Z = cuboid["position.z"] DIMENSIONS_X = cuboid["dimensions.x"] DIMENSIONS_Y = cuboid["dimensions.y"] DIMENSIONS_Z = cuboid["dimensions.z"] YAW = cuboid.yaw LABEL = cuboid.label for idx, UUID in enumerate(cuboid.uuid): label_marker = Marker() label_marker.header = header label_marker.type = Marker.CUBE label_marker.action = Marker.ADD ID = int(UUID[len(UUID)-8:len(UUID)].encode("utf-8").hex(), 16) label_marker.id = np.int32(ID) label_marker.lifetime = rospy.Duration(0.1) label_marker.pose.position.x = POSITION_X[idx] label_marker.pose.position.y = POSITION_Y[idx] label_marker.pose.position.z = POSITION_Z[idx] label_marker.scale.x = DIMENSIONS_X[idx] label_marker.scale.y = DIMENSIONS_Y[idx] label_marker.scale.z = DIMENSIONS_Z[idx] quaternion = quaternion_from_euler(0, 0, YAW[idx]) label_marker.pose.orientation.x = quaternion[1] label_marker.pose.orientation.y = quaternion[2] label_marker.pose.orientation.z = quaternion[3] label_marker.pose.orientation.w = quaternion[0] label_marker.color.a = 0.5 if LABEL[idx] == "Car": label_marker.color.b = Car[0] label_marker.color.g = Car[1] label_marker.color.r = Car[2] label_marker.ns = "Car" elif LABEL[idx] == "Motorized Scooter": label_marker.color.b = Motorized_Scooter[0] label_marker.color.g = Motorized_Scooter[1] label_marker.color.r = Motorized_Scooter[2] label_marker.ns = "Motorized Scooter" elif LABEL[idx] == "Pickup Truck": label_marker.color.b = Pickup_Truck[0] label_marker.color.g = Pickup_Truck[1] label_marker.color.r = Pickup_Truck[2] label_marker.ns = "Pickup Truck" elif LABEL[idx] == "Pedestrian": label_marker.color.b = Pedestrian[0] label_marker.color.g = Pedestrian[1] label_marker.color.r = Pedestrian[2] label_marker.ns = "Pedestrian" elif LABEL[idx] == "Signs": label_marker.color.b = Signs[0] label_marker.color.g = Signs[1] label_marker.color.r = Signs[2] label_marker.ns = "Signs" elif LABEL[idx] == "Bicycle": label_marker.color.b = Bicycle[0] label_marker.color.g = Bicycle[1] label_marker.color.r = Bicycle[2] label_marker.ns = "Bicycle" lidar_label.markers.append(label_marker) bag.write("/lidar_label", lidar_label, header.stamp) rate.sleep() rospy.loginfo("===cuboids_complete===") ###camera visualization back_camera = f.camera['back_camera'] front_camera = f.camera['front_camera'] front_left_camera = f.camera['front_left_camera'] front_right_camera = f.camera['front_right_camera'] left_camera = f.camera['left_camera'] right_camera = f.camera['right_camera'] for index, back_img in enumerate(back_camera): camera_setting(index, back_img, back_camera, lidar_obj, f, "back_camera", bag, rate) rospy.loginfo("===back_camera_complete===") for index, front_img in enumerate(front_camera): camera_setting(index, front_img, front_camera, lidar_obj, f, "front_camera", bag, rate) rospy.loginfo("===front_camera_complete===") for index, front_left_img in enumerate(front_left_camera): camera_setting(index, front_left_img, front_left_camera, lidar_obj, f, "front_left_camera", bag, rate) rospy.loginfo("===front_left_camera_complete===") for index, front_right_img in enumerate(front_right_camera): camera_setting(index, front_right_img, front_right_camera, lidar_obj, f, "front_right_camera", bag, rate) rospy.loginfo("===front_right_camera_complete===") for index, left_img in enumerate(left_camera): camera_setting(index, left_img, left_camera, lidar_obj, f, "left_camera", bag, rate) rospy.loginfo("===left_camera_complete===") for index, right_img in enumerate(right_camera): camera_setting(index, right_img, right_camera, lidar_obj, f, "right_camera", bag, rate) rospy.loginfo("===right_camera_complete===") bag.close() rospy.loginfo("===all complete===") print()
def callback(data, list): pub, ekf, publisher_marker = list # print("Start of current picture") # print(ekf.get_x_est()) # print(ekf.get_p_mat()) # print(image.encoding) # brige = CvBridge() # try: # frame = brige.imgmsg_to_cv2(image, "passthrough") # # frame = brige.compressed_imgmsg_to_cv2(image, "passthrough") # except CvBridgeError as e: # print(e) # # print(frame.shape) # point_data_3d = np.zeros((frame.shape[0] * frame.shape[1], 3)) # print(point_data_3d.shape) # for i in range(point_data_3d.shape[0]): # # print(i) # # print(np.mod(i, 480)) # # print(i/640) # point_data_3d[i, 0] = np.mod(i, 480)/300.0 # point_data_3d[i, 1] = i/640/300.0 # point_data_3d[i, 2] = frame[np.mod(i, 480), i/640] # point_data_3d = np.float32(point_data_3d) # #print(point_data_3d[1000,:]) pc = ros_numpy.numpify(data) points = np.zeros((pc.shape[0], 3)) points[:, 0] = pc['x'] points[:, 1] = pc['y'] points[:, 2] = pc['z'] # print(points.shape) points = points[::10, :] # print(points.shape) points = np.float32(points) criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 10, 1.0) K = 10 ret, label, center = cv2.kmeans(points, K, None, criteria, 10, cv2.KMEANS_RANDOM_CENTERS) # print(center) best_match = np.argmin(abs(center[:, 2] - 1)) # print(best_match) A = points[label.ravel() == best_match] # B = points[label.ravel() == 1] # C = points[label.ravel() == 2] # print(A.shape) A = A[::10, :] # print(A.shape) points = [] lim = 8 # publish point cloud # CALCULATE THE DIFFERENT SIDES LEFT AND RIGHT print("center", center[best_match, :]) median_center = np.zeros((3)) # from koordinate system (z raus) zu x aus der kamera raus median_center[0] = center[best_match, 2] median_center[1] = -center[best_match, 0] median_center[2] = -center[best_match, 1] print("median_center", median_center) current_mean_angle = np.arctan2(median_center[1], median_center[0]) print("current_mean_angle", current_mean_angle) left_segment = [] right_segment = [] for i in range(A.shape[0]): x = A[i, 2] y = -A[i, 0] if np.arctan2(y, x) > current_mean_angle: left_segment.append(A[i, :]) else: right_segment.append(A[i, :]) left_segment = np.asarray(left_segment) print("left_segment", left_segment.shape) right_segment = np.asarray(right_segment) print("right_segment", right_segment.shape) current_segment = left_segment #update EKF ekf.prediction() print("EKF Update:") ekf.update(current_segment) current_state_ekf = ekf.get_x_est() # print("x_est", current_state_ekf) # calculat 3 vectors for ebene representation such that p=s_v+mu*r_v_1+theta*r_v_1 s_v = np.asarray([[0, 0, current_state_ekf[3] / current_state_ekf[2]]], dtype="float32") r_v_1 = np.asarray([[ 0.5, 0, (current_state_ekf[3] - 0.5 * current_state_ekf[0]) / current_state_ekf[2] ]], dtype="float32") r_v_2 = np.cross(r_v_1, np.transpose(current_state_ekf[0:3])) r_v_2 = np.asarray(r_v_2, dtype="float32") r_v_2 = normalize_vector(r_v_2) r_v_1 = normalize_vector(r_v_1) print("current_state_ekf", current_state_ekf) # PUBLISH CURRENT POINT CLOUD A = current_segment for i in range(A.shape[0]): x = A[i, 2] y = -A[i, 0] z = -A[i, 1] r = 255 g = 0 b = 0 a = 150 rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0] pt = [x, y, z, rgb] points.append(pt) fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1), # PointField('rgb', 12, PointField.UINT32, 1), PointField('rgba', 12, PointField.UINT32, 1), ] # print points header = Header() header.frame_id = "d435i_link" pc2 = point_cloud2.create_cloud(header, fields, points) pub.publish(pc2) rviz = True if rviz: markerArray = MarkerArray() i = 1 for mu in np.linspace(-1, 1, 10): for theta in np.linspace(-1, 1, 10): current_point = np.transpose(s_v) + mu * np.transpose( r_v_1) + theta * np.transpose(r_v_2) r = 0.02 marker = Marker() marker.header.frame_id = "d435i_link" marker.id = i marker.type = marker.SPHERE marker.action = marker.ADD marker.scale.x = r * 2 # r*2 marker.scale.y = r * 2 marker.scale.z = r * 2 marker.color.r = 1 marker.color.g = 1 marker.color.a = 1 # transparency marker.pose.orientation.w = 1.0 marker.pose.position.x = current_point[2] # x marker.pose.position.y = -current_point[0] # y marker.pose.position.z = -current_point[1] # z markerArray.markers.append(marker) i = i + 1 publisher_marker.publish(markerArray)
def node(): global frontiers, mapData, global1, global2, global3, globalmaps, litraIndx, n_robots, namespace_init_count rospy.init_node('filter', anonymous=False) # fetching all parameters map_topic = rospy.get_param('~map_topic', '/map') threshold = rospy.get_param('~costmap_clearing_threshold', 70) info_radius = rospy.get_param( '~info_radius', 1.0 ) #this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate goals_topic = rospy.get_param('~goals_topic', '/detected_points') n_robots = rospy.get_param('~n_robots', 1) namespace = rospy.get_param('~namespace', '') namespace_init_count = rospy.get_param('namespace_init_count', 1) rateHz = rospy.get_param('~rate', 100) litraIndx = len(namespace) rate = rospy.Rate(rateHz) #------------------------------------------- rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack) #--------------------------------------------------------------------------------------------------------------- for i in range(0, n_robots): globalmaps.append(OccupancyGrid()) if len(namespace) > 0: for i in range(0, n_robots): rospy.Subscriber( namespace + str(i + namespace_init_count) + '/move_base_node/global_costmap/costmap', OccupancyGrid, globalMap) elif len(namespace) == 0: rospy.Subscriber('/move_base_node/global_costmap/costmap', OccupancyGrid, globalMap) #wait if map is not received yet while (len(mapData.data) < 1): pass #wait if any of robots' global costmap map is not received yet for i in range(0, n_robots): while (len(globalmaps[i].data) < 1): pass global_frame = "/" + mapData.header.frame_id tfLisn = tf.TransformListener() if len(namespace) > 0: for i in range(0, n_robots): tfLisn.waitForTransform( global_frame[1:], namespace + str(i + namespace_init_count) + '/base_link', rospy.Time(0), rospy.Duration(10.0)) elif len(namespace) == 0: tfLisn.waitForTransform(global_frame[1:], '/base_link', rospy.Time(0), rospy.Duration(10.0)) rospy.Subscriber(goals_topic, PointStamped, callback=callBack, callback_args=[tfLisn, global_frame[1:]]) pub = rospy.Publisher('frontiers', Marker, queue_size=10) pub2 = rospy.Publisher('centroids', Marker, queue_size=10) filterpub = rospy.Publisher('filtered_points', PointArray, queue_size=10) rospy.loginfo("the map and global costmaps are received") # wait if no frontier is received yet while len(frontiers) < 1: pass points = Marker() points_clust = Marker() #Set the frame ID and timestamp. See the TF tutorials for information on these. points.header.frame_id = mapData.header.frame_id points.header.stamp = rospy.Time.now() points.ns = "markers2" points.id = 0 points.type = Marker.POINTS #Set the marker action for latched frontiers. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) points.action = Marker.ADD points.pose.orientation.w = 1.0 points.scale.x = 0.2 points.scale.y = 0.2 points.color.r = 255.0 / 255.0 points.color.g = 255.0 / 255.0 points.color.b = 0.0 / 255.0 points.color.a = 1 points.lifetime = rospy.Duration() p = Point() p.z = 0 pp = [] pl = [] points_clust.header.frame_id = mapData.header.frame_id points_clust.header.stamp = rospy.Time.now() points_clust.ns = "markers3" points_clust.id = 4 points_clust.type = Marker.POINTS #Set the marker action for centroids. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) points_clust.action = Marker.ADD points_clust.pose.orientation.w = 1.0 points_clust.scale.x = 0.2 points_clust.scale.y = 0.2 points_clust.color.r = 0.0 / 255.0 points_clust.color.g = 255.0 / 255.0 points_clust.color.b = 0.0 / 255.0 points_clust.color.a = 1 points_clust.lifetime = rospy.Duration() temppoint = PointStamped() temppoint.header.frame_id = mapData.header.frame_id temppoint.header.stamp = rospy.Time(0) temppoint.point.z = 0.0 arraypoints = PointArray() tempPoint = Point() tempPoint.z = 0.0 #------------------------------------------------------------------------- #--------------------- Main Loop ------------------------------- #------------------------------------------------------------------------- while not rospy.is_shutdown(): #------------------------------------------------------------------------- #Clustering frontier points centroids = [] front = copy(frontiers) if len(front) > 1: ms = MeanShift(bandwidth=0.3) ms.fit(front) centroids = ms.cluster_centers_ #centroids array is the centers of each cluster #if there is only one frontier no need for clustering, i.e. centroids=frontiers if len(front) == 1: centroids = front frontiers = copy(centroids) #------------------------------------------------------------------------- #clearing old frontiers z = 0 while z < len(centroids): cond = False temppoint.point.x = centroids[z][0] temppoint.point.y = centroids[z][1] for i in range(0, n_robots): transformedPoint = tfLisn.transformPoint( globalmaps[i].header.frame_id, temppoint) x = array([transformedPoint.point.x, transformedPoint.point.y]) cond = (gridValue(globalmaps[i], x) > threshold) or cond if (cond or (informationGain(mapData, [centroids[z][0], centroids[z][1]], info_radius * 0.5)) < 0.2): centroids = delete(centroids, (z), axis=0) z = z - 1 z += 1 #------------------------------------------------------------------------- #publishing arraypoints.points = [] for i in centroids: tempPoint.x = i[0] tempPoint.y = i[1] arraypoints.points.append(copy(tempPoint)) filterpub.publish(arraypoints) pp = [] for q in range(0, len(frontiers)): p.x = frontiers[q][0] p.y = frontiers[q][1] pp.append(copy(p)) points.points = pp pp = [] for q in range(0, len(centroids)): p.x = centroids[q][0] p.y = centroids[q][1] pp.append(copy(p)) points_clust.points = pp pub.publish(points) pub2.publish(points_clust) rate.sleep()
def publish_tracked_people(self, now): """ Publish markers of tracked people to Rviz and to <people_tracked> topic """ people_tracked_msg = PersonArray() people_tracked_msg.header.stamp = now people_tracked_msg.header.frame_id = self.publish_people_frame marker_id = 0 # Make sure we can get the required transform first: if self.use_scan_header_stamp_for_tfs: tf_time = now try: self.listener.waitForTransform(self.publish_people_frame, self.fixed_frame, tf_time, rospy.Duration(1.0)) transform_available = True except: transform_available = False else: tf_time = rospy.Time(0) transform_available = self.listener.canTransform( self.publish_people_frame, self.fixed_frame, tf_time) marker_id = 0 if not transform_available: rospy.loginfo( "Person tracker: tf not avaiable. Not publishing people") else: for person in self.objects_tracked: if person.is_person == True: if self.publish_occluded or person.seen_in_current_scan: # Only publish people who have been seen in current scan, unless we want to publish occluded people # Get position in the <self.publish_people_frame> frame ps = PointStamped() ps.header.frame_id = self.fixed_frame ps.header.stamp = tf_time ps.point.x = person.pos_x ps.point.y = person.pos_y try: ps = self.listener.transformPoint( self.publish_people_frame, ps) except: rospy.logerr( "Not publishing people due to no transform from fixed_frame-->publish_people_frame" ) continue # publish to people_tracked topic new_person = Person() new_person.pose.position.x = ps.point.x new_person.pose.position.y = ps.point.y yaw = math.atan2(person.vel_y, person.vel_x) quaternion = tf.transformations.quaternion_from_euler( 0, 0, yaw) new_person.pose.orientation.x = quaternion[0] new_person.pose.orientation.y = quaternion[1] new_person.pose.orientation.z = quaternion[2] new_person.pose.orientation.w = quaternion[3] new_person.id = person.id_num people_tracked_msg.people.append(new_person) # publish rviz markers # Cylinder for body marker = Marker() marker.header.frame_id = self.publish_people_frame marker.header.stamp = now marker.ns = "People_tracked" marker.color.r = person.colour[0] marker.color.g = person.colour[1] marker.color.b = person.colour[2] marker.color.a = ( rospy.Duration(3) - (rospy.get_rostime() - person.last_seen) ).to_sec() / rospy.Duration(3).to_sec() + 0.1 marker.pose.position.x = ps.point.x marker.pose.position.y = ps.point.y marker.id = marker_id marker_id += 1 marker.type = Marker.CYLINDER marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 1.2 marker.pose.position.z = 0.8 self.marker_pub.publish(marker) # Sphere for head shape marker.type = Marker.SPHERE marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 marker.pose.position.z = 1.5 marker.id = marker_id marker_id += 1 self.marker_pub.publish(marker) # Text showing person's ID number marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 1.0 marker.color.a = 1.0 marker.id = marker_id marker_id += 1 marker.type = Marker.TEXT_VIEW_FACING marker.text = str(person.id_num) marker.scale.z = 0.2 marker.pose.position.z = 1.7 self.marker_pub.publish(marker) # Arrow pointing in direction they're facing with magnitude proportional to speed marker.color.r = person.colour[0] marker.color.g = person.colour[1] marker.color.b = person.colour[2] marker.color.a = ( rospy.Duration(3) - (rospy.get_rostime() - person.last_seen) ).to_sec() / rospy.Duration(3).to_sec() + 0.1 start_point = Point() end_point = Point() start_point.x = marker.pose.position.x start_point.y = marker.pose.position.y end_point.x = start_point.x + 0.5 * person.vel_x end_point.y = start_point.y + 0.5 * person.vel_y marker.pose.position.x = 0. marker.pose.position.y = 0. marker.pose.position.z = 0.1 marker.id = marker_id marker_id += 1 marker.type = Marker.ARROW marker.points.append(start_point) marker.points.append(end_point) marker.scale.x = 0.05 marker.scale.y = 0.1 marker.scale.z = 0.2 self.marker_pub.publish(marker) # <self.confidence_percentile>% confidence bounds of person's position as an ellipse: cov = person.filtered_state_covariances[0][ 0] + person.var_obs # cov_xx == cov_yy == cov std = cov**(1. / 2.) gate_dist_euclid = scipy.stats.norm.ppf( 1.0 - (1.0 - self.confidence_percentile) / 2., 0, std) marker.pose.position.x = ps.point.x marker.pose.position.y = ps.point.y marker.type = Marker.SPHERE marker.scale.x = 2 * gate_dist_euclid marker.scale.y = 2 * gate_dist_euclid marker.scale.z = 0.01 marker.color.r = person.colour[0] marker.color.g = person.colour[1] marker.color.b = person.colour[2] marker.color.a = 0.1 marker.pose.position.z = 0.0 marker.id = marker_id marker_id += 1 self.marker_pub.publish(marker) # Clear previously published people markers for m_id in xrange(marker_id, self.prev_person_marker_id): marker = Marker() marker.header.stamp = now marker.header.frame_id = self.publish_people_frame marker.ns = "People_tracked" marker.id = m_id marker.action = marker.DELETE self.marker_pub.publish(marker) self.prev_person_marker_id = marker_id # Publish people tracked message self.people_tracked_pub.publish(people_tracked_msg)
import rospy from visualization_msgs.msg import Marker rospy.init_node("marker_pub") pub = rospy.Publisher("visualization_marker", Marker, queue_size=10) rate = rospy.Rate(25) while not rospy.is_shutdown(): marker_data = Marker() marker_data.header.frame_id = "map" marker_data.header.stamp = rospy.Time.now() marker_data.ns = "basic_shapes" marker_data.id = 0 marker_data.action = Marker.ADD marker_data.pose.position.x = 0.0 marker_data.pose.position.y = 0.0 marker_data.pose.position.z = 0.0 marker_data.pose.orientation.x = 0.0 marker_data.pose.orientation.y = 0.0 marker_data.pose.orientation.z = 1.0 marker_data.pose.orientation.w = 0.0 marker_data.color.r = 1.0 marker_data.color.g = 0.0 marker_data.color.b = 0.0 marker_data.color.a = 1.0
y = float(line.strip().split()[1]) if len(line.strip().split()) >= 3: mode = int(line.strip().split()[2]) path_x.append(x) path_y.append(y) modes.append(mode) path_len += 1 print(x, y, mode) rospy.sleep(1) while path_len > count: marker = Marker() marker.header.frame_id = "/base_link" marker.type = marker.SPHERE marker.action = marker.ADD marker.scale.x = 0.3 marker.scale.y = 0.3 marker.scale.z = 0.3 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 1.0 if modes[count]%2 == 0: marker.color.r = 1 marker.color.g = 0.5 marker.color.b = 0
def lidar_cb(self, msg): rospy.loginfo("starting lidar cb") if self.road_map is None: return rospy.loginfo("got map") ranges = np.array(msg.ranges) num_ranges = len(ranges) angles = (np.ones(num_ranges) * msg.angle_min) + ( np.arange(num_ranges) * msg.angle_increment) # Discard extraneous data (< range_min or > range_max) angles = angles[ranges > msg.range_min] ranges = ranges[ranges > msg.range_min] angles = angles[ranges < msg.range_max] ranges = ranges[ranges < msg.range_max] # Remove data points not in the viable range in front of the car, ie theta E (-viable_angle_range, viable_angle_range) ranges = ranges[angles > -self.viable_angle_range] angles = angles[angles > -self.viable_angle_range] ranges = ranges[angles < self.viable_angle_range] angles = angles[angles < self.viable_angle_range] # Remove data points outside self.in_range angles = angles[ranges < self.in_range] ranges = ranges[ranges < self.in_range] # Convert to cartesian, LIDAR-frame positions x = ranges * np.cos(angles) y = ranges * np.sin(angles) new_num_ranges = len(x) arr = np.vstack((x, y)) # Find transform from LIDAR frame to map frame try: (trans, rot) = self.listener.lookupTransform(msg.header.frame_id, self.road_map.frame, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rate.sleep() return # Convert translation + quaternion to homogeneous transformation matrix T_lidar_map = transformations.quaternion_matrix(rot) T_lidar_map = T_lidar_map.T TEMP = transformations.quaternion_matrix(rot) TEMP = np.linalg.inv(TEMP[:3, :3]) T_lidar_map[:3, 3] = np.matmul(TEMP, -np.array(trans).T) #T_lidar_map = T_lidar_map.T # Convert LIDAR points from LIDAR frame to map frame arr_LIDAR = np.vstack( (x, y, np.zeros(new_num_ranges), np.ones(new_num_ranges))) arr_MAP = np.matmul(T_lidar_map, arr_LIDAR) print(T_lidar_map) arr_MAP = arr_MAP[:2] x_map = arr_MAP[0] y_map = arr_MAP[1] # Find which points are on the roads on_road = self.road_map.check_free(arr_MAP) x_obs = x_map[on_road] y_obs = y_map[on_road] """ # Find occupancy grid matrix with detected obstacles filled in new_occ_matrix = self.road_map.add_obstructions(np.vstack((x_obs, y_obs))) # Creates and publishes OccupancyGrid message with obstacles to /obstacles obs_msg = OccupancyGrid() # now = rospy.get_time() # obs_msg.header.stamp = now obs_msg.header.frame_id = self.road_map.frame obs_msg.info = self.road_map.msg.info # obs_msg.info.map_load_time = rospy.Time(0) # Probably unnecessary? obs_msg.data = np.ravel(new_occ_matrix) rospy.loginfo("publishing") self.obs_pub.publish(obs_msg) print("test") """ marker = Marker() marker.header.frame_id = "map" marker.type = marker.POINTS marker.action = marker.ADD marker.scale.x = 0.5 marker.scale.y = 0.5 for i in range(len(x_obs)): TEMP = Point() TEMP.x = x_obs[i] TEMP.y = y_obs[i] TEMP.z = .05 marker.points.append(TEMP) TEMP = ColorRGBA() TEMP.r = 1 TEMP.g = 0 TEMP.b = 0 TEMP.a = 1 marker.colors.append(TEMP) self.vis_pub.publish(marker)
#painting all the syntetic points for i in xrange(10, 5000): p = numpy.random.multivariate_normal(meanModel, covMatModel) if syntetic_samples == None: syntetic_samples = [p] else: syntetic_samples = concatenate((syntetic_samples, [p]), axis=0) marker = Marker() marker.header.frame_id = "/root" marker.header.stamp = rospy.Time.now() marker.ns = "my_namespace2" marker.id = i marker.type = visualization_msgs.msg.Marker.SPHERE marker.action = visualization_msgs.msg.Marker.ADD marker.pose.position.x = p[0] marker.pose.position.y = p[1] marker.pose.position.z = p[2] marker.pose.orientation.x = 1 marker.pose.orientation.y = 1 marker.pose.orientation.z = 1 marker.pose.orientation.w = 1 marker.scale.x = 0.05 marker.scale.y = 0.05 marker.scale.z = 0.05 marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 0.0 marker.color.b = 0.0 points_pub.publish(marker)
from geometry_msgs.msg import Point import tf import numpy as np rospy.init_node("marker_node", anonymous=True) pub = rospy.Publisher("marker_test", Marker, queue_size=10) rate = rospy.Rate(25) while not rospy.is_shutdown(): marker = Marker() marker.header.frame_id = "map" marker.header.stamp = rospy.Time.now() marker.id = 0 marker.action = Marker.ADD marker.lifetime = rospy.Duration() marker.type = Marker.MESH_RESOURCE marker.mesh_resource = "file:///media/psf/Home/Downloads/ros-kitti-project/src/ROS-notes-by-kwea123/kitti/bmw_x5/BMW X5 4.dae" # marker.mesh_use_embedded_materials = True marker.pose.position.x = 0.0 marker.pose.position.y = 0.0 marker.pose.position.z = -1.73 q = tf.transformations.quaternion_from_euler(np.pi/2, 0, np.pi); marker.pose.orientation.x = q[0] marker.pose.orientation.y = q[1] marker.pose.orientation.z = q[2] marker.pose.orientation.w = q[3]
def lidar_cb(self, msg): if msg.header.stamp - self.last_update < self.min_time: return #read in ranges from lidar only for a given range #global r r = np.array(msg.ranges, dtype=np.float64) rsmall = r[(a >= angle_from) * (a <= angle_to)] asmall = a[(a >= angle_from) * (a <= angle_to)] #a = np.array(msg.angle, dtype=mp.float64) t = msg.header.stamp #rospy.Time(0) if self.tf_listener.canTransform('map', 'laser', t): (self.lx, self.ly, _), rot = self.tf_listener.lookupTransform('map', 'laser', t) _, _, self.ltheta = euler_from_quaternion(rot) (self.x, self.y, _), rot = self.tf_listener.lookupTransform('map', 'base', t) _, _, self.theta = euler_from_quaternion(rot) log.info('set pose: %.2f, %.2f, %.2f', self.lx, self.ly, self.ltheta) else: log.warn('did not find current pose, ignoring update') return with timed_code('update'): h, w = self.log_map.shape rsmall[rsmall > VIEW_DISTANCE] = VIEW_DISTANCE points = np.c_[rsmall, -asmall + self.ltheta] global xs, ys xs, ys = np.meshgrid( si(np.arange(w)) - self.x0, si(np.arange(h)) - self.y0) with timed_code('-beam map'): hits = beam_map(self.lx, self.ly, points, self.x, self.y, self.theta) self.log_map[hits > 0] += (l_view - l_0) #translate log map to occupancygrid map (0-100) self.explore_map[:, :] = 1.0 - 1.0 / (1.0 + np.exp(self.log_map)) #self.getnewexploregoal(hitslidar) self.last_update = msg.header.stamp #********************************************** # get cool new point ***************************** hits = np.zeros(xs.shape, dtype=np.uint8) #ris = [i for i in range(len(r)) if (0.15 < r[i] < 4)] ris = list(range(0, len(r), 2)) sample = [i for i in ris if r[i] > 0.12] #sample = np.random.choice(sample, 100) randomr = [uniform(0.12, r[i] - 0.05) for i in sample] randomx = [ self.lx + randomr[j] * np.cos(-a[i] + self.ltheta) for j, i in enumerate(sample) ] randomy = [ self.ly + randomr[j] * np.sin(-a[i] + self.ltheta) for j, i in enumerate(sample) ] costs = np.zeros(len(randomx)) countfree = 0 for i in range(len(randomx)): if ((randomx[i] > np.min(xs)) * (randomx[i] < np.max(xs)) * (randomy[i] > np.min(ys)) * (randomy[i] < np.max(ys))): costs[i] += np.sqrt((self.x - randomx[i])**2 + (self.y - randomy[i])**2) * 1 angle = (-a[i] + self.ltheta - self.theta) * 180 / np.pi costs[i] += np.abs((np.mod(angle + 180, 360) - 180)) * 100 #info gain hits = beam_map2(randomx[i], randomy[i], -a[i] + self.ltheta) costs[i] += np.sum(self.explore_map[hits == HIT_VIEW]) * 1 #self.explore_map[hits == HIT_VIEW]=1 #penelty for point we have been to if (self.explore_map[gu(randomy[i] + self.y0), gu(randomx[i] + self.x0)] > 0.2): costs[i] += 1000 countfree += 1 if (self.occ_map[gu(randomy[i] + self.y0), gu(randomx[i] + self.x0)] > 0.2): costs[i] += 10e3 countfree += 1 elif (self.occ_map.line_max( (self.x, self.y), (randomx[i] + self.x0, randomy[i] + self.y0)) > 0.2): costs[i] += 1000 countfree += 1 else: costs[i] = np.inf #new points as we are in a deadend if False and (countfree > len(randomx) - 2): costs = np.zeros(len(randomx)) randomx = [uniform(self.xmin, self.xmax) for i in range(0, len(r))] randomy = [uniform(self.ymin, self.ymax) for i in range(0, len(r))] randoma = [uniform(-np.pi, np.pi) for i in range(0, len(r))] costs = np.zeros(len(randomx)) countfree = 0 for i in range(len(randomx)): if ((randomx[i] > np.min(xs)) * (randomx[i] < np.max(xs)) * (randomy[i] > np.min(ys)) * (randomy[i] < np.max(ys))): costs[i] += np.sqrt((self.x - randomx[i])**2 + (self.y - randomy[i])**2) * 1 angle = (-a[i] + self.ltheta - self.theta) * 180 / np.pi costs[i] += np.abs((np.mod(angle + 180, 360) - 180)) * 1 hits = beam_map2(randomx[i], randomy[i], randoma[i]) costs[i] += np.sum(self.explore_map[hits == HIT_VIEW]) * 1 if (self.explore_map[gu(randomy[i] + self.y0), gu(randomx[i] + self.x0)] > 0.2): costs[i] += 1000 if (self.occ_map[gu(randomy[i] + self.y0), gu(randomx[i] + self.x0)] > 0.2): costs[i] += 10000 #weights = 1.0/costs #ps = weights/weights.sum() #j = np.random.choice(len(costs), p=ps) j = np.argmin(costs) msg = PoseStamped() msg.header.stamp = rospy.Time.now() msg.header.frame_id = "map" msg.pose.orientation.w = 1.0 msg.pose.position.x = randomx[j] msg.pose.position.y = randomy[j] msg.pose.orientation.z = a[j] #pub.publish(msg) self.pub_goal.publish(msg) print(randomx[j]) print(randomy[j]) print(a[j] * 180 / np.pi) print(costs[j]) marker = Marker() marker.header.frame_id = "/map" marker.type = marker.SPHERE marker.action = marker.ADD marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.pose.orientation.w = 1.0 marker.pose.position.x = randomx[j] marker.pose.position.y = randomy[j] self.pub_point.publish(marker) markerArray = MarkerArray() for i in range(len(randomx)): markera = Marker() markera.header.frame_id = "/map" markera.type = marker.SPHERE markera.action = marker.ADD markera.scale.x = 0.05 markera.scale.y = 0.05 markera.scale.z = 0.05 markera.color.a = 1.0 markera.color.r = 0.0 markera.color.g = 1.0 markera.color.b = 0.0 markera.pose.orientation.w = 1.0 markera.id = i markera.pose.position.x = randomx[i] markera.pose.position.y = randomy[i] markerArray.markers.append(markera) self.pub_allpoints.publish(markerArray)
def create_frustum_marker(self, markerArray, view, pose, view_values): marker1 = Marker() marker1.id = self.marker_id self.marker_id += 1 marker1.header.frame_id = "/map" marker1.type = marker1.LINE_LIST marker1.action = marker1.ADD marker1.scale.x = 0.05 marker1.color.a = 0.3 vals = view_values.values() max_val = max(vals) non_zero_vals = filter(lambda a: a != 0, vals) min_val = min(non_zero_vals) print min_val, max_val, view_values[view.ID] marker1.color.r = r_func( float((view_values[view.ID] - min_val)) / float( (max_val - min_val + 1))) marker1.color.g = g_func( float((view_values[view.ID] - min_val)) / float( (max_val - min_val + 1))) marker1.color.b = b_func( float((view_values[view.ID] - min_val)) / float( (max_val - min_val + 1))) marker1.pose.orientation = pose.orientation marker1.pose.position = pose.position points = view.get_frustum() marker1.points.append(points[0]) marker1.points.append(points[1]) marker1.points.append(points[2]) marker1.points.append(points[3]) marker1.points.append(points[0]) marker1.points.append(points[2]) marker1.points.append(points[1]) marker1.points.append(points[3]) marker1.points.append(points[4]) marker1.points.append(points[5]) marker1.points.append(points[6]) marker1.points.append(points[7]) marker1.points.append(points[4]) marker1.points.append(points[6]) marker1.points.append(points[5]) marker1.points.append(points[7]) marker1.points.append(points[0]) marker1.points.append(points[4]) marker1.points.append(points[2]) marker1.points.append(points[6]) marker1.points.append(points[1]) marker1.points.append(points[5]) marker1.points.append(points[3]) marker1.points.append(points[7]) markerArray.markers.append(marker1)
def sim(): rospy.init_node('optimization') o = Optimizer() c = Cfs(21, 0) rospy.Subscriber('/odom', Odometry, o.odom_callback) rospy.Subscriber('/filteredGPS', Point, o.filter_callback) rospy.Subscriber('/obstacle1_poly', PolygonStamped, o.obstacle_poly_callback) # from fake_human nodes rospy.Subscriber('/obstacle1_v', Twist, o.obstacle_vel_callback) rospy.Subscriber('/obstacle2_poly', Polygons, o.lobstacle_poly_callback) # from lidar #rospy.Subscriber('/obstacle2_v', Twist, o.lobstacle_vel_callback) rospy.Subscriber('/human', Point32, o.human_callback) rospy.Subscriber('/goal_state', Point32, o.goal_callback) path_pub = rospy.Publisher('/path', Path, queue_size=100) margin_rviz_pub = rospy.Publisher('/margins', PolygonStamped, queue_size=100) goal_pub = rospy.Publisher('/goalrviz', PointStamped, queue_size=100) line_viz = rospy.Publisher('/line', Marker, queue_size=50) cmv_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) rate = rospy.Rate(10) try: barrier = rospy.get_param('~barrier_radius') except: barrier = 1.5 c.set_barrier(barrier) while not rospy.is_shutdown(): o.move_pred() c.set_margin(o.margin) obstacles = o.obstacles + o.lidar_obs vels = o.vels + o.lidar_vels refpath, linegen = c.optimize( obstacles, vels, o.x, o.goal, human=o.human) # o.human if tracking human marker = Marker() marker.header.frame_id = "/odom" marker.type = marker.LINE_STRIP marker.action = marker.ADD marker.scale.x = 0.03 marker.scale.y = 0.03 marker.scale.z = 0.03 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.color.a = 1.0 pts = [] for i in range(-10, 10): add = Point() add.x = i add.y = linegen[0] * i + linegen[1] pts.append(add) marker.points = pts line_viz.publish(marker) if stopCheck(refpath, 0.2): path = to_path(refpath) path_pub.publish(path) else: path = to_path([]) path_pub.publish(path) margin = o.get_margin_polygon() margin_rviz_pub.publish(margin) goal_pub.publish(o.get_goal()) rate.sleep()
audio_pub = rospy.Publisher('train_sound', AudioData, queue_size=100) marker_pub = rospy.Publisher('train_ground_truth', MarkerArray, queue_size=1) assert sample_rate % MSG_FREQ == 0 frames_per_msg = sample_rate / MSG_FREQ for sounds, positions in gen: if rospy.is_shutdown(): break sounds = sounds[0] positions = positions[0] rate = rospy.Rate(MSG_FREQ) sound = np.int16(sounds.mean(0) * 32768) delete_all = Marker() delete_all.action = Marker.DELETEALL delete_all.ns = 'sound_sources' delete_all = MarkerArray(markers=[delete_all]) for t in xrange((sound.shape[0]+frames_per_msg-1)/frames_per_msg): begin = t*frames_per_msg end = min((t+1)*frames_per_msg, sound.shape[0]) audio_pub.publish(AudioData(data = sound[begin:end].tostring())) markers = MarkerArray() for i, (snd, pos) in enumerate(zip(sounds, positions)): scale = np.abs(snd[begin:end]).max() if scale == 0.: continue marker = Marker() marker.header.stamp = rospy.Time.now()
def publish_tracked_objects(self, now): """ Publish markers of tracked objects to Rviz """ # Make sure we can get the required transform first: if self.use_scan_header_stamp_for_tfs: tf_time = now try: self.listener.waitForTransform(self.publish_people_frame, self.fixed_frame, tf_time, rospy.Duration(1.0)) transform_available = True except: transform_available = False else: tf_time = rospy.Time(0) transform_available = self.listener.canTransform( self.publish_people_frame, self.fixed_frame, tf_time) marker_id = 0 if not transform_available: rospy.loginfo( "Person tracker: tf not avaiable. Not publishing people") else: for track in self.objects_tracked: if track.is_person: continue if self.publish_occluded or track.seen_in_current_scan: # Only publish people who have been seen in current scan, unless we want to publish occluded people # Get the track position in the <self.publish_people_frame> frame ps = PointStamped() ps.header.frame_id = self.fixed_frame ps.header.stamp = tf_time ps.point.x = track.pos_x ps.point.y = track.pos_y try: ps = self.listener.transformPoint( self.publish_people_frame, ps) except: continue # publish rviz markers marker = Marker() marker.header.frame_id = self.publish_people_frame marker.header.stamp = now marker.ns = "objects_tracked" if track.in_free_space < self.in_free_space_threshold: marker.color.r = track.colour[0] marker.color.g = track.colour[1] marker.color.b = track.colour[2] else: marker.color.r = 0 marker.color.g = 0 marker.color.b = 0 marker.color.a = 1 marker.pose.position.x = ps.point.x marker.pose.position.y = ps.point.y marker.id = marker_id marker_id += 1 marker.type = Marker.CYLINDER marker.scale.x = 0.05 marker.scale.y = 0.05 marker.scale.z = 0.2 marker.pose.position.z = 0.15 self.marker_pub.publish(marker) # # Publish a marker showing distance travelled: # if track.dist_travelled > 1: # marker.color.r = 1.0 # marker.color.g = 1.0 # marker.color.b = 1.0 # marker.color.a = 1.0 # marker.id = marker_id # marker_id += 1 # marker.type = Marker.TEXT_VIEW_FACING # marker.text = str(round(track.dist_travelled,1)) # marker.scale.z = 0.1 # marker.pose.position.z = 0.6 # self.marker_pub.publish(marker) # Publish <self.confidence_percentile>% confidence bounds of track as an ellipse: # cov = track.filtered_state_covariances[0][0] + track.var_obs # cov_xx == cov_yy == cov # std = cov**(1./2.) # gate_dist_euclid = scipy.stats.norm.ppf(1.0 - (1.0-self.confidence_percentile)/2., 0, std) # marker.type = Marker.SPHERE # marker.scale.x = 2*gate_dist_euclid # marker.scale.y = 2*gate_dist_euclid # marker.scale.z = 0.01 # marker.color.r = 1.0 # marker.color.g = 1.0 # marker.color.b = 1.0 # marker.color.a = 0.1 # marker.pose.position.z = 0.0 # marker.id = marker_id # marker_id += 1 # self.marker_pub.publish(marker) # Clear previously published track markers for m_id in xrange(marker_id, self.prev_track_marker_id): marker = Marker() marker.header.stamp = now marker.header.frame_id = self.publish_people_frame marker.ns = "objects_tracked" marker.id = m_id marker.action = marker.DELETE self.marker_pub.publish(marker) self.prev_track_marker_id = marker_id
def create_axis_marker(self, axis, id_num, rgba=None, name=None, axes_scale=1.0): marker = Marker() marker.header.frame_id = self.frame_id marker.header.stamp = self.timestamp marker.id = id_num marker.type = marker.ARROW marker.action = marker.ADD marker.lifetime = rospy.Duration(1.0) if name is not None: # although useful, this causes a warning and rviz and goes # against the documentation "NOTE: only used for text markers # string text" marker.text = name # axis_arrow = {'head_diameter': 0.02, # 'shaft_diameter': 0.012, # 'head_length': 0.012, # 'length': 0.08} axis_arrow = { 'head_diameter': 0.02 * axes_scale, 'shaft_diameter': 0.012 * axes_scale, 'head_length': 0.012 * axes_scale, 'length': 0.08 * axes_scale } # "scale.x is the shaft diameter, and scale.y is the # head diameter. If scale.z is not zero, it specifies # the head length." - # http://wiki.ros.org/rviz/DisplayTypes/Marker#Arrow_.28ARROW.3D0.29 marker.scale.x = axis_arrow['shaft_diameter'] marker.scale.y = axis_arrow['head_diameter'] marker.scale.z = axis_arrow['head_length'] if rgba is None: # make as bright as possible den = float(np.max(self.id_color)) marker.color.r = self.id_color[2] / den #1.0 marker.color.g = self.id_color[1] / den #0.0 marker.color.b = self.id_color[0] / den #0.0 marker.color.a = 0.5 else: c = marker.color c.r, c.g, c.b, c.a = rgba start_point = Point() x = self.marker_position[0] y = self.marker_position[1] z = self.marker_position[2] start_point.x = x start_point.y = y start_point.z = z end_point = Point() length = axis_arrow['length'] end_point.x = x + (axis[0] * length) end_point.y = y + (axis[1] * length) end_point.z = z + (axis[2] * length) marker.points = [start_point, end_point] return marker
def create_marker(pos, orientation=1.0, color=(1.0, 1.0, 1.0), m_scale=0.5, frame_id="/velodyneVPL", duration=10, marker_id=0, mesh_resource=None, marker_type=2, marker_text=""): """Create marker object using the map information and the node position :param pos: list of 3d postion for the marker :param orientation: orientation of the maker (1 for no orientation) :param color: a 3 vector of 0-1 rgb values :param m_scale: scale of the marker (1.0) for normal scale :param frame_id: ROS frame id :param duration: duration in seconds for this marker dissapearance :param marker_id: :param mesh_resource: :param marker_type: one of the following types (use the int value) http://wiki.ros.org/rviz/DisplayTypes/Marker ARROW = 0 CUBE = 1 SPHERE = 2 CYLINDER = 3 LINE_STRIP = 4 LINE_LIST = 5 CUBE_LIST = 6 SPHERE_LIST = 7 POINTS = 8 TEXT_VIEW_FACING = 9 MESH_RESOURCE = 10 TRIANGLE_LIST = 11 :param marker_text: text string used for the marker :return: """ marker = Marker() marker.header.frame_id = frame_id marker.id = marker_id if mesh_resource: marker.type = marker.MESH_RESOURCE marker.mesh_resource = mesh_resource else: marker.type = marker_type marker.action = marker.ADD marker.scale.x = m_scale marker.scale.y = m_scale marker.scale.z = m_scale marker.color.a = 1.0 marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] marker.pose.orientation.w = orientation marker.text = marker_text marker.pose.position.x = pos[0] marker.pose.position.y = pos[1] marker.pose.position.z = pos[2] d = rospy.Duration.from_sec(duration) marker.lifetime = d return marker
def people_tracked_callback(self, people_tracked_msg): toc = timeit.default_timer() # Don't start keeping tracking of tracking time until at least one person has started to be tracked # This is because the tracking programs are often slow to startup when no tfs have been published yet if len(people_tracked_msg.people) > 0: self.tracking_started = True if self.tracking_started: tracking_time = toc - self.tic self.tracking_time_cum += tracking_time self.tracking_msg_count += 1 if tracking_time > self.worst_tracking_time: self.worst_tracking_time = tracking_time rospy.loginfo( "***************************Worst tracking time so far: %.2f***************************", self.worst_tracking_time) if self.savebag_filename is not None: save_time = (self.next_scan_msg_time - self.cur_scan_msg_time) / 2. + self.cur_scan_msg_time people_tracked_msg.header.stamp = save_time self.savebag.write('/people_tracked', people_tracked_msg, save_time) # save rviz markers marker_id = 0 for person in people_tracked_msg.people: self.tracking_started = True if person.id not in self.person_colours: self.person_colours[person.id] = (random.random(), random.random(), random.random()) marker = Marker() marker.header.frame_id = people_tracked_msg.header.frame_id marker.header.stamp = save_time marker.ns = "playback_and_record_tracked" marker.color.r = self.person_colours[person.id][0] marker.color.g = self.person_colours[person.id][1] marker.color.b = self.person_colours[person.id][2] marker.color.a = 1 marker.pose.position.x = person.pose.position.x marker.pose.position.y = person.pose.position.y for i in xrange( 2 ): # publish two markers per person: one for body and one for head marker.id = marker_id marker_id += 1 if i == 0: # cylinder for body shape marker.type = Marker.CYLINDER marker.scale.x = 0.15 marker.scale.y = 0.15 marker.scale.z = 0.6 marker.pose.position.z = 0.6 else: # sphere for head shape marker.type = Marker.SPHERE marker.scale.x = 0.15 marker.scale.y = 0.15 marker.scale.z = 0.15 marker.pose.position.z = 0.975 self.savebag.write('/visualization_marker', marker, save_time) # Text showing person's ID number marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 1.0 marker.color.a = 1.0 marker.id = marker_id marker_id += 1 marker.type = Marker.TEXT_VIEW_FACING marker.text = str(person.id) marker.scale.z = 0.2 marker.pose.position.z = 1.3 self.savebag.write('/visualization_marker', marker, save_time) # Clear previously published people markers for m_id in xrange(marker_id, self.prev_person_marker_id): marker = Marker() marker.header.stamp = people_tracked_msg.header.stamp marker.header.frame_id = people_tracked_msg.header.frame_id marker.ns = "playback_and_record_tracked" marker.id = m_id marker.action = marker.DELETE self.savebag.write('/visualization_marker', marker, save_time) self.prev_person_marker_id = marker_id self.nextScanMsg()
def main (): SUFFIX = '' ONE_BY_ONE = False # Whether to visualize normals VIS_NORMS = True rospy.init_node ('visualize_contacts', anonymous=True) vis_pub = rospy.Publisher ('visualization_marker', Marker, queue_size=5) vis_arr_pub = rospy.Publisher ('visualization_marker_array', MarkerArray, queue_size=5) objs = ConfigReadYAML.read_scene_paths () # List of strings obj_names = objs [ConfigReadYAML.NAME_IDX] # For marker color based on grasp quality ENERGY_MIN = -1.8 #0.7 #0.4 ENERGY_MAX = -0.8 #1.6 #0.6 # Number of segments to calculate colormap N_COLOR_SEG = 10 #SEG_WIDTH = (ENERGY_MAX - ENERGY_MIN) / N_COLOR_SEG mesh_dir = 'package://grasp_collection/graspit_input/models/objects/dexnet/' # Marker size in meters CONTACT_SIZE = 0.003 # Length of normal vector NORM_LEN = 0.01 terminate = False # Loop through each object for o_i in range (len (obj_names)): # Delete all markers so next round starts clean del_mkr = Marker () create_marker (Marker.SPHERE_LIST, 'contacts', '/world', 0, 0, 0, 0, 0, 0, 0, 0.5, CONTACT_SIZE, CONTACT_SIZE, CONTACT_SIZE, del_mkr, duration=0) del_mkr.action = Marker.DELETEALL vis_pub.publish (del_mkr) # File name of world XML, one file per object world_fname = os.path.join (world_subdir, obj_names [o_i]) # A set of contacts per object mesh_mkr = Marker () create_marker (Marker.MESH_RESOURCE, 'mesh', '/world', 0, 0, 0, 0, 1, 1, 1, 0.5, 1, 1, 1, mesh_mkr, duration=0) # OBJ file path mesh_mkr.mesh_resource = mesh_dir + objects [o_i] # nGrasps x 3 contacts_m, normals_m = GraspIO.read_contacts (os.path.basename ( world_fname), SUFFIX) # Contacts meta tells how many contacts per grasp cmeta = GraspIO.read_contact_meta (os.path.basename (world_fname), SUFFIX) # List of nGrasps elts energies = GraspIO.read_energies (os.path.basename (world_fname), ENERGY_ABBREV, SUFFIX) print ('Energy min: %g, max: %g, mean: %g, median: %g' % (np.min (energies), np.max (energies), np.mean (energies), np.median (energies))) # A set of contacts per object o_contacts_mkr = Marker () create_marker (Marker.SPHERE_LIST, 'contacts', '/world', 0, 0, 0, 0, 0, 0, 0, 0.5, CONTACT_SIZE, CONTACT_SIZE, CONTACT_SIZE, o_contacts_mkr, duration=0) o_normals_arr = MarkerArray () # Contact index for current grasp ct_start_i = 0 # Loop through each grasp for g_i in range (len (energies)): # A set of contacts per object contacts_mkr = Marker () create_marker (Marker.SPHERE_LIST, 'contacts', '/world', 0, 0, 0, 0, 0, 0, 0, 0.5, CONTACT_SIZE, CONTACT_SIZE, CONTACT_SIZE, contacts_mkr, duration=0) normals_arr = MarkerArray () # Energies are negative. Use absolute value #fraction = (abs (energies [g_i]) - ENERGY_MIN) / (ENERGY_MAX - ENERGY_MIN) fraction = 1 - (energies [g_i] - ENERGY_MIN) / (ENERGY_MAX - ENERGY_MIN) color_tuple = mpl_color (fraction, N_COLOR_SEG, colormap_name='jet') print ('energy: %g, fraction: %g' % (energies [g_i], fraction)) color = ColorRGBA () color.r = color_tuple [0] color.g = color_tuple [1] color.b = color_tuple [2] color.a = 0.8 # Loop through each contact point x y z in this grasp for p_i in range (ct_start_i, ct_start_i + cmeta [g_i]): # Contact point pt = Point () pt.x = contacts_m [p_i, 0] pt.y = contacts_m [p_i, 1] pt.z = contacts_m [p_i, 2] contacts_mkr.points.append (pt) contacts_mkr.colors.append (color) o_contacts_mkr.points.append (pt) o_contacts_mkr.colors.append (color) # Contact normal vector norm_vec = normals_m [p_i, :] - contacts_m [p_i, :] norm_vec /= np.linalg.norm (norm_vec) norm_vec *= NORM_LEN normals_mkr = Marker () create_marker (Marker.ARROW, 'normals', '/world', p_i, # sx: shaft diameter, sy: arrowhead diameter 0, 0, 0, color.r, color.g, color.b, color.a, 0.001, 0.002, 0, normals_mkr, duration=0) # Start at contact point norm_s = Point () norm_s.x = contacts_m [p_i, 0] norm_s.y = contacts_m [p_i, 1] norm_s.z = contacts_m [p_i, 2] normals_mkr.points.append (norm_s) # End at endpoint of vector norm_t = Point () norm_t.x = contacts_m [p_i, 0] + norm_vec [0] norm_t.y = contacts_m [p_i, 1] + norm_vec [1] norm_t.z = contacts_m [p_i, 2] + norm_vec [2] normals_mkr.points.append (norm_t) normals_arr.markers.append (normals_mkr) o_normals_arr.markers.append (normals_mkr) #print ('Normal length: %g' % (np.linalg.norm (normals_m [p_i, :] - contacts_m [p_i, :]))) # Update for next grasp ct_start_i += cmeta [g_i] if ONE_BY_ONE: for i in range (5): vis_pub.publish (mesh_mkr) vis_pub.publish (contacts_mkr) if VIS_NORMS: vis_arr_pub.publish (normals_arr) rospy.sleep (0.5) uinput = raw_input ('Press enter to go to next grasp, s to skip to next object, q to quit: ') if uinput.lower () == 's': break elif uinput.lower () == 'q': terminate = True break # Delete all markers so next round starts clean del_mkr = Marker () create_marker (Marker.SPHERE_LIST, 'contacts', '/world', 0, 0, 0, 0, 0, 0, 0, 0.5, CONTACT_SIZE, CONTACT_SIZE, CONTACT_SIZE, del_mkr, duration=0) del_mkr.action = Marker.DELETEALL vis_pub.publish (del_mkr) #print (o_contacts_mkr.points) #print (o_contacts_mkr.colors) if terminate: break # Cumulative per-object contacts for i in range (5): vis_pub.publish (mesh_mkr) vis_pub.publish (o_contacts_mkr) if VIS_NORMS: vis_arr_pub.publish (o_normals_arr) rospy.sleep (0.5) uinput = raw_input ('Press enter to go to next object, q to quit: ') if uinput.lower () == 'q': break
def det_result_cb(self, msg): dets_list = None info_list = None for idx, det in enumerate(msg.dets_list): if dets_list is None: dets_list = np.array([det.x, det.y, det.radius], dtype=np.float32) info_list = np.array([det.confidence, det.class_id], dtype=np.float32) else: dets_list = np.vstack([dets_list, [det.x, det.y, det.radius]]) info_list = np.vstack( [info_list, [det.confidence, det.class_id]]) # if len(dets_list.shape) == 1: dets_list = np.expand_dims(dets_list, axis=0) # if len(info_list.shape) == 1: info_list = np.expand_dims(info_list, axis=0) # if dets_list.shape[1] == 0: # # If there is no detection, just pack the laserscan topic for localmap creation # trk3d_array = Trk3DArray() # trk3d_array.header.frame_id = msg.header.frame_id # trk3d_array.header.stamp = rospy.Time.now() # trk3d_array.scan = msg.scan # self.pub_trk3d_result.publish(trk3d_array) # return # rospy.loginfo('number of alive trks: {}'.format(len(self.mot_tracker.trackers))) if dets_list is not None: if len(dets_list.shape) == 1: dets_list = np.expand_dims(dets_list, axis=0) if len(info_list.shape) == 1: info_list = np.expand_dims(info_list, axis=0) dets_all = {'dets': dets_list, 'info': info_list} trackers = self.mot_tracker.update(dets_all) else: # if no detection in a sequence trackers = self.mot_tracker.update_with_no_dets() # Skip the visualization at first callback if self.last_time is None: self.last_time = rospy.Time.now() # self.last_time = time.time() return # saving results, loop over each tracklet marker_array = MarkerArray() trk3d_array = Trk3DArray() time_now = rospy.Time.now() delta_t = (time_now - self.last_time).to_sec() if delta_t < 0.1: delta_t = 0.1 # small_time = (0.1 - delta_t) * 0.92 # delta_t += small_time # rospy.sleep(small_time) # sys.stdout.write("{:.4f} s \r".format(delta_t)) # sys.stdout.flush() self.last_time = time_now for idx, d in enumerate(trackers): ''' x, y, r, vx, vy, id, confidence, class_id ''' d_now = np.sqrt(d[0]**2 + d[1]**2) #print(d[0]) #print(d[1]) # vx, vy = np.array([d[3], d[4]]) / (time.time() - self.last_time) vx, vy = np.array( [d[3], d[4]] ) / delta_t #- np.array([self.ego_velocity.x, self.ego_velocity.y])-np.array([d_now*math.sin(self.ego_theta.z),d_now*math.cos(self.ego_theta.z)])- speed = np.sqrt( vx**2 + vy** 2) # Note: reconstruct speed by multipling the sampling rate yaw = np.arctan2(vy, vx) if (speed > 2): speed = 2 if (speed > 0.5): dangerous = 2 * speed / ( 1 + np.exp(0.3 * np.sqrt(d[0]**2 + d[1]**2))) #print("peolple dangerous:",dangerous) else: dangerous = 2 * 0.5 / ( 1 + np.exp(0.3 * np.sqrt(d[0]**2 + d[1]**2))) #print("peolple dangerous:",dangerous) #print(speed) # Custom ROS message trk3d_msg = Trk3D() trk3d_msg.x, trk3d_msg.y = d[0], d[1] # print("peolple x:",trk3d_msg.x) # print("peolple y:",trk3d_msg.y) trk3d_msg.radius = d[2] trk3d_msg.vx, trk3d_msg.vy = vx, vy trk3d_msg.yaw = yaw trk3d_msg.confidence = d[6] trk3d_msg.class_id = int(d[7]) trk3d_msg.dangerous = dangerous trk3d_array.trks_list.append(trk3d_msg) #print("peolple speed:",speed) # Visualization marker = Marker() marker.header.frame_id = 'odom_filtered' marker.header.stamp = rospy.Time() marker.ns = 'object' marker.id = idx marker.lifetime = rospy.Duration( MARKER_LIFETIME ) #The lifetime of the bounding-box, you can modify it according to the power of your machine. marker.type = Marker.CYLINDER marker.action = Marker.ADD marker.scale.x = d[2] * 2 marker.scale.y = d[2] * 2 marker.scale.z = 1.0 marker.color.b = 1.0 marker.color.a = 0.5 #The alpha of the bounding-box marker.pose.position.x = d[0] marker.pose.position.y = d[1] marker.pose.position.z = 0.5 q = euler_to_quaternion(0, 0, yaw) marker.pose.orientation.x = q[0] marker.pose.orientation.y = q[1] marker.pose.orientation.z = q[2] marker.pose.orientation.w = q[3] marker_array.markers.append(marker) # Show tracking ID str_marker = Marker() str_marker.header.frame_id = 'odom_filtered' str_marker.header.stamp = rospy.Time() str_marker.ns = 'text' str_marker.id = idx str_marker.scale.z = 0.4 #The size of the text str_marker.color.b = 1.0 str_marker.color.g = 1.0 str_marker.color.r = 1.0 str_marker.color.a = 1.0 str_marker.pose.position.x = d[0] str_marker.pose.position.y = d[1] str_marker.pose.position.z = 0.5 str_marker.lifetime = rospy.Duration(MARKER_LIFETIME) str_marker.type = Marker.TEXT_VIEW_FACING str_marker.action = Marker.ADD str_marker.text = "{}".format(int(d[5])) # str(d[5]) marker_array.markers.append(str_marker) # Show direction arrow_marker = copy.deepcopy(marker) arrow_marker.type = Marker.ARROW arrow_marker.ns = 'direction' arrow_marker.scale.x = 2 * (speed / 1.5) arrow_marker.scale.y = 0.2 arrow_marker.scale.z = 0.2 marker_array.markers.append(arrow_marker) self.pub_trk3d_vis.publish(marker_array) trk3d_array.header.frame_id = msg.header.frame_id trk3d_array.header.stamp = rospy.Time().now() trk3d_array.pointcloud = msg.pointcloud self.pub_trk3d_result.publish(trk3d_array)
def nextScanMsg(self): self.mutex.acquire() # TODO necessary? # First display and save <next_scan_msg> # It is now referred to as the cur_scan_msg # And we'll iterate through the rosbag until we reach the next_scan_msg if self.next_scan_msg is not None: topic, msg, time = self.next_scan_msg self.cur_scan_msg_time = time self.next_scan_msg = None if self.savebag_filename is not None: self.savebag.write(topic, msg, time) self.tic = timeit.default_timer() self.laser_pub.publish(msg) if self.bag_empty: # Shut down and close rosbags safetly rospy.loginfo("End of readbag file, shutting down") rospy.signal_shutdown("End of readbag file, shutting down") marker_id = 0 topic = None while topic != self.scan_topic and not self.bag_empty: try: # Get the next message from the rosbag topic, msg, time = next(self.msg_gen) if topic == self.scan_topic: self.next_scan_msg = (topic, msg, time) self.next_scan_msg_time = time else: # Save message to savebag if self.savebag_filename is not None: self.savebag.write(topic, msg, time) # Publish them if topic == "/tf": self.tf_pub.publish(msg) elif topic == "/visualization_marker": marker = Marker() marker.header = msg.header marker.ns = msg.ns marker.id = msg.id marker.type = msg.type marker.action = msg.action marker.pose = msg.pose marker.scale = msg.scale marker.color = msg.color marker.lifetime = msg.lifetime marker.frame_locked = msg.frame_locked marker.points = msg.points marker.colors = msg.colors marker.text = msg.text marker.mesh_resource = msg.mesh_resource marker.mesh_use_embedded_materials = msg.mesh_use_embedded_materials self.marker_pub.publish(marker) except StopIteration: # Passed the last message in the rosbag self.bag_empty = True # Clear previously published people markers if self.prev_marker_id > marker_id: for m_id in xrange(marker_id, self.prev_marker_id): marker = Marker() marker.header.frame_id = self.fixed_frame marker.ns = "clear_mot_display" marker.id = m_id marker.action = marker.DELETE self.marker_pub.publish(marker) self.prev_marker_id = marker_id self.mutex.release()
[[0, 0, 0, px], [0, 0, 0, py], [0, 0, 0, pz], [0, 0, 0, 0]]) T_base_d = T_base_tl6 * T_tl6_cam * T_cam_opt * T_opt_d door_points = door_pos_points base_door_points = [] for i in range(0, door_points.shape[0]): p = numpy.matrix([[door_points[i, 0]], [door_points[i, 1]], [0.0], [1.0]]) base_door_points.append(T_base_d * p) marker = Marker() marker.header.frame_id = "/tl_base" marker.type = marker.TRIANGLE_LIST marker.action = marker.ADD marker.scale.x = 1.0 marker.scale.y = 1.0 marker.scale.z = 1.0 marker.color.a = 1.0 marker.color.g = 1.0 marker.pose.orientation.w = 1.0 p = Point() p.x = base_door_points[0][0] p.y = base_door_points[0][1] p.z = base_door_points[0][2] p1 = Point() p1.x = base_door_points[1][0] p1.y = base_door_points[1][1] p1.z = base_door_points[1][2]
markerArray = create_delta_simulation() initializedMarkerArray = set_initial_delta_pose(markerArray) idcount = 100 pointwidth = 0.005 with open('valid_points_generated.txt', 'rb') as f: reader = csv.reader(f) for row in reader: if idcount < 50000: if len(row) > 0: testpoint = Marker() testpoint.header.frame_id = "/map" testpoint.type = testpoint.SPHERE testpoint.action = testpoint.ADD testpoint.scale.x = pointwidth testpoint.scale.y = pointwidth testpoint.scale.z = pointwidth testpoint.color.a = 1.0 testpoint.color.r = 1.0 testpoint.color.g = 1.0 testpoint.color.b = 1.0 testpoint.pose.orientation.w = 1.0 testpoint.pose.position.x = float(row[0]) testpoint.pose.position.y = float(row[1]) testpoint.pose.position.z = float(row[2]) testpoint.id = idcount idcount += 1 initializedMarkerArray.markers.append(testpoint)
def get_markers(self): """ Return a ROS marker array message structure with an sphere marker to represent the plume source, the bounding box and an arrow marker to show the direction of the current velocity if it's norm is greater than zero. """ if self._pnts is None: return None marker_array = MarkerArray() # Generate marker for the source source_marker = Marker() source_marker.header.stamp = rospy.Time.now() source_marker.header.frame_id = 'world' source_marker.ns = 'plume' source_marker.id = 0 source_marker.type = Marker.SPHERE source_marker.action = Marker.ADD source_marker.color.r = 0.35 source_marker.color.g = 0.45 source_marker.color.b = 0.89 source_marker.color.a = 1.0 source_marker.scale.x = 1.0 source_marker.scale.y = 1.0 source_marker.scale.z = 1.0 source_marker.pose.position.x = self._source_pos[0] source_marker.pose.position.y = self._source_pos[1] source_marker.pose.position.z = self._source_pos[2] source_marker.pose.orientation.x = 0 source_marker.pose.orientation.y = 0 source_marker.pose.orientation.z = 0 source_marker.pose.orientation.w = 1 marker_array.markers.append(source_marker) # Creating the marker for the bounding box limits where the plume # particles are generated limits_marker = Marker() limits_marker.header.stamp = rospy.Time.now() limits_marker.header.frame_id = 'world' limits_marker.ns = 'plume' limits_marker.id = 1 limits_marker.type = Marker.CUBE limits_marker.action = Marker.ADD limits_marker.color.r = 0.1 limits_marker.color.g = 0.1 limits_marker.color.b = 0.1 limits_marker.color.a = 0.3 limits_marker.scale.x = self._x_lim[1] - self._x_lim[0] limits_marker.scale.y = self._y_lim[1] - self._y_lim[0] limits_marker.scale.z = self._z_lim[1] - self._z_lim[0] limits_marker.pose.position.x = self._x_lim[0] + (self._x_lim[1] - self._x_lim[0]) / 2 limits_marker.pose.position.y = self._y_lim[0] + (self._y_lim[1] - self._y_lim[0]) / 2 limits_marker.pose.position.z = self._z_lim[0] + (self._z_lim[1] - self._z_lim[0]) / 2 limits_marker.pose.orientation.x = 0 limits_marker.pose.orientation.y = 0 limits_marker.pose.orientation.z = 0 limits_marker.pose.orientation.w = 0 marker_array.markers.append(limits_marker) # Creating marker for the current velocity vector cur_vel_marker = Marker() cur_vel_marker.header.stamp = rospy.Time.now() cur_vel_marker.header.frame_id = 'world' cur_vel_marker.id = 1 cur_vel_marker.type = Marker.ARROW if np.linalg.norm(self._current_vel) > 0: cur_vel_marker.action = Marker.ADD # Calculating the pitch and yaw angles for the current velocity # vector yaw = np.arctan2(self._current_vel[1], self._current_vel[0]) pitch = np.arctan2(self._current_vel[2], np.sqrt(self._current_vel[0]**2 + self._current_vel[1]**2)) qt = quaternion_from_euler(0, -pitch, yaw) cur_vel_marker.pose.position.x = self._source_pos[0] cur_vel_marker.pose.position.y = self._source_pos[1] cur_vel_marker.pose.position.z = self._source_pos[2] - 0.8 cur_vel_marker.pose.orientation.x = qt[0] cur_vel_marker.pose.orientation.y = qt[1] cur_vel_marker.pose.orientation.z = qt[2] cur_vel_marker.pose.orientation.w = qt[3] cur_vel_marker.scale.x = 1.0 cur_vel_marker.scale.y = 0.1 cur_vel_marker.scale.z = 0.1 cur_vel_marker.color.a = 1.0 cur_vel_marker.color.r = 0.0 cur_vel_marker.color.g = 0.0 cur_vel_marker.color.b = 1.0 else: cur_vel_marker.action = Marker.DELETE marker_array.markers.append(cur_vel_marker) return marker_array
def talker(): rospy.init_node('referee', anonymous=True) listener = tf.TransformListener() broadcaster = tf.TransformBroadcaster() global rate rate = rospy.Rate(2) # 10hz rate.sleep() hunting_distance = rospy.get_param('/hunting_distance') global teamA teamA = rospy.get_param('/red') global teamB teamB = rospy.get_param('/green') global teamC teamC = rospy.get_param('/blue') rospy.Timer(rospy.Duration(0.1), timerCallback, oneshot=False) rospy.Timer(rospy.Duration(game_duration), gameEndCallback, oneshot=True) #rospy.Timer(rospy.Duration(5), gameQueryCallback, oneshot=False) game_start = rospy.get_time() while not rospy.is_shutdown(): #print killed tic = rospy.Time.now() for i in killed: d = tic - i[1] if d.to_sec() > 10: rospy.logwarn("Ressuscitating %s", i[0]) killed.remove(i) broadcaster.sendTransform( (random.random() * 10 - 5, random.random() * 10 - 5, 0), tf.transformations.quaternion_from_euler(0, 0, 0), tic, i[0], "/map") #print killed #rospy.loginfo("Checking ...") to_be_killed_A = [] to_be_killed_B = [] to_be_killed_C = [] out_of_arena_A = [] out_of_arena_B = [] out_of_arena_C = [] max_distance_from_center_of_arena = 8 #Checking if players don't stray from the arena for player in teamA: #check if hunter is killed result = [item for item in killed if item[0] == player] if not result: try: (trans, rot) = listener.lookupTransform("/map", player, rospy.Time(0)) distance = math.sqrt(trans[0] * trans[0] + trans[1] * trans[1]) #rospy.loginfo("D from %s to %s is %f", hunter, prey, distance) if distance > max_distance_from_center_of_arena: out_of_arena_A.append(player) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): t = 1 #rospy.loginfo("Referee: tf error") for player in teamB: #check if hunter is killed result = [item for item in killed if item[0] == player] if not result: try: (trans, rot) = listener.lookupTransform("/map", player, rospy.Time(0)) distance = math.sqrt(trans[0] * trans[0] + trans[1] * trans[1]) #rospy.loginfo("D from %s to %s is %f", hunter, prey, distance) if distance > max_distance_from_center_of_arena: out_of_arena_B.append(player) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): t = 1 #rospy.loginfo("Referee: tf error") for player in teamC: #check if hunter is killed result = [item for item in killed if item[0] == player] if not result: try: (trans, rot) = listener.lookupTransform("/map", player, rospy.Time(0)) distance = math.sqrt(trans[0] * trans[0] + trans[1] * trans[1]) #rospy.loginfo("D from %s to %s is %f", hunter, prey, distance) if distance > max_distance_from_center_of_arena: out_of_arena_C.append(player) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): t = 1 #rospy.loginfo("Referee: tf error") rospy.loginfo("to_be_killed is %s", str(to_be_killed_B)) #Check if anyone is hunted #Team A hunting team B for hunter in teamA: #check if hunter is killed result = [item for item in killed if item[0] == hunter] if not result: for prey in teamB: try: (trans, rot) = listener.lookupTransform( hunter, prey, rospy.Time(0)) distance = math.sqrt(trans[0] * trans[0] + trans[1] * trans[1]) #rospy.loginfo("D from %s to %s is %f", hunter, prey, distance) if distance < hunting_distance: to_be_killed_A.append(prey) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): t = 1 #rospy.loginfo("Referee: tf error") #Team A hunting team B for hunter in teamB: #check if hunter is killed result = [item for item in killed if item[0] == hunter] if not result: for prey in teamC: try: (trans, rot) = listener.lookupTransform( hunter, prey, rospy.Time(0)) distance = math.sqrt(trans[0] * trans[0] + trans[1] * trans[1]) #rospy.loginfo("D from %s to %s is %f", hunter, prey, distance) if distance < hunting_distance: to_be_killed_B.append(prey) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): t = 1 #rospy.loginfo("Referee: tf error") #Team C hunting team A for hunter in teamC: #check if hunter is killed result = [item for item in killed if item[0] == hunter] if not result: for prey in teamA: try: (trans, rot) = listener.lookupTransform( hunter, prey, rospy.Time(0)) distance = math.sqrt(trans[0] * trans[0] + trans[1] * trans[1]) #rospy.loginfo("D from %s to %s is %f", hunter, prey, distance) if distance < hunting_distance: to_be_killed_C.append(prey) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): t = 1 #rospy.loginfo("Referee: tf error") ma_killed = MarkerArray() kill_time = rospy.Time.now() for tbk in to_be_killed_A: #rospy.logwarn("%s is to be killed by Team A", tbk) #Check if tbk is in the list of killed players found = False for item in killed: if item[0] == tbk: found = True if found == False: killed.append((tbk, kill_time)) rospy.logwarn("Red hunted %s", tbk) score[0] = score[0] + positive_score score[1] = score[1] + negative_score s = String() s.data = tbk #pub_killer.publish(s) mk1 = Marker() mk1.header.frame_id = "/map" (trans, rot) = listener.lookupTransform("/map", tbk, rospy.Time(0)) mk1.pose.position.x = trans[0] mk1.pose.position.y = trans[1] mk1.type = mk1.TEXT_VIEW_FACING mk1.action = mk1.ADD mk1.id = 0 mk1.ns = tbk mk1.scale.z = 0.4 mk1.color.a = 1.0 mk1.text = tbk mk1.lifetime = rospy.Duration.from_sec(5) mk1.frame_locked = 0 ma_killed.markers.append(mk1) broadcaster.sendTransform( (-100, -100, 0), tf.transformations.quaternion_from_euler(0, 0, 0), kill_time, tbk, "/map") else: t = 1 #rospy.logwarn("%s was already hunted", tbk) for tbk in to_be_killed_B: #rospy.logwarn("%s is to be killed by Team B", tbk) #Check if tbk is in the list of killed players found = False for item in killed: if item[0] == tbk: found = True if found == False: killed.append((tbk, kill_time)) rospy.logwarn("Green hunted %s", tbk) score[1] = score[1] + positive_score score[2] = score[2] + negative_score s = String() s.data = tbk #pub_killer.publish(s) mk1 = Marker() mk1.header.frame_id = "/map" (trans, rot) = listener.lookupTransform("/map", tbk, rospy.Time(0)) mk1.pose.position.x = trans[0] mk1.pose.position.y = trans[1] mk1.type = mk1.TEXT_VIEW_FACING mk1.action = mk1.ADD mk1.id = 0 mk1.ns = tbk mk1.scale.z = 0.4 mk1.color.a = 1.0 mk1.text = tbk mk1.lifetime = rospy.Duration.from_sec(5) mk1.frame_locked = 0 ma_killed.markers.append(mk1) broadcaster.sendTransform( (100, -100, 0), tf.transformations.quaternion_from_euler(0, 0, 0), kill_time, tbk, "/map") else: t = 1 #rospy.logwarn("%s was already hunted", tbk) for tbk in to_be_killed_C: #rospy.logwarn("%s is to be killed by Team C", tbk) #Check if tbk is in the list of killed players found = False for item in killed: if item[0] == tbk: found = True if found == False: killed.append((tbk, kill_time)) rospy.logwarn("Blue hunted %s", tbk) score[2] = score[2] + positive_score score[0] = score[0] + negative_score s = String() s.data = tbk #pub_killer.publish(s) mk1 = Marker() mk1.header.frame_id = "/map" (trans, rot) = listener.lookupTransform("/map", tbk, rospy.Time(0)) mk1.pose.position.x = trans[0] mk1.pose.position.y = trans[1] mk1.type = mk1.TEXT_VIEW_FACING mk1.action = mk1.ADD mk1.id = 0 mk1.ns = tbk mk1.scale.z = 0.4 mk1.color.a = 1.0 mk1.text = tbk mk1.lifetime = rospy.Duration.from_sec(5) mk1.frame_locked = 0 ma_killed.markers.append(mk1) broadcaster.sendTransform( (100, 100, 0), tf.transformations.quaternion_from_euler(0, 0, 0), kill_time, tbk, "/map") else: t = 1 #rospy.logwarn("%s was already hunted", tbk) #-------------------------------- #Killing because of stray from arena #-------------------------------- for tbk in out_of_arena_A: #rospy.logwarn("%s is to be killed by Team A", tbk) #Check if tbk is in the list of killed players found = False for item in killed: if item[0] == tbk: found = True if found == False: killed.append((tbk, kill_time)) rospy.logwarn("Red hunted %s", tbk) score[0] = score[0] + negative_score s = String() s.data = tbk #pub_killer.publish(s) mk1 = Marker() mk1.header.frame_id = "/map" (trans, rot) = listener.lookupTransform("/map", tbk, rospy.Time(0)) mk1.pose.position.x = trans[0] mk1.pose.position.y = trans[1] mk1.type = mk1.TEXT_VIEW_FACING mk1.action = mk1.ADD mk1.id = 0 mk1.ns = tbk mk1.scale.z = 0.4 mk1.color.a = 1.0 mk1.text = "Queres fugir " + tbk + "?" mk1.lifetime = rospy.Duration.from_sec(5) mk1.frame_locked = 0 ma_killed.markers.append(mk1) broadcaster.sendTransform( (-100, -100, 0), tf.transformations.quaternion_from_euler(0, 0, 0), kill_time, tbk, "/map") else: t = 1 #rospy.logwarn("%s was already hunted", tbk) for tbk in out_of_arena_B: #rospy.logwarn("%s is to be killed by Team B", tbk) #Check if tbk is in the list of killed players found = False for item in killed: if item[0] == tbk: found = True if found == False: killed.append((tbk, kill_time)) rospy.logwarn("Green hunted %s", tbk) score[1] = score[1] + negative_score s = String() s.data = tbk #pub_killer.publish(s) mk1 = Marker() mk1.header.frame_id = "/map" (trans, rot) = listener.lookupTransform("/map", tbk, rospy.Time(0)) mk1.pose.position.x = trans[0] mk1.pose.position.y = trans[1] mk1.type = mk1.TEXT_VIEW_FACING mk1.action = mk1.ADD mk1.id = 0 mk1.ns = tbk mk1.scale.z = 0.4 mk1.color.a = 1.0 mk1.text = "Queres fugir " + tbk + "?" mk1.lifetime = rospy.Duration.from_sec(5) mk1.frame_locked = 0 ma_killed.markers.append(mk1) broadcaster.sendTransform( (100, -100, 0), tf.transformations.quaternion_from_euler(0, 0, 0), kill_time, tbk, "/map") else: t = 1 #rospy.logwarn("%s was already hunted", tbk) for tbk in out_of_arena_C: #rospy.logwarn("%s is to be killed by Team C", tbk) #Check if tbk is in the list of killed players found = False for item in killed: if item[0] == tbk: found = True if found == False: killed.append((tbk, kill_time)) rospy.logwarn("Blue hunted %s", tbk) score[2] = score[2] + negative_score s = String() s.data = tbk #pub_killer.publish(s) mk1 = Marker() mk1.header.frame_id = "/map" (trans, rot) = listener.lookupTransform("/map", tbk, rospy.Time(0)) mk1.pose.position.x = trans[0] mk1.pose.position.y = trans[1] mk1.type = mk1.TEXT_VIEW_FACING mk1.action = mk1.ADD mk1.id = 0 mk1.ns = tbk mk1.scale.z = 0.4 mk1.color.a = 1.0 mk1.text = "Queres fugir " + tbk + "?" mk1.lifetime = rospy.Duration.from_sec(5) mk1.frame_locked = 0 ma_killed.markers.append(mk1) broadcaster.sendTransform( (100, 100, 0), tf.transformations.quaternion_from_euler(0, 0, 0), kill_time, tbk, "/map") else: t = 1 #rospy.logwarn("%s was already hunted", tbk) if ma_killed.markers: pub_rip.publish(ma_killed) #print killed #rospy.logwarn("Team A: %s hunted %s", hunter, prey) ##os.system('rosnode kill ' + prey) #score[0] = score[0] + positive_score #score[1] = score[1] + negative_score #s = String() #s.data = prey #pub_killer.publish(s) ma = MarkerArray() m1 = Marker() m1.header.frame_id = "/map" m1.type = m1.TEXT_VIEW_FACING m1.action = m1.ADD m1.id = 0 m1.scale.x = 0.2 m1.scale.y = 0.2 m1.scale.z = 0.6 m1.color.a = 1.0 m1.color.r = 1.0 m1.color.g = 0.0 m1.color.b = 0.0 m1.text = "R=" + str(score[0]) m1.pose.position.x = -5.0 m1.pose.position.y = 5.2 m1.pose.position.z = 0 ma.markers.append(m1) m2 = Marker() m2.header.frame_id = "/map" m2.type = m2.TEXT_VIEW_FACING m2.action = m2.ADD m2.id = 1 m2.scale.x = 0.2 m2.scale.y = 0.2 m2.scale.z = 0.6 m2.color.a = 1.0 m2.color.r = 0.0 m2.color.g = 1.0 m2.color.b = 0.0 m2.text = "G=" + str(score[1]) m2.pose.position.x = -3.0 m2.pose.position.y = 5.2 m2.pose.position.z = 0 ma.markers.append(m2) m3 = Marker() m3.header.frame_id = "/map" m3.type = m3.TEXT_VIEW_FACING m3.action = m3.ADD m3.id = 2 m3.scale.x = 0.2 m3.scale.y = 0.2 m3.scale.z = 0.6 m3.color.a = 1.0 m3.color.r = 0.0 m3.color.g = 0.0 m3.color.b = 1.0 m3.text = "B=" + str(score[2]) m3.pose.position.x = -1.0 m3.pose.position.y = 5.2 m3.pose.position.z = 0 ma.markers.append(m3) m4 = Marker() m4.header.frame_id = "/map" m4.type = m4.TEXT_VIEW_FACING m4.action = m4.ADD m4.id = 3 m4.scale.x = 0.2 m4.scale.y = 0.2 m4.scale.z = 0.6 m4.color.a = 1.0 m4.color.r = 0.0 m4.color.g = 0.0 m4.color.b = 1.0 #,c rospy.Time( time_now = rospy.get_time() m4.text = "Time " + str(format(time_now - game_start, '.2f')) + " of " + str(game_duration) m4.pose.position.x = 3.5 m4.pose.position.y = 5.2 m4.pose.position.z = 0 ma.markers.append(m4) pub_score.publish(ma) rate.sleep()
def publishState(self): msg = Odometry() if self.time is None: return # Construct header msg.header.seq = self.seq # sequence ID self.seq += 1 # increment sequence ID for next frame msg.header.stamp = self.time # time stamp msg.header.frame_id = 'odom' # reference frame msg.child_frame_id = 'base_link' # Construct content current_pose = np.array( self.KF.getStateMean()[0:dimR_]) # latest (x, y, yaw) rospy.loginfo("Current pose {}".format(current_pose)) rospy.loginfo("Current pose z {}".format(current_pose[2])) msg.pose.pose.position.x = current_pose[0] msg.pose.pose.position.y = current_pose[1] msg.pose.pose.position.z = 0.0 # quat = transformations.quaternion_from_euler(0.0, 0.0, current_pose[2]) r = R.from_euler('xyz', [0, 0, current_pose[2]]) quat = r.as_quat() msg.pose.pose.orientation.x = quat[0] msg.pose.pose.orientation.y = quat[1] msg.pose.pose.orientation.z = quat[2] msg.pose.pose.orientation.w = quat[3] current_covariance = np.array( self.KF.getStateCovariance()[0:dimR_, 0:dimR_]) # latest 3x3 covariance cov = np.zeros((36, 1), dtype=np.float) cov[0] = current_covariance[0, 0] cov[1] = current_covariance[0, 1] cov[5] = current_covariance[0, 2] cov[6] = current_covariance[1, 0] cov[7] = current_covariance[1, 1] cov[11] = current_covariance[1, 2] cov[30] = current_covariance[2, 0] cov[31] = current_covariance[2, 1] cov[35] = current_covariance[2, 2] msg.pose.covariance = np.ravel(cov) self.pub_odom.publish(msg) rospy.loginfo("State mean {}".format(self.KF.getStateMean())) rospy.loginfo("seen landmarks {}".format(seenLandmarks_)) # state_mean = np.array() Landmark_poses = self.KF.getStateMean()[dimR_:] print('landmark poses ', Landmark_poses) marker_array_msg = MarkerArray() for i in range(int(len(Landmark_poses) / 2)): marker = Marker() marker.header.frame_id = 'odom' marker.id = seenLandmarks_[i] marker.type = 3 marker.action = 0 marker.pose.position.x = Landmark_poses[2 * i] marker.pose.position.y = Landmark_poses[2 * i + 1] marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 marker.color.a = 1.0 marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.frame_locked = False marker.lifetime = rospy.Duration(0.1) marker_array_msg.markers.append(marker) self.pub_landmarks.publish(marker_array_msg)
def publishMarkers(self, objects, pruned_objects, bin_pose, camera_to_bin, bin_dimensions, header): # construct centroids marker array marker_array = MarkerArray() # clear old markers marker = Marker() marker.action = 3 # secret action to delete all markers marker_array.markers.append(marker) self.marker_publisher.publish(marker_array) marker_array.markers = [] # add centroid markers for index, object in enumerate(objects): marker = Marker() marker.header = header marker.type = marker.SPHERE marker.action = marker.ADD marker.scale.x = 0.02 marker.scale.y = 0.02 marker.scale.z = 0.02 marker.color.g = 1.0 marker.color.a = 0.5 marker.pose.position = object.centroid marker.ns = apc16delft_msgs.objects.objectTypeToString[ object.object.type] marker.id = len(marker_array.markers) marker_array.markers.append(marker) # add pruned centroid markers for index, object in enumerate(pruned_objects): marker = Marker() marker.header = header marker.type = marker.SPHERE marker.action = marker.ADD marker.scale.x = 0.02 marker.scale.y = 0.02 marker.scale.z = 0.02 marker.color.r = 1.0 marker.color.a = 0.5 marker.pose.position = object.centroid marker.ns = apc16delft_msgs.objects.objectTypeToString[ object.object.type] marker.id = len(marker_array.markers) marker_array.markers.append(marker) # add bin marker marker = Marker() marker.header = header marker.type = marker.CUBE marker.action = marker.ADD marker.scale.x = bin_dimensions[0] + self.bin_margin[0] * 2 marker.scale.y = bin_dimensions[1] + self.bin_margin[1] * 2 marker.scale.z = bin_dimensions[2] + self.bin_margin[2] * 2 marker.color.b = 1.0 marker.color.a = 0.2 marker.ns = "bin_marker" marker.id = len(marker_array.markers) # transform so that the marker is originating from the bin_pose position = np.append(np.array(bin_dimensions) * 0.5, 1) position = transformations.inverse_matrix(camera_to_bin).dot(position) marker.pose.position.x = position[0] marker.pose.position.y = position[1] marker.pose.position.z = position[2] marker.pose.orientation = bin_pose.orientation marker_array.markers.append(marker) self.marker_publisher.publish(marker_array)
def callback(self, cluster): human_cluster = np.zeros((len(cluster.candidates), 3)) human_cluster = map( lambda info: np.array( [info.centroid.x, info.centroid.y, info.centroid.z]), cluster.info) human_prob = map(lambda prob: prob.all, cluster.prob) thresh = 0.4 if len(cluster.candidates) > 0: try: clusters = hcluster.fclusterdata(human_cluster, thresh, criterion="distance") except ValueError: self.pos.x = 0.0 self.pos.y = 0.0 self.pos.z = 0.0 self.pos_pub.publish(self.pos) cluster_point = np.asarray( [0.0 for i in range(len(cluster.candidates))]) candidate_cluster = [] for i in range(len(clusters)): if human_prob[i] > 0.85: cluster_point[clusters[i] - 1] += human_prob[i] candidate_cluster.append(clusters[i]) centroids = [ np.zeros((1, 3)) for i in range(len(candidate_cluster)) ] nums = [0 for i in range(len(candidate_cluster))] for i in range(len(candidate_cluster)): for j in range(len(clusters)): if clusters[j] == candidate_cluster[i]: centroids[i] += human_cluster[j] nums[i] += 1 if len(nums) > 0: rospy.loginfo("human candidates : %d", len(nums)) centroids = [ centroids[i] / nums[i] for i in range(len(candidate_cluster)) ] markerArray = MarkerArray() people = Person() for i in range(len(centroids)): marker = Marker() marker.header.frame_id = "realsense_frame" marker.action = marker.ADD marker.type = marker.MESH_RESOURCE marker.mesh_resource = "package://object_recognizer/meshes/lovelive/" + self.name[ i] + ".dae" marker.mesh_use_embedded_materials = True marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 1.0 marker.scale.x = 0.08 marker.scale.y = 0.08 marker.scale.z = 0.08 marker.pose.position.x = centroids[i][0][0] marker.pose.position.y = centroids[i][0][1] + 0.8 marker.pose.position.z = centroids[i][0][2] q = tf.transformations.quaternion_from_euler(math.pi, 0, 0) marker.pose.orientation.x = q[0] marker.pose.orientation.y = q[1] marker.pose.orientation.z = q[2] marker.pose.orientation.w = q[3] markerArray.markers.append(marker) centroid = Vector3() centroid.x = centroids[i][0][0] centroid.y = centroids[i][0][1] centroid.z = centroids[i][0][2] people.centroids.append(centroid) for i in range(len(markerArray.markers)): markerArray.markers[i].id = i markerArray.markers[i].lifetime = rospy.Duration(1.0) #self.marker_pub.publish(markerArray) self.pos_pub.publish(people) else: rospy.loginfo("no cluster found") people = Person() centroid = Vector3() centroid.x = 0.0 centroid.y = 0.0 centroid.z = 0.0 people.centroids.append(centroid) self.pos_pub.publish(people) else: people = Person() centroid = Vector3() centroid.x = 0.0 centroid.y = 0.0 centroid.z = 0.0 people.centroids.append(centroid) self.pos_pub.publish(people)
def pubTF(self, timer): br = tf.TransformBroadcaster() marker_tmp = Marker() marker_tmp.header.frame_id = "world" marker_tmp.type = marker_tmp.CUBE_LIST marker_tmp.action = marker_tmp.ADD marker_static = copy.deepcopy(marker_tmp) marker_dynamic = copy.deepcopy(marker_tmp) marker_dynamic.color = color_dynamic # marker_dynamic.scale.x=self.world.bbox_dynamic[0] # marker_dynamic.scale.y=self.world.bbox_dynamic[1] # marker_dynamic.scale.z=self.world.bbox_dynamic[2] marker_static.color = color_static ###################3 marker_array_static_mesh = MarkerArray() marker_array_dynamic_mesh = MarkerArray() for i in range(self.world.num_of_dyn_objects + self.world.num_of_stat_objects): t_ros = rospy.Time.now() t = rospy.get_time() #Same as before, but it's float dynamic_trajectory_msg = DynTraj() bbox_i = self.bboxes[i] s = self.world.scale if (self.type[i] == "dynamic"): [x_string, y_string, z_string] = self.trefoil(self.x_all[i], self.y_all[i], self.z_all[i], s, s, s, self.offset_all[i], self.slower[i]) # print("self.bboxes[i]= ", self.bboxes[i]) dynamic_trajectory_msg.bbox = bbox_i marker_dynamic.scale.x = bbox_i[0] marker_dynamic.scale.y = bbox_i[1] marker_dynamic.scale.z = bbox_i[2] else: # [x_string, y_string, z_string] = self.static(self.x_all[i], self.y_all[i], self.z_all[i]); dynamic_trajectory_msg.bbox = bbox_i marker_static.scale.x = bbox_i[0] marker_static.scale.y = bbox_i[1] marker_static.scale.z = bbox_i[2] if (self.type[i] == "static_vert"): [x_string, y_string, z_string] = self.wave_in_z(self.x_all[i], self.y_all[i], self.z_all[i], s, self.offset_all[i], 1.0) else: [x_string, y_string, z_string] = self.wave_in_z(self.x_all[i], self.y_all[i], self.z_all[i], s, self.offset_all[i], 1.0) x = eval(x_string) y = eval(y_string) z = eval(z_string) dynamic_trajectory_msg.is_agent = False dynamic_trajectory_msg.header.stamp = t_ros dynamic_trajectory_msg.function = [x_string, y_string, z_string] dynamic_trajectory_msg.pos.x = x #Current position dynamic_trajectory_msg.pos.y = y #Current position dynamic_trajectory_msg.pos.z = z #Current position dynamic_trajectory_msg.id = 4000 + i #Current id 4000 to avoid interference with ids from agents #TODO self.pubTraj.publish(dynamic_trajectory_msg) br.sendTransform((x, y, z), (0, 0, 0, 1), t_ros, self.name + str(dynamic_trajectory_msg.id), "world") #If you want to move the objets in gazebo # gazebo_state = ModelState() # gazebo_state.model_name = str(i) # gazebo_state.pose.position.x = x # gazebo_state.pose.position.y = y # gazebo_state.pose.position.z = z # gazebo_state.reference_frame = "world" # self.pubGazeboState.publish(gazebo_state) #If you want to see the objects in rviz point = Point() point.x = x point.y = y point.z = z ########################## marker = Marker() marker.id = i marker.ns = "mesh" marker.header.frame_id = "world" marker.type = marker.MESH_RESOURCE marker.action = marker.ADD marker.pose.position.x = x marker.pose.position.y = y marker.pose.position.z = z 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.lifetime = rospy.Duration.from_sec(0.0) marker.mesh_use_embedded_materials = True marker.mesh_resource = self.meshes[i] if (self.type[i] == "dynamic"): marker_dynamic.points.append(point) marker.scale.x = bbox_i[0] marker.scale.y = bbox_i[1] marker.scale.z = bbox_i[2] marker_array_dynamic_mesh.markers.append(marker) if (self.type[i] == "static_vert" or self.type[i] == "static_horiz"): marker.scale.x = bbox_i[0] marker.scale.y = bbox_i[1] marker.scale.z = bbox_i[2] marker_array_static_mesh.markers.append(marker) ########################## marker_static.points.append(point) self.pubShapes_dynamic_mesh.publish(marker_array_dynamic_mesh) self.pubShapes_dynamic.publish(marker_dynamic) # if(self.already_published_static_shapes==False): self.pubShapes_static_mesh.publish(marker_array_static_mesh) self.pubShapes_static.publish(marker_static)