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 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 draw_rod(self, rod): marker = Marker() marker.header.frame_id = "/pl_base" marker.ns = "basic_shapes" marker.id = 10 + rod marker.type = marker.CYLINDER marker.action = marker.ADD marker.pose.position.x = self.rod_position[rod][0] marker.pose.position.y = self.rod_position[rod][1] marker.pose.position.z = self.position_z marker.pose.orientation.w = 1.0 marker.scale.x = ROD_SCALE_XYZ[0] marker.scale.y = ROD_SCALE_XYZ[1] marker.scale.z = ROD_SCALE_XYZ[2] marker.color.r = ROD_COLOR_RGB[0] marker.color.g = ROD_COLOR_RGB[1] marker.color.b = ROD_COLOR_RGB[2] marker.color.a = 1.0 self.rviz_pub.publish(marker) # pink top of the rod marker.id = 30 + rod marker.type = marker.SPHERE marker.pose.position.z = marker.pose.position.z + 0.0475 marker.scale.z = TOP_SCALE_Z marker.color.r = TOP_COLOR_RGB[0] marker.color.g = TOP_COLOR_RGB[1] marker.color.b = TOP_COLOR_RGB[2] self.rviz_pub.publish(marker)
def visualize_traj(points): traj = Marker() traj.header.frame_id = FRAME traj.header.stamp = rospy.get_rostime() traj.ns = "love_letter" traj.action = Marker.ADD traj.pose.orientation.w = 1.0 traj.type = Marker.LINE_STRIP traj.scale.x = 0.001 # line width traj.color.r = 1.0 traj.color.b = 0.0 traj.color.a = 1.0 if(WRITE_MULTIPLE_SHAPES): traj.id = shapeCount; else: traj.id = 0; #overwrite any existing shapes traj.lifetime.secs = 1; #timeout for display traj.points = list(points) # use interactive marker from place_paper instead #pub_markers.publish(a4_sheet()) pub_markers.publish(traj)
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 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 make_marker(self, robot_id, x, y, ns): """ Create a Marker message with the given x,y coordinates """ m = Marker() m.header.stamp = rospy.Time.now() m.header.frame_id = 'map' m.ns = ns m.action = m.ADD m.pose.position.x = x m.pose.position.y = y if robot_id == 0: m.color.r = 255 m.color.g = 0 m.color.b = 0 else: m.color.r = 0 m.color.g = 0 m.color.b = 255 m.color.a = 1.0 if ns == 'mess': m.type = m.CUBE m.id = len(self.messes) m.scale.x = m.scale.y = m.scale.z = .15 self.messes.append(m) else: m.type = m.SPHERE m.id = robot_id m.scale.x = m.scale.y = m.scale.z = .35 return m
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 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 create_force_marker(self, force, axis, frame_id): m = Marker() m.header.frame_id = frame_id m.type = m.CYLINDER m.scale.x = 0.02 # diameter m.scale.y = 0.02 # also diameter m.scale.z = abs(force) / self.force_to_length # length # MarkerArray visualizer in Rviz complains if a scale is 0 if m.scale.z == 0.0: m.scale.z = 0.0001 # namespaced so we can disable axis m.ns = axis if axis == 'x': m.id = 555 m.color.r = 1.0 m.color.g = 0.0 m.color.b = 0.0 m.pose.position.x = m.scale.z / 2.0 # Aligned with the X axis of the frame if force > 0: m.pose.orientation = Quaternion( *quaternion_from_euler(0, radians(90), 0)) else: # if the force is negative the cylinder goes to the other side m.pose.orientation = Quaternion( *quaternion_from_euler(0, radians(-90), 0)) m.pose.position.x *= -1.0 elif axis == 'y': m.id = 666 m.color.r = 0.0 m.color.g = 1.0 m.color.b = 0.0 m.pose.position.y = m.scale.z / 2.0 # Aligned with the Y axis of the frame if force > 0: m.pose.orientation = Quaternion( *quaternion_from_euler(radians(90), 0, 0)) else: m.pose.orientation = Quaternion( *quaternion_from_euler(radians(-90), 0, 0)) m.pose.position.y *= -1.0 elif axis == 'z': m.id = 777 m.color.r = 0.0 m.color.g = 0.0 m.color.b = 1.0 m.pose.position.z = m.scale.z / 2.0 # Aligned with the Z axis of the frame if force > 0: m.pose.orientation = Quaternion( *quaternion_from_euler(0, 0, 0)) else: m.pose.orientation = Quaternion( *quaternion_from_euler(radians(180), 0, 0)) m.pose.position.z *= -1.0 m.color.a = 1.0 return m
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_waypoint_markers(self): pub_waypoint_markers = rospy.Publisher('waypoint_markers', MarkerArray) marker_array = MarkerArray() for i in range(len(self.waypoints)): waypoint = self.waypoints[i] waypoint_marker = Marker() waypoint_marker.id = i waypoint_marker.header.frame_id = "/map" waypoint_marker.header.stamp = rospy.Time.now() if (waypoint.type == 'P'): waypoint_marker.type = 5 # Line List waypoint_marker.text = 'waypoint_%s_point' % i waypoint_marker.color.r = 176.0 waypoint_marker.color.g = 224.0 waypoint_marker.color.b = 230.0 waypoint_marker.color.a = 0.5 waypoint_marker.scale.x = 0.5 c = waypoint.coordinate waypoint_marker.points.append(Point(c.x, c.y, c.z)) waypoint_marker.points.append(Point(c.x, c.y, c.z + 3.0)) else: waypoint_marker.type = 3 # Cylinder waypoint_marker.text = 'waypoint_%s_cone' % i waypoint_marker.color.r = 255.0 waypoint_marker.color.g = 69.0 waypoint_marker.color.b = 0.0 waypoint_marker.color.a = 1.0 waypoint_marker.scale.x = 0.3 waypoint_marker.scale.y = 0.3 waypoint_marker.scale.z = 0.5 waypoint_marker.pose.position = waypoint.coordinate marker_array.markers.append(waypoint_marker) if self.current_waypoint_idx != len(self.waypoints): current_waypoint_marker = Marker() current_waypoint_marker.id = 999 current_waypoint_marker.header.frame_id = "/map" current_waypoint_marker.header.stamp = rospy.Time.now() current_waypoint_marker.type = 2 # Sphere current_waypoint_marker.text = 'current_waypoint' current_waypoint_marker.color.r = 255.0 current_waypoint_marker.color.g = 0.0 current_waypoint_marker.color.b = 0.0 current_waypoint_marker.color.a = 1.0 current_waypoint_marker.scale.x = 0.3 current_waypoint_marker.scale.y = 0.3 current_waypoint_marker.scale.z = 0.3 current_waypoint = self.waypoints[self.current_waypoint_idx] current_waypoint_marker.pose.position.x = current_waypoint.coordinate.x current_waypoint_marker.pose.position.y = current_waypoint.coordinate.y current_waypoint_marker.pose.position.z = 1.0 marker_array.markers.append(current_waypoint_marker) pub_waypoint_markers.publish(marker_array)
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 node(): pub = rospy.Publisher('shapes', Marker, queue_size=10) rospy.init_node('plotter', anonymous=False) rate = rospy.Rate(1) points=Marker() line=Marker() #Set the frame ID and timestamp. See the TF tutorials for information on these. points.header.frame_id=line.header.frame_id="/map" points.header.stamp=line.header.stamp=rospy.Time.now() points.ns=line.ns = "markers" points.id = 0 line.id =2 points.type = Marker.POINTS line.type=Marker.LINE_STRIP #Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) points.action = line.action = Marker.ADD; points.pose.orientation.w = line.pose.orientation.w = 1.0; line.scale.x = 0.02; points.scale.x=0.03; line.scale.y= 0.02; points.scale.y=0.03; line.color.r = 1.0; points.color.g = 1.0; points.color.a=line.color.a = 1.0; points.lifetime =line.lifetime = rospy.Duration(); p=Point() p.x = 1; p.y = 1; p.z = 1; pp=[] pp.append(copy(p)) while not rospy.is_shutdown(): p.x+=0.1 pp.append(copy(p)) points.points=pp #print points.points,'\n','----------------','\n' line.points=pp pub.publish(points) pub.publish(line) rate.sleep()
def add_rviz_rod(self, rod): marker = Marker() actual = self.irpos.get_cartesian_pose() marker.header.frame_id = "/pl_base" marker.ns = "basic_shapes" marker.id = 10 + rod marker.type = marker.CYLINDER marker.action = marker.ADD marker.pose.position.x = actual.position.x marker.pose.position.y = actual.position.y marker.pose.position.z = actual.position.z - 0.108 + 0.009 marker.pose.orientation.w = 1.0 marker.scale.x = 0.01 marker.scale.y = 0.01 marker.scale.z = 0.095 marker.color.r = 0.75 marker.color.g = 0.70 marker.color.b = 0.5 marker.color.a = 1.0 self.rviz_pub.publish(marker) # pink top of the rod marker.id = 30 + rod marker.type = marker.SPHERE marker.pose.position.z = marker.pose.position.z + 0.0475 marker.scale.z = 0.01 marker.color.r = 1. marker.color.g = 0.4 marker.color.b = 0.4 self.rviz_pub.publish(marker) if(rod == 1): marker.type = marker.CUBE marker.pose.position.z = actual.position.z - 0.143 marker.scale.x = 0.079 marker.scale.y = 0.23 marker.scale.z = 0.014 marker.color.r = 0.75 marker.color.g = 0.70 marker.color.b = 0.5 marker.id = 20 self.rviz_pub.publish(marker) marker.id = 30 + rod marker.type = marker.SPHERE marker.pose.position.z
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_cartesian_markers(arm, time_stamp, cep, rot, c1, c2, marker_id): marker = Marker() marker.header.frame_id = ar.link_tf_name(arm, 0) marker.header.stamp = time_stamp marker.ns = arm marker.type = Marker.ARROW marker.action = Marker.ADD marker.pose.position.x = cep[0,0] marker.pose.position.y = cep[1,0] marker.pose.position.z = cep[2,0] marker.scale.x = 0.1 marker.scale.y = 0.2 marker.scale.z = 0.2 marker.lifetime = rospy.Duration() marker.id = marker_id*100 + 0 #rot1 = tr.Ry(math.radians(90.)) * rot.T rot1 = rot * tr.rotY(math.pi/2) quat = tr.matrix_to_quaternion(rot1) marker.pose.orientation.x = quat[0] marker.pose.orientation.y = quat[1] marker.pose.orientation.z = quat[2] marker.pose.orientation.w = quat[3] marker.color.r = c1[0] marker.color.g = c1[1] marker.color.b = c1[2] marker.color.a = 1. marker_pub.publish(marker) marker.id = marker_id*100 + 1 if arm == 'left_arm': #rot2 = tr.Rz(math.radians(90.)) * rot.T rot2 = rot * tr.rotZ(-math.pi/2) else: #rot2 = tr.Rz(math.radians(-90.)) * rot.T rot2 = rot * tr.rotZ(math.pi/2) quat = tr.matrix_to_quaternion(rot2) marker.pose.orientation.x = quat[0] marker.pose.orientation.y = quat[1] marker.pose.orientation.z = quat[2] marker.pose.orientation.w = quat[3] marker.color.r = c2[0] marker.color.g = c2[1] marker.color.b = c2[2] marker.color.a = 1. marker_pub.publish(marker)
def publishObstacles(self): """ Publishes the obstacles as markers """ mk = Marker() mk.header.stamp = rospy.get_rostime() mk.header.frame_id = '/base_link' mk.ns='basic_shapes' mk.id = 0 mk.type = Marker.POINTS mk.scale.x = 0.3 mk.scale.y = 0.3 mk.scale.z = 0.3 mk.color.r = 1.0 mk.color.a = 1.0 for value in self.obstacle_map.obstacles_in_memory: p = Point() p.x = value[0] p.y = value[1] mk.points.append(p) self.obs_pub.publish(mk)
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 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 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 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 __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 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 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 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_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_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 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_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 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 new_person.speed = np.sqrt(person.vel_x**2 + person.vel_y**2) people_tracked_msg.people.append(copy.deepcopy(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 = "C: {:.2f}".format(person.confidence) #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) self.polar_grid.publish_fov(now)
def pubTF(self, timer): # rospy.loginfo("[CallbackPy]***************In PUBTF") 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.static(self.x_all[i], self.y_all[i], self.z_all[i]) else: [x_string, y_string, z_string] = self.wave_in_z(self.x_all[i], self.y_all[i], self.z_all[i], 2.0, 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)
def find_affordance(self, data): aff_list = list() if self.affordance[0] == 'pcb': try: tmp_img = self.bridge.imgmsg_to_cv2(data.assos_img, 'rgb8') tmp_img2 = ImageEnhance.Color( Image.fromarray(tmp_img)).enhance(2) self.curr_img = image.img_to_array(tmp_img2) except CvBridgeError as e: print(e) for part in data.part_array: partname = part.part_id[:-1] if partname == 'pcb': pcb = part aff = Affordance() aff.object_name = pcb.part_id pcb_bounding_values = self.returnPcbBoundingValues(pcb) rospy.set_param('pcb_bounding_values', pcb_bounding_values) img_w = self.curr_img.shape[0] img_h = self.curr_img.shape[1] aff_mask = self.model.predict(self.curr_img) leverup_points, confidences = self.sample_leverup_points(aff_mask) if len(leverup_points) == 0: return aff_list aff.effect_name = 'levered' aff.affordance_name = 'leverable' leverup_points_converted = ConvertPixel2WorldRequest() for i in range(len(leverup_points)): lever_up_point = geometry_msgs.msg.Point() lever_up_point.y = leverup_points[i][0] * img_w / 256.0 lever_up_point.x = leverup_points[i][1] * img_h / 256.0 lever_up_point.z = 0 leverup_points_converted.pixels.append(lever_up_point) resp = self.pixel_world_srv(leverup_points_converted) affordance_vis_image = cv2.resize(tmp_img, (256, 256)) tmp_img = self.bridge.imgmsg_to_cv2( pcb.part_outline.part_mask, pcb.part_outline.part_mask.encoding) tmp_img = cv2.resize(tmp_img, (256, 256)) markerArray = MarkerArray() for i in range(len(leverup_points)): marker = Marker() ap = ActionParameters() ap.confidence = confidences[i] lever_up_point = AscPair() lever_up_point.key = 'start_pose' lever_up_point.value_type = 2 lever_up_point.value_pose.position.x = resp.points[i].x lever_up_point.value_pose.position.y = resp.points[i].y lever_up_point.value_pose.position.z = resp.points[i].z x = leverup_points[i][0] y = leverup_points[i][1] w_size = 9 temp = tmp_img[max(0, x - w_size):min(256, x + w_size), max(0, y - w_size):min(256, y + w_size)] x1, y1 = np.where(temp == 255) x2, y2 = np.where(temp == 0) direction = math.pi + math.atan2( np.mean(y2) - np.mean(y1), np.mean(x2) - np.mean(x1)) if len(y2) == 0 or len(y1) == 0 or len(x2) == 0 or len( x1) == 0 or np.isnan(direction) or np.isinf(direction): continue import tf point_inverted = np.array(leverup_points[i][::-1]) _ = cv2.arrowedLine( affordance_vis_image, tuple(point_inverted.astype(np.int16)), tuple((point_inverted + [np.sin(direction) * 15, np.cos(direction) * 15]).astype(np.int16)), (0, 0, 255), 1, tipLength=0.5) _ = cv2.arrowedLine( affordanceWrapper.affordance_vis_image, tuple(point_inverted.astype(np.int16)), tuple((point_inverted + [np.sin(direction) * 15, np.cos(direction) * 15]).astype(np.int16)), (0, 0, 255), 1, tipLength=0.5) quaternion = tf.transformations.quaternion_from_euler( 0, 0, direction) lever_up_point.value_pose.orientation.x = quaternion[0] lever_up_point.value_pose.orientation.y = quaternion[1] lever_up_point.value_pose.orientation.z = quaternion[2] lever_up_point.value_pose.orientation.w = quaternion[3] ap.parameters.append(lever_up_point) h = std_msgs.msg.Header() h.stamp = rospy.Time.now() h.frame_id = resp.header.frame_id marker.header = h marker.ns = "lever_up_points" marker.id = i marker.type = 0 # arrow marker.action = 0 marker.scale.x = 0.01 marker.scale.y = 0.001 marker.scale.z = 0.002 marker.lifetime = rospy.Duration(150) marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 marker.color.a = 1.0 marker.pose = lever_up_point.value_pose markerArray.markers.append(marker) aff.action_parameters_array.append(ap) aff_list.append(aff) rate = rospy.Rate(10) comp_img = self.bridge.cv2_to_compressed_imgmsg( affordance_vis_image) for _ in range(5): self.lever_up_rviz.publish(markerArray) self.lever_up_image_viz.publish(comp_img) rate.sleep() elif self.affordance[0] == 'magnet': for part in data.part_array: partname = part.part_id[:-1] if partname == 'magnet': aff = Affordance() aff.object_name = part.part_id aff.effect_name = 'levered' aff.affordance_name = 'leverable' ap = ActionParameters() ap.confidence = 0.8 # Dummy Value asc_pair = AscPair() asc_pair.key = 'start_pose' asc_pair.value_type = 2 asc_pair.value_pose = part.pose ap.parameters.append(asc_pair) aff.action_parameters_array.append(ap) aff_list.append(aff) return aff_list
def publish_3dbox(box3d_pub, corners_3d_velos, track_ids, types=None, publish_id=True): """ Publish 3d boxes in velodyne coordinate, with color specified by object_types If object_types is None, set all color to cyan corners_3d_velos : list of (8, 4) 3d corners """ marker_array = MarkerArray() for i, corners_3d_velo in enumerate(corners_3d_velos): # 3d box marker = Marker() marker.header.frame_id = FRAME_ID marker.header.stamp = rospy.Time.now() marker.id = i marker.action = Marker.ADD marker.lifetime = rospy.Duration(LIFETIME) marker.type = Marker.LINE_LIST b, g, r = DETECTION_COLOR_MAP[types[i]] if types is None: marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 1.0 else: marker.color.r = r/255.0 marker.color.g = g/255.0 marker.color.b = b/255.0 marker.color.a = 1.0 marker.scale.x = 0.1 marker.points = [] for l in LINES: p1 = corners_3d_velo[l[0]] marker.points.append(Point(p1[0], p1[1], p1[2])) p2 = corners_3d_velo[l[1]] marker.points.append(Point(p2[0], p2[1], p2[2])) marker_array.markers.append(marker) # add track id if publish_id: track_id = track_ids[i] text_marker = Marker() text_marker.header.frame_id = FRAME_ID text_marker.header.stamp = rospy.Time.now() text_marker.id = track_id + 1000 text_marker.action = Marker.ADD text_marker.lifetime = rospy.Duration(LIFETIME) text_marker.type = Marker.TEXT_VIEW_FACING p4 = corners_3d_velo[4] # upper front left corner text_marker.pose.position.x = p4[0] text_marker.pose.position.y = p4[1] text_marker.pose.position.z = p4[2] + 0.5 text_marker.text = str(track_id) text_marker.scale.x = 1 text_marker.scale.y = 1 text_marker.scale.z = 1 if types is None: text_marker.color.r = 0.0 text_marker.color.g = 1.0 text_marker.color.b = 1.0 else: b, g, r = DETECTION_COLOR_MAP[types[i]] text_marker.color.r = r/255.0 text_marker.color.g = g/255.0 text_marker.color.b = b/255.0 text_marker.color.a = 1.0 marker_array.markers.append(text_marker) box3d_pub.publish(marker_array)
r = rospy.Rate(30) f = float(0) while not rospy.is_shutdown(): points, line_strip, line_list = Marker(), Marker(), Marker() points.header.frame_id = line_strip.header.frame_id = line_list.header.frame_id = "/my_frame" points.header.stamp = line_strip.header.stamp = line_list.header.stamp = rospy.Time.now( ) points.ns = line_strip.ns = line_list.ns = "points_and_lines" points.action = line_list.action = line_list.action = Marker.ADD points.pose.orientation.w = line_strip.pose.orientation.w = line_list.pose.orientation.w = 1.0 points.id = 0 line_strip.id = 1 line_list.id = 2 points.type = Marker.POINTS line_strip.type = Marker.LINE_STRIP line_list.type = Marker.LINE_LIST # POINTS markers use x and y scale for width/height respectively points.scale.x = 0.2 points.scale.y = 0.2 # LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width line_strip.scale.x = 0.1 line_list.scale.x = 0.1
for j in range(N_MARKER): for k in range(N_MARKER): marker = Marker() marker.header.frame_id = "/world" marker.type = marker.CUBE marker.action = marker.ADD marker.scale.x = cube_scale marker.scale.y = cube_scale marker.scale.z = cube_scale 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.id = idx marker.pose.position.x = l_w / N_MARKER * i + r_min #math.cos(count / 50.0) marker.pose.position.y = l_w / N_MARKER * j + r_min #math.cos(count / 40.0) marker.pose.position.z = l_w / N_MARKER * k + r_min #math.cos(count / 30.0) markerArray.markers.append(marker) idx += 1 while not rospy.is_shutdown(): # We add the new marker to the MarkerArray, removing the oldest # marker from it when necessary # if(count > MARKERS_MAX): # markerArray.markers.pop(0) # Renumber the marker IDs # id = 0
def approach(event): global trajs global records global cur_msg # print(len(records[0,:])) print records for prs in range(0, len(records[0, :])): # if prs == processed_prs: # continue # global buffer_full #print('records:') #print(records) #print((prs,records[0,prs])) if records[0, prs] >= 7: if len(cur_msg.poses) > prs: j = cur_msg.poses[prs] xpos = j.position.x ypos = j.position.y global xpos_array global ypos_array xpos_array = trajs[prs][0] ypos_array = trajs[prs][1] # print('xpos_array:') # print(xpos_array) # print('ypos_array:') # print(ypos_array) xpos_std = statistics.stdev(xpos_array) ypos_std = statistics.stdev(ypos_array) xpos_array = [] ypos_array = [] #records[0,prs] = 0 # print('records:') # print(records) stopped = False if xpos_std < 0.005 and ypos_std < 0.005: print('%d.%d - Person %d stopped!' % (cur_msg.header.stamp.secs, cur_msg.header.stamp.nsecs / 10000000, prs)) stopped = True else: print('%d.%d - Person %d moving...' % (cur_msg.header.stamp.secs, cur_msg.header.stamp.nsecs / 1000000, prs)) stopped = False #print((xpos_std,ypos_std)) if stopped: global processed_prs processed_prs = prs quaternion = (j.orientation.x, j.orientation.y, j.orientation.z, j.orientation.w) euler = tf.transformations.euler_from_quaternion( quaternion) roll = euler[0] pitch = euler[1] yaw = euler[2] #angle = math.pi-yaw xdif = math.fabs(xpos - cur_robposx) ydif = math.fabs(ypos - cur_robposy) if xpos > cur_robposx: if ypos > cur_robposy: angle = math.atan(ydif / xdif) else: angle = (2 * math.pi) - math.atan(ydif / xdif) else: if ypos > cur_robposy: angle = math.pi - math.atan(ydif / xdif) else: angle = math.pi + math.atan(ydif / xdif) #print((xpos,ypos,cur_robposx,cur_robposy,(angle/math.pi)*180)) print((prs, xpos, ypos)) xd = math.fabs(math.cos(angle) * 0.65) yd = math.fabs(math.sin(angle) * 0.65) #print(((angle/math.pi)*180, xpos, ypos, xd, yd)) if angle >= 0 and angle < math.pi: yrob = ypos - yd if angle <= math.pi / 2: xrob = xpos - xd else: xrob = xpos + xd else: yrob = ypos + yd if angle <= (3 * math.pi / 2): xrob = xpos + xd else: xrob = xpos - xd marker = Marker() marker.header = cur_msg.header marker.ns = 'ROBOT' marker.id = 0 marker.type = marker.SPHERE marker.action = marker.ADD marker.lifetime.secs = 0 marker.lifetime.nsecs = 500000000 marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 marker.color.a = 1.0 marker.color.b = 1.0 marker.color.r = 1.0 marker.pose.orientation.w = 1.0 marker.pose.position.x = xrob marker.pose.position.y = yrob marker.pose.position.z = 0 publisher.publish(marker) new_angle = angle #-math.pi #print((angle/math.pi)*180, xpos, ypos, xrob, yrob,(new_angle/math.pi)*180) marker2 = Marker() marker2.header = cur_msg.header marker2.ns = 'ROBOT' marker2.id = 0 marker2.type = marker2.ARROW marker2.action = marker2.ADD marker2.lifetime.secs = 0 marker2.lifetime.nsecs = 500000000 marker2.scale.x = 1.0 marker2.scale.y = 0.1 marker2.scale.z = 0.1 marker2.color.a = 1.0 marker2.color.b = 0.5 marker2.color.r = 1.0 new_quaternion = tf.transformations.quaternion_from_euler( 0, 0, new_angle) marker2.pose.orientation.x = new_quaternion[0] marker2.pose.orientation.y = new_quaternion[1] marker2.pose.orientation.z = new_quaternion[2] marker2.pose.orientation.w = new_quaternion[3] marker2.pose.position.x = xrob marker2.pose.position.y = yrob marker2.pose.position.z = 0 publisher2.publish(marker2) #print(rospy.Duration()) client.wait_for_server() goal = MoveBaseGoal() goal.target_pose.header.frame_id = "odom_combined" goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.pose.position.x = xrob goal.target_pose.pose.position.y = yrob goal.target_pose.pose.orientation.x = new_quaternion[0] goal.target_pose.pose.orientation.y = new_quaternion[1] goal.target_pose.pose.orientation.z = new_quaternion[2] goal.target_pose.pose.orientation.w = new_quaternion[3] client.send_goal(goal) client.wait_for_result(rospy.Duration.from_sec(5.0)) # #rospy.sleep(3) idx = random.randint(0, len(speechs) - 1) #soundhandle.say(speechs[idx]) #rospy.sleep(3) # speechClient.wait_for_server() #speechGoal = maryttsActionGoal() #speechGoal.header.frame_id = '' #speechGoal.header.stamp = rospy.Time.now() # sGoal = maryttsGoal() # sGoal.text = speechs[idx] #print(sGoal) #speechGoal.goal = sGoal # speechClient.send_goal(sGoal) # speechClient.wait_for_result(rospy.Duration.from_sec(3.0)) speech_msg = String() speech_msg.data = speechs[idx] pub3.publish(speech_msg) rospy.sleep(3) do_randomwalk()
def observation_callback(self, tracks_msg): markers_msg = MarkerArray() self.last_marker_id = 0 for track in tracks_msg.changes.nodes: if track.has_shape is True: for shape_idx, shape in enumerate(track.shapes): marker = Marker() self.last_marker_id += 1 marker.id = self.last_marker_id marker.ns = track.id marker.action = Marker.MODIFY marker.header = tracks_msg.header position = [ track.pose_stamped.pose.pose.position.x, track.pose_stamped.pose.pose.position.y, track.pose_stamped.pose.pose.position.z ] orientation = [ track.pose_stamped.pose.pose.orientation.x, track.pose_stamped.pose.pose.orientation.y, track.pose_stamped.pose.pose.orientation.z, track.pose_stamped.pose.pose.orientation.w ] world_transform = np.dot(translation_matrix(position), quaternion_matrix(orientation)) position = [ shape.pose.position.x, shape.pose.position.y, shape.pose.position.z ] orientation = [ shape.pose.orientation.x, shape.pose.orientation.y, shape.pose.orientation.z, shape.pose.orientation.w ] shape_transform = np.dot(translation_matrix(position), quaternion_matrix(orientation)) shape_world_transform = np.dot(world_transform, shape_transform) position = translation_from_matrix(shape_world_transform) orientation = quaternion_from_matrix(shape_world_transform) marker.pose.position.x = position[0] marker.pose.position.y = position[1] marker.pose.position.z = position[2] marker.pose.orientation.x = orientation[0] marker.pose.orientation.y = orientation[1] marker.pose.orientation.z = orientation[2] marker.pose.orientation.w = orientation[3] if shape.type == PrimitiveShape.CYLINDER: marker.type = Marker.CYLINDER marker.scale = Vector3(x=shape.dimensions[0], y=shape.dimensions[0], z=shape.dimensions[1]) elif shape.type == PrimitiveShape.SPHERE: marker.type = Marker.SPHERE marker.scale = Vector3(x=shape.dimensions[0], y=shape.dimensions[0], z=shape.dimensions[0]) elif shape.type == PrimitiveShape.BOX: marker.type = Marker.CUBE marker.scale = Vector3(x=shape.dimensions[0], y=shape.dimensions[1], z=shape.dimensions[2]) elif shape.type == PrimitiveShape.MESH: if shape.mesh_resource != "": marker.type = Marker.MESH_RESOURCE marker.mesh_resource = shape.mesh_resource marker.mesh_use_embedded_materials = True else: marker.type = Marker.TRIANGLE_LIST marker.points = shape.vertices marker.scale = Vector3(x=1.0, y=1.0, z=1.0) else: raise NotImplementedError("Shape not implemented") marker.color = shape.color marker.color.a = 0.75 marker.lifetime = rospy.Duration(0.25) markers_msg.markers.append(marker) self.markers_publisher.publish(markers_msg)
def publishMarkers(self): drone_marker = Marker() drone_marker.header.frame_id = "/map" drone_marker.header.stamp = rospy.Time.now() drone_marker.id = ord(self._id[-1]) drone_marker.ns = "uavs" drone_marker.type = Marker.MESH_RESOURCE drone_marker.mesh_resource = "package://robots_description/models/mbzirc/meshes/multirotor.dae" drone_marker.color.a = 1 drone_marker.action = Marker.ADD drone_marker.pose = self._pose.pose drone_marker.scale.x = 0.001 drone_marker.scale.y = 0.001 drone_marker.scale.z = 0.001 drone_marker.mesh_use_embedded_materials = True drone_marker.color.r = 0.0 drone_marker.color.g = 0.0 drone_marker.color.b = 0.0 # Drone identifier id_marker = Marker() id_marker.header.frame_id = "/map" id_marker.header.stamp = rospy.Time.now() id_marker.id = ord(self._id[-1]) id_marker.ns = "uavs_state" id_marker.type = Marker.TEXT_VIEW_FACING id_marker.text = self._id id_marker.pose.position.z = self._pose.pose.position.z + 2.0 id_marker.pose.position.y = self._pose.pose.position.y id_marker.pose.position.x = self._pose.pose.position.x id_marker.color.a = 1 id_marker.color.r = 1.0 id_marker.color.g = 1.0 id_marker.color.b = 1.0 id_marker.scale.x = 1 id_marker.scale.y = 1 id_marker.scale.z = 1 id_marker.mesh_use_embedded_materials = True # Drone goal goal_marker = Marker() goal_marker.header.frame_id = "/map" goal_marker.header.stamp = rospy.Time.now() goal_marker.id = ord(self._id[-1]) goal_marker.ns = "uavs_state" goal_marker.type = Marker.SPHERE goal_marker.text = self._id goal_marker.pose.position.z = self._goal[2] goal_marker.pose.position.y = self._goal[1] goal_marker.pose.position.x = self._goal[0] goal_marker.color.a = 1 goal_marker.color.r = 0.0 goal_marker.color.g = 1.0 goal_marker.color.b = 0.0 goal_marker.scale.x = 1 goal_marker.scale.y = 1 goal_marker.scale.z = 1 goal_marker.mesh_use_embedded_materials = True # Drone cylinder uav_cylinder = Marker() uav_cylinder.header.frame_id = "/map" uav_cylinder.header.stamp = rospy.Time.now() uav_cylinder.id = ord(self._id[-1]) uav_cylinder.ns = "uavs" uav_cylinder.type = Marker.CYLINDER uav_cylinder.color.a = 0.5 uav_cylinder.color.r = 1.0 uav_cylinder.color.g = 0.647 uav_cylinder.color.b = 0 uav_cylinder.action = Marker.ADD uav_cylinder.pose.position = self._pose.pose.position uav_cylinder.scale.x = 2 * self._radius # Diameter uav_cylinder.scale.y = 2 * self._radius uav_cylinder.scale.z = 5.0 uav_cylinder.mesh_use_embedded_materials = True # Publish arrow for velocity arrow = Marker() arrow.header.frame_id = "/map" arrow.header.stamp = rospy.Time.now() arrow.id = ord(self._id[-1]) arrow.ns = "uavs" arrow.type = Marker.ARROW arrow.lifetime = rospy.Duration(0.15) arrow.color.a = 1 arrow.color.r = 1.0 arrow.color.g = 0.0 arrow.color.b = 0.0 arrow.action = Marker.ADD arrow.points = [] arrow.points.append( Point(self._pose.pose.position.x, self._pose.pose.position.y, self._pose.pose.position.z)) arrow.points.append( Point(self._pose.pose.position.x + self._vel.twist.linear.x * 2, self._pose.pose.position.y + self._vel.twist.linear.y * 2, self._pose.pose.position.z)) arrow.scale.x = 0.1 arrow.scale.y = 0.2 self._vel_pub.publish(arrow) self._pose_pub.publish(drone_marker) self._cylinder_pub.publish(uav_cylinder) self._id_pub.publish(id_marker) self._goal_pub.publish(goal_marker)
def planPath(self, x0, y0, yaw0, signID): self.firstNode = True self.hitta = False self.signID = signID self.openSet = [] self.closedSet = [] #Target position self.grid = [[None] * self.cell_x ] * self.cell_y # Creating the matrix: [[empty]*x]*y targetSign = next( (x for x in self.mapInfo["roadsigns"] if x["sign"] == self.signID), None) [xt0, yt0] = targetSign["pose"]["position"][:2] self.yaw_t = math.radians( targetSign["pose"]["orientation"][2]) + math.pi / 2 [self.xt, self.yt] = self.mapToGrid([ xt0 + math.cos(self.yaw_t) * self.target_threshold, yt0 + math.sin(self.yaw_t) * self.target_threshold ]) self.Start_node = AstarNode(self, x0, y0, yaw0) self.openSet.append(self.Start_node) self.node = self.Start_node #Starting position [self.Start_node.x, self.Start_node.y] = self.mapToGrid([x0, y0]) #print([self.Start_node.x, self.Start_node.y]) self.grid[int(self.Start_node.y)][int( self.Start_node.x)] = self.openSet[0] tm_array = MarkerArray() #Creating marker for displaying start location tm = Marker() tm.header.stamp = rospy.Time.now() tm.header.frame_id = "map" tm.id = 91 tm.type = tm.CYLINDER tm.action = tm.ADD tm.pose.position.x = x0 tm.pose.position.y = y0 tm.pose.position.z = 0.4 (tm.pose.orientation.x, tm.pose.orientation.y, tm.pose.orientation.z, tm.pose.orientation.w) = quaternion_from_euler(0, 0, 0) tm.scale.x = 0.05 tm.scale.y = 0.05 tm.scale.z = 0.8 tm.color = ColorRGBA(0.2, 1, 1, 1) tm_array.markers.append(tm) #Creating marker for displaying target location tm = Marker() tm.header.stamp = rospy.Time.now() tm.header.frame_id = "map" tm.id = 92 tm.type = tm.CYLINDER tm.action = tm.ADD tm.pose.position.x = self.gridToMap([self.xt, self.yt])[0] tm.pose.position.y = self.gridToMap([self.xt, self.yt])[1] tm.pose.position.z = 0.4 (tm.pose.orientation.x, tm.pose.orientation.y, tm.pose.orientation.z, tm.pose.orientation.w) = quaternion_from_euler(0, 0, 0) tm.scale.x = 0.05 tm.scale.y = 0.05 tm.scale.z = 0.8 tm.color = ColorRGBA(1, 1, 0, 1) tm_array.markers.append(tm) #Creating marker for displaying target roadsign tm = Marker() tm.header.stamp = rospy.Time.now() tm.header.frame_id = "map" tm.id = 93 tm.type = tm.CUBE tm.action = tm.ADD tm.pose.position.x = [ xt0 + math.cos(self.yaw_t) * 0.02, yt0 + math.sin(self.yaw_t) * 0.02 ][0] tm.pose.position.y = [ xt0 + math.cos(self.yaw_t) * 0.02, yt0 + math.sin(self.yaw_t) * 0.02 ][1] tm.pose.position.z = 0.4 (tm.pose.orientation.x, tm.pose.orientation.y, tm.pose.orientation.z, tm.pose.orientation.w) = quaternion_from_euler( 0, 0, self.yaw_t - math.pi / 2) tm.scale.x = 0.2 tm.scale.y = 0.01 tm.scale.z = 0.2 tm.color = ColorRGBA(1, 0, 0, 1) tm_array.markers.append(tm) rospy.sleep(0.1) self.markerPub.publish(tm_array) self.Create_Node() result = self.PathPlanning() # print(result) return result
def map_to_pathPlanner(self): #Read map info from json file mapFilePath = "/home/alsarmi/Dropbox/KTH_Classes/Project_Course_Drone/dd2419_ws/src/course_packages/dd2419_resources/worlds_json/awesome.world.json" mapString = "" with open(mapFilePath, "r") as file: for line in file: #for each row l = line.strip().replace(" ", "") #remove all blankspace mapString += l self.mapInfo = ast.literal_eval( mapString ) #convert string representation of read file into dictionary through some kind of black magic # Upper bounds self.x_ub = self.mapInfo["airspace"]["max"][ 0] + 6 * self.threshold # From json self.y_ub = self.mapInfo["airspace"]["max"][ 1] + 6 * self.threshold # from json # Lower bounds self.x_lb = self.mapInfo["airspace"]["min"][ 0] - 6 * self.threshold # From json self.y_lb = self.mapInfo["airspace"]["min"][ 1] - 6 * self.threshold # from json # Making the grid self.cell_x = int((self.x_ub - self.x_lb) / self.cell_constant) # Number of cells in x self.cell_y = int((self.y_ub - self.y_lb) / self.cell_constant) # Number of cells in y lines = [] for wall in self.mapInfo["walls"]: wallStart = [ wall["plane"]["start"][0], wall["plane"]["start"][1], wall["plane"]["start"][2] ] wallStop = [ wall["plane"]["stop"][0], wall["plane"]["stop"][1], wall["plane"]["start"][2] ] lines.append( LineString([(wallStart[0], wallStart[1]), (wallStop[0], wallStop[1])])) for line in lines: self.obstacles.append(line.buffer(self.obstacleInflation)) self.walls.append(line.buffer(self.wall_inflation)) tm_array = MarkerArray() #Creating marker for displaying airspace boundaries airspaceStart = self.mapInfo["airspace"]["min"] airspaceStop = self.mapInfo["airspace"]["max"] tm = Marker() tm.header.stamp = rospy.Time.now() tm.header.frame_id = "map" tm.id = 90 tm.type = tm.CUBE tm.action = tm.ADD tm.pose.position.x = (airspaceStart[0] + airspaceStop[0]) / 2 tm.pose.position.y = (airspaceStart[1] + airspaceStop[1]) / 2 tm.pose.position.z = 0.01 (tm.pose.orientation.x, tm.pose.orientation.y, tm.pose.orientation.z, tm.pose.orientation.w) = quaternion_from_euler(0, 0, 0) tm.scale.x = abs(airspaceStart[0]) + abs(airspaceStop[0]) tm.scale.y = abs(airspaceStart[1]) + abs(airspaceStop[1]) tm.scale.z = 0.02 tm.color = ColorRGBA(1, 1, 1, 0.3) tm_array.markers.append(tm) for i, ii in enumerate(lines): #Creating marker for displaying wall tm = Marker() wall_angle = math.atan2( (list(ii.coords)[0][1] - list(ii.coords)[1][1]), (list(ii.coords)[0][0] - list(ii.coords)[1][0])) tm.header.stamp = rospy.Time.now() tm.header.frame_id = "map" tm.id = 100 + i * 5 tm.type = tm.CUBE tm.action = tm.ADD tm.pose.position.x = (list(ii.coords)[0][0] + list(ii.coords)[1][0]) / 2 tm.pose.position.y = (list(ii.coords)[0][1] + list(ii.coords)[1][1]) / 2 tm.pose.position.z = 1.5 (tm.pose.orientation.x, tm.pose.orientation.y, tm.pose.orientation.z, tm.pose.orientation.w) = quaternion_from_euler(0, 0, wall_angle) tm.scale.x = math.sqrt( (list(ii.coords)[0][0] - list(ii.coords)[1][0])**2 + (list(ii.coords)[0][1] - list(ii.coords)[1][1])**2) tm.scale.y = 0.02 tm.scale.z = 3 tm.color = ColorRGBA(1, .5, .1, 1) tm_array.markers.append(tm) #Creating markers for displaying inflated wall tm = Marker() wall_angle = math.atan2( (list(ii.coords)[0][1] - list(ii.coords)[1][1]), (list(ii.coords)[0][0] - list(ii.coords)[1][0])) tm.header.stamp = rospy.Time.now() tm.header.frame_id = "map" tm.id = 101 + i * 5 tm.type = tm.CUBE tm.action = tm.ADD tm.pose.position.x = (list(ii.coords)[0][0] + list(ii.coords)[1][0]) / 2 tm.pose.position.y = (list(ii.coords)[0][1] + list(ii.coords)[1][1]) / 2 tm.pose.position.z = 1.5 (tm.pose.orientation.x, tm.pose.orientation.y, tm.pose.orientation.z, tm.pose.orientation.w) = quaternion_from_euler(0, 0, wall_angle) tm.scale.x = math.sqrt( (list(ii.coords)[0][0] - list(ii.coords)[1][0])**2 + (list(ii.coords)[0][1] - list(ii.coords)[1][1])**2) tm.scale.y = self.obstacleInflation * 2 tm.scale.z = 3 tm.color = ColorRGBA(1, .5, .1, 0.2) tm_array.markers.append(tm) tm = Marker() wall_angle = math.atan2( (list(ii.coords)[0][1] - list(ii.coords)[1][1]), (list(ii.coords)[0][0] - list(ii.coords)[1][0])) tm.header.stamp = rospy.Time.now() tm.header.frame_id = "map" tm.id = 102 + i * 5 tm.type = tm.CYLINDER tm.action = tm.ADD tm.pose.position.x = list(ii.coords)[0][0] tm.pose.position.y = list(ii.coords)[0][1] tm.pose.position.z = 1.5 (tm.pose.orientation.x, tm.pose.orientation.y, tm.pose.orientation.z, tm.pose.orientation.w) = quaternion_from_euler(0, 0, wall_angle) tm.scale.x = self.obstacleInflation * 2 tm.scale.y = self.obstacleInflation * 2 tm.scale.z = 3 tm.color = ColorRGBA(1, .5, .1, 0.2) tm_array.markers.append(tm) tm = Marker() wall_angle = math.atan2( (list(ii.coords)[0][1] - list(ii.coords)[1][1]), (list(ii.coords)[0][0] - list(ii.coords)[1][0])) tm.header.stamp = rospy.Time.now() tm.header.frame_id = "map" tm.id = 103 + i * 5 tm.type = tm.CYLINDER tm.action = tm.ADD tm.pose.position.x = list(ii.coords)[1][0] tm.pose.position.y = list(ii.coords)[1][1] tm.pose.position.z = 1.5 (tm.pose.orientation.x, tm.pose.orientation.y, tm.pose.orientation.z, tm.pose.orientation.w) = quaternion_from_euler(0, 0, wall_angle) tm.scale.x = self.obstacleInflation * 2 tm.scale.y = self.obstacleInflation * 2 tm.scale.z = 3 tm.color = ColorRGBA(1, .5, .1, 0.2) tm_array.markers.append(tm) rospy.sleep(.5) self.markerPub.publish(tm_array)
## Creating Arena with obstacles boundary = Marker() boundary.header.frame_id = "map" # This is to pair the frames with Rviz boundary.type = Marker.LINE_STRIP boundary.action = Marker.ADD boundary.scale.x = 0.2 boundary.color.a = 1.0 boundary.color.r = 1.0 boundary.color.g = 1.0 boundary.color.b = 0.0 boundary.ns = "Obstacles" boundary.id = 0 #boundary.points = [] boundary.points.append(Point(-width / 2, -width / 2, 0)) boundary.points.append(Point(width / 2, -width / 2, 0)) boundary.points.append(Point(width / 2, width / 2, 0)) boundary.points.append(Point(-width / 2, width / 2, 0)) boundary.points.append(Point(-width / 2, -width / 2, 0)) markerArray.markers.append(boundary) ################################################### ## Creating Obstacles for i in range(4): obst = Marker() obst.type = Marker.CUBE
listener = tf.TransformListener() mpub = rospy.Publisher("/graph/viz", MarkerArray, latch=True, queue_size=1) rospy.sleep(1.0) ma = MarkerArray() now = rospy.Time.now() print get_pkg_dir('floor_graph') + "/graph2d.picklez" g = ig.Graph.Read_Picklez(get_pkg_dir('floor_graph') + "/graph2d.picklez.gz") id = 0 for v in g.vs: marker = Marker() marker.header.stamp = now marker.header.frame_id = "/world" marker.ns = "graph_nodes" marker.id = id id += 1 marker.type = Marker.CYLINDER marker.action = Marker.ADD marker.pose.position.x = v["x"] marker.pose.position.y = v["y"] marker.pose.position.z = -0.05 marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.01 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 0.0 ma.markers.append(marker) for e in g.es:
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.81, 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 marker.pose.orientation.y = 0.0
def main(): # Load File folder_path = '/home/dank-engine/3d_ws/json2_merger/' path = list(pathlib.Path(folder_path).glob('*.json')) id = 0 k = 0 num = 20 arr_rx = np.zeros(num) arr_ry = np.zeros(num) arr_lx = np.zeros(num) arr_ly = np.zeros(num) arr_cx = np.zeros(num) arr_cy = np.zeros(num) for i in range(100, 552): filename = folder_path + '{i:0{width}}'.format(i=i + 1, width=4) + '.json' with open(filename, 'r') as f: print(filename) data = json.load(f) # GPS Topic gps_coor = GeoPointStamped() gps_map = NavSatFix() gps_pub = rospy.Publisher('/gps', GeoPointStamped, queue_size=10) gps_map_pub = rospy.Publisher('/gps/fix', NavSatFix, queue_size=10) #IMU Topic imu_coor = Imu() imu_pub = rospy.Publisher('/imu', Imu, queue_size=10) #PointCloud Topic cloud = PointCloud2() pc_data = rospy.Publisher('/velodyne_points', PointCloud2, queue_size=32) rospy.init_node('converter') # Publish GPS Data in ROS gps_coor.header.frame_id = "gps" gps_coor.position.latitude = data["INS_SWIFT"]["Latitude"] gps_coor.position.longitude = data["INS_SWIFT"]["Longitude"] gps_coor.position.altitude = float('nan') gps_map.header.frame_id = "base_link" gps_map.latitude = data["INS_SWIFT"]["Latitude"] gps_map.longitude = data["INS_SWIFT"]["Longitude"] gps_map.altitude = data["INS_SWIFT"]["Altitude"] gps_pub.publish(gps_coor) gps_map_pub.publish(gps_map) # Publish IMU Data in ROS roll = data["INS_SWIFT"]["Roll"] pitch = data["INS_SWIFT"]["Pitch"] yaw = data["INS_SWIFT"]["Yaw"] quaternion = tf.transformations.quaternion_from_euler( roll, pitch, yaw) imu_coor.header.frame_id = "imu" imu_coor.header.stamp = rospy.get_rostime() imu_coor.orientation.x = quaternion[0] imu_coor.orientation.y = quaternion[1] imu_coor.orientation.z = quaternion[2] imu_coor.orientation.w = quaternion[3] imu_coor.orientation_covariance = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] imu_pub.publish(imu_coor) # Publish Point Cloud Data point = [] scan = data["PointCloud"] for i in range(len(scan)): if (scan[i]["x"] != None): xyzi = (scan[i]["x"], scan[i]["y"], scan[i]["z"], scan[i]["intensity"]) point.append(xyzi) else: xyzi = (np.nan, np.nan, np.nan, 0) point.append(xyzi) cloud = xyzi_array_to_pointcloud2(point, rospy.get_rostime(), "velodyne") pc_data.publish(cloud) lane_mark = rospy.Publisher('/marker_lane', Marker, queue_size=100) marker_l = Marker() marker_l.header.frame_id = "base_link" marker_l.type = marker_l.TEXT_VIEW_FACING marker_l.action = marker_l.ADD marker_l.scale.x = 5 marker_l.scale.y = 5 marker_l.scale.z = 5 marker_l.color.a = 1.0 marker_l.color.r = 1.0 marker_l.color.g = 1.0 marker_l.color.b = 0.0 marker_l.pose.position.x = 0 marker_l.pose.position.y = 30 marker_l.pose.position.z = 1 marker_l.pose.orientation.w = 1.0 marker_l.text = "Lanes: " + str( data["Lanes"]) + '\n' + "Width: " + str( data["Width"]) + '\n' + "Slope_Hoizontal: " + str( data["Slope"]["Slope_Hoizontal"]) marker_l.id = 0 lane_mark.publish(marker_l) try: r = rospy.Rate(1) # 10hz lane_r = data["Right_Marker"] lane_l = data["Left_Marker"] lane_c = data["Center_Marker"] publisher = rospy.Publisher('/drive_region', MarkerArray, queue_size=100) arr_rx[k % num] = lane_r["x"] arr_ry[k % num] = lane_r["y"] arr_lx[k % num] = lane_l["x"] arr_ly[k % num] = lane_l["y"] arr_cx[k % num] = lane_c["x"] arr_cy[k % num] = lane_c["y"] # if k == 10: markerArray = MarkerArray() marker = Marker() marker.header.frame_id = "base_link" marker.type = marker.SPHERE marker.action = marker.ADD marker.scale.x = 2 marker.scale.y = 2 marker.scale.z = 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 = np.average(arr_rx) marker.pose.position.y = np.average(arr_ry) marker.pose.position.z = 0 marker1 = Marker() marker1.header.frame_id = "base_link" marker1.type = marker.SPHERE marker1.action = marker.ADD marker1.scale.x = 2 marker1.scale.y = 2 marker1.scale.z = 2 marker1.color.a = 1.0 marker1.color.r = 1.0 marker1.color.g = 1.0 marker1.color.b = 0.0 marker1.pose.orientation.w = 1.0 marker1.pose.position.x = np.average(arr_lx) marker1.pose.position.y = np.average(arr_ly) marker1.pose.position.z = 0 marker2 = Marker() marker2.header.frame_id = "base_link" marker2.type = marker.SPHERE marker2.action = marker.ADD marker2.scale.x = 2 marker2.scale.y = 2 marker2.scale.z = 2 marker2.color.a = 1.0 marker2.color.r = 0.0 marker2.color.g = 0.0 marker2.color.b = 1.0 marker2.pose.orientation.w = 1.0 marker2.pose.position.x = np.average(arr_cx) marker2.pose.position.y = np.average(arr_cy) marker2.pose.position.z = 0 markerArray.markers.append(marker) markerArray.markers.append(marker1) markerArray.markers.append(marker2) # Renumber the marker IDs for m in markerArray.markers: m.id = id id += 1 # Publish the MarkerArray publisher.publish(markerArray) # k = 0 k = k + 1 r.sleep() except KeyError: r = rospy.Rate(1) # 10hz r.sleep()
def marker_calc(copterPos, force, namespace): markerArray = MarkerArray() #marker for copter marker = Marker() marker.header.frame_id = "/neck" marker.ns = namespace marker.type = marker.SPHERE marker.action = marker.ADD marker.id = 0 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 = copterPos.x marker.pose.position.y = copterPos.y marker.pose.position.z = 0.0 markerArray.markers.append(marker) #marker for force markerArrow = Marker() markerArrow.header.frame_id = "/neck" markerArrow.ns = namespace markerArrow.type = marker.ARROW markerArrow.action = marker.ADD markerArrow.id = 1 #marker.scale.x = 1 #Scaling the force magnitude to the width of the arrow by scaling in y #This can be used if the arrow has no start and end point F = math.sqrt((force[0] * force[0]) + (force[1] * force[1])) markerArrow.scale.x = 0.1 #* abs (F) #length markerArrow.scale.y = 0.2 # * abs (F) #width markerArrow.scale.z = 0.2 #height markerArrow.color.a = 1.0 markerArrow.color.r = 1.0 markerArrow.color.g = 1.0 markerArrow.color.b = 0.0 #Setting marker position #markerArrow.pose.orientation.w = - 0.5 #markerArrow.pose.orientation.x = 0.0 #markerArrow.pose.orientation.y = 0.0 #markerArrow.pose.orientation.z = 0.0 #markerArrow.pose.position.x = copterPos.x #markerArrow.pose.position.y = copterPos.y #markerArrow.pose.position.z = 0.1 start = geometry_msgs.msg.Point() start.x = copterPos.x start.y = copterPos.y start.z = 0.0 end = geometry_msgs.msg.Point() end.x = copterPos.x + (force[0]) end.y = copterPos.y + (force[1]) end.z = 0.0 markerArrow.points.append(start) markerArrow.points.append(end) markerArray.markers.append(markerArrow) publisher.publish(markerArray)
def node(): r1=1 r2=1 global mapData rospy.Subscriber("/map", OccupancyGrid, mapCallBack) pub = rospy.Publisher('shapes', Marker, queue_size=10) rospy.init_node('RRTexplorer', anonymous=False) #Actionlib client client1 = actionlib.SimpleActionClient('/move_base', MoveBaseAction) client1.wait_for_server() goal = MoveBaseGoal() goal.target_pose.header.stamp=rospy.Time.now() goal.target_pose.header.frame_id="map" goal.target_pose.pose.position.x=1 goal.target_pose.pose.position.y=0 goal.target_pose.pose.position.z=0 goal.target_pose.pose.orientation.w = 1.0 client1.send_goal(goal) h=client1.get_state() print h,'\n ------',rospy.Time.now(),'------ \n' client1.wait_for_result() client1.get_result() rate = rospy.Rate(50) listener = tf.TransformListener() listener.waitForTransform('map', 'base_link', rospy.Time(0),rospy.Duration(10.0)) try: (trans,rot) = listener.lookupTransform('map', 'base_link', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): trans=[0,0] xinx=trans[0] xiny=trans[1] x_init=array([xinx,xiny]) V=array([x_init]) i=1.0 E=concatenate((x_init,x_init)) points=Marker() line=Marker() #Set the frame ID and timestamp. See the TF tutorials for information on these. points.header.frame_id=line.header.frame_id="map" points.header.stamp=line.header.stamp=rospy.Time.now() points.ns=line.ns = "markers" points.id = 0 line.id =1 points.type = Marker.POINTS line.type=Marker.LINE_LIST #Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) points.action = line.action = Marker.ADD; points.pose.orientation.w = line.pose.orientation.w = 1.0; line.scale.x = 0.02; points.scale.x=0.05; line.scale.y= 0.02; points.scale.y=0.05; line.color.r =9.0/255.0 line.color.g= 91.0/255.0 line.color.b =236.0/255.0 points.color.r = 255.0/255.0 points.color.g = 244.0/255.0 points.color.b = 0.0/255.0 points.color.a=1; line.color.a = 0.6; points.lifetime =line.lifetime = rospy.Duration(); p=Point() p.x = x_init[0] ; p.y = x_init[0] ; p.z = 0; pp=[] pl=[] pp.append(copy(p)) xdim=mapData.info.width ydim=mapData.info.height resolution=mapData.info.resolution Xstartx=mapData.info.origin.position.x Xstarty=mapData.info.origin.position.y #raw_input('Press Enter to start exploration') #-------------------------------RRT------------------------------------------ while not rospy.is_shutdown(): # Sample free #indxRand= floor( len(mapData.data)*random()) #yr=ceil(indxRand/xdim) #xr=indxRand-(floor(indxRand/xdim))*xdim #xr=xr*resolution+Xstartx #yr=yr*resolution+Xstarty xr=(random()*20.0)-10.0 yr=(random()*20.0)-10.0 x_rand = array([xr,yr]) # Nearest x_nearest=V[Nearest(V,x_rand),:] # Steer x_new=Steer(x_nearest,x_rand,param.eta) goal.target_pose.pose.position.x=x_new[0] goal.target_pose.pose.position.y=x_new[1] goal.target_pose.pose.orientation.w = 1.0 # unKnow discovery if gridValue(mapData,x_new)==-1: assigner1(goal,x_new,client1,listener) # ObstacleFree if ObstacleFree(x_nearest,x_new,mapData): # Near function X_near=Near(V,x_new,param.rneighb) s_Xnear=X_near.shape[0] if All(X_near==array([0])): s_Xnear=-1 V=vstack((V,x_new)) xmin=x_nearest cmin=Cost(E,x_nearest)+LA.norm(x_new-x_nearest) ii=0 for ii in range(0,s_Xnear): xnear=copy(X_near[ii,:]) if ObstacleFree(xnear,x_new,mapData) and ( Cost(E,xnear)+LA.norm(xnear-x_new) )<cmin: xmin=copy(xnear) cmin=Cost(E,xnear)+LA.norm(xnear-x_new) temp=concatenate((xmin,x_new)) E=vstack((E,temp)) iii=0 for iii in range(0,s_Xnear): xnear=copy(X_near[iii,:]) if ObstacleFree(xnear,x_new,mapData) and ( Cost(E,x_new)+LA.norm(xnear-x_new) )<Cost(E,xnear): row=Find(E,xnear) E=delete(E, (row), axis=0) temp=concatenate((x_new,xnear)) E=vstack((E,temp)) #Plotting pl=prepEdges(E) p.x=x_new[0] p.y=x_new[1] pp.append(copy(p)) points.points=pp line.points=pl pub.publish(points) pub.publish(line) rate.sleep()
def _make_lane_markings(clockwise=True, lane_index=-1): _lane_marking = Marker() _lane_marking.header.frame_id = "/map" _lane_marking.ns = 'lane' if lane_index == -1: _lane_marking.id = 0 # center lane _deviation = 0 elif clockwise: _lane_marking.id = 1 + lane_index _deviation = -_lane_marking.id * self._lane_width else: _lane_marking.id = 1 + self._lane_num_clockwise + lane_index _deviation = (1 + lane_index) * self._lane_width _lane_marking.type = Marker.LINE_STRIP _lane_marking.action = Marker.ADD _lane_marking.pose = convert_to_message( tf.transformations.translation_matrix((0, 0, 0))) _lane_marking.scale.x = 0.3 # lane marking width _lane_marking.color.r = 1.0 _lane_marking.color.g = 1.0 if lane_index == -1: _lane_marking.color.b = 0.0 else: _lane_marking.color.b = 1.0 _lane_marking.color.a = 1.0 lm_init_position = (0, _deviation, 0) lm_init_point = geometry_msgs.msg.Point(*lm_init_position) curve_radius = self._radius_curve_track + _deviation lm_length = 2 * self._length_straight_track + 2 * math.pi * curve_radius lm_discrete_num = int(lm_length / s) + 1 print(lm_discrete_num) lm_landmark1 = int(self._length_straight_track / s) lm_landmark2 = int( (self._length_straight_track + math.pi * curve_radius) / s) lm_landmark3 = int( (2 * self._length_straight_track + math.pi * curve_radius) / s) theta = s / curve_radius for i in range(lm_discrete_num - 1): if i < lm_landmark1: pos = (i * s, _deviation, 0) elif i < lm_landmark2: pos = (self._length_straight_track + math.sin((i - lm_landmark1) * theta) \ * curve_radius, _deviation + math.cos((i - lm_landmark1) * theta) * curve_radius - curve_radius, 0) elif i < lm_landmark3: pos = (self._length_straight_track - (i - lm_landmark2) * s, _deviation - 2 * curve_radius, 0) else: pos = (-math.sin((i - lm_landmark3) * theta) \ * curve_radius, _deviation - math.cos((i - lm_landmark3) * theta) * curve_radius - curve_radius, 0) _lane_marking.points.append(geometry_msgs.msg.Point(*pos)) _lane_marking.points.append(lm_init_point) return _lane_marking
def publish_marker_callback(self, _): # publish checkpoints lines marker checkpoints_marker = Marker() checkpoints_marker.header.frame_id = "map" checkpoints_marker.header.stamp = rospy.Time.now() checkpoints_marker.ns = "checkpoints" checkpoints_marker.type = Marker.LINE_LIST checkpoints_marker.action = Marker.ADD checkpoints_marker.pose.orientation.w = 1 checkpoints_marker.scale.x = 0.05 checkpoints_marker.color.r = 0.85 checkpoints_marker.color.g = 0.85 checkpoints_marker.color.b = 1.0 checkpoints_marker.color.a = 1.0 next_checkpoint_marker = deepcopy(checkpoints_marker) next_checkpoint_marker.id = 1 next_checkpoint_marker.color.r = 0.0 next_checkpoint_marker.color.g = 0.0 next_checkpoint_marker.color.b = 1.0 reached_checkpoints_marker = deepcopy(checkpoints_marker) reached_checkpoints_marker.id = 2 reached_checkpoints_marker.color.r = 0.0 reached_checkpoints_marker.color.g = 1.0 reached_checkpoints_marker.color.b = 0.0 marker_array = MarkerArray() for checkpoint_index, checkpoint_segment in enumerate( self.checkpoints): text_checkpoints_marker = Marker() text_checkpoints_marker.header.frame_id = "map" text_checkpoints_marker.header.stamp = rospy.Time.now() text_checkpoints_marker.ns = "checkpoints" text_checkpoints_marker.action = Marker.ADD text_checkpoints_marker.id = checkpoint_index + 3 text_checkpoints_marker.type = Marker.TEXT_VIEW_FACING text_checkpoints_marker.color.r = 1.0 text_checkpoints_marker.color.g = 1.0 text_checkpoints_marker.color.b = 1.0 text_checkpoints_marker.color.a = 1.0 text_checkpoints_marker.text = str(checkpoint_index + 1) text_checkpoints_marker.scale.z = 0.3 text_checkpoints_marker.pose.position.x = ( checkpoint_segment[0][0] + checkpoint_segment[1][0]) / 2 text_checkpoints_marker.pose.position.y = ( checkpoint_segment[0][1] + checkpoint_segment[1][1]) / 2 text_checkpoints_marker.pose.orientation.w = 1 marker_array.markers.append(text_checkpoints_marker) if checkpoint_index == self.next_checkpoint_index: for point in checkpoint_segment: next_checkpoint_marker.points.append( Point(x=point[0], y=point[1])) if checkpoint_index < self.next_checkpoint_index: for point in checkpoint_segment: reached_checkpoints_marker.points.append( Point(x=point[0], y=point[1])) if checkpoint_index > self.next_checkpoint_index: for point in checkpoint_segment: checkpoints_marker.points.append( Point(x=point[0], y=point[1])) if len(checkpoints_marker.points): marker_array.markers.append(checkpoints_marker) if len(next_checkpoint_marker.points): marker_array.markers.append(next_checkpoint_marker) if len(reached_checkpoints_marker.points): marker_array.markers.append(reached_checkpoints_marker) self.vis_pub.publish(marker_array)
current_pose = PoseStamped() def pose_local_cb(pose): global current_pose current_pose = pose points = Marker() line_strip = Marker() landmark = Marker() points.header.frame_id = line_strip.header.frame_id = landmark.header.frame_id = "map" #points.header.stamp = line_strip.header.stamp = landmark.header.stamp = rospy.Time.now() points.ns = line_strip.ns = landmark.ns = "points_and_lines" points.action = line_strip.action = landmark.action = Marker.ADD points.id = 0 line_strip.id = 1 landmark.id = 2 points.type = Marker.SPHERE_LIST line_strip.type = Marker.LINE_STRIP landmark.type = Marker.POINTS points.pose.orientation.w = line_strip.pose.orientation.w = landmark.pose.orientation.w = 1.0 # POINTS markers use x and y scale for width/height respectively points.scale.x = 0.05 # 0.05, 0.1 points.scale.y = 0.05 points.scale.z = 0.05 line_strip.scale.x = 0.03 line_strip.scale.x = 0.03 line_strip.scale.x = 0.03
def on_new_point_cloud(data): pc = pointcloud2_to_array(data) points = np.zeros((pc.shape[0], 4), dtype=np.float32) for i in range(pc.shape[0]): points[i][0] = pc[i][0] points[i][1] = pc[i][1] points[i][2] = pc[i][2] - offset points[i][3] = pc[i][3] / 255 #points[i][3] = random.random() box_preds_lidar, labels, scores = pointp_based_detection( '/home/these/second.pytorch/path/for_ped/model_dir', params.net, params.input_cfg, params.model_cfg, params.train_cfg, params.class_names, params.voxel_generator, params.target_assigner, points, None, True, None, None, True) # box_preds_lidar,labels,scores =pointp_based_detection('/home/these/catkin_ws/src/pointpillars/src/path/to/model_dir', # params.net,params.input_cfg,params.model_cfg,params.train_cfg,params.class_names,params.voxel_generator,params.target_assigner, # points, # None, # True, # None, # None, # True # ) #visualization#### topic = '/pointpillars/visualization/marker_box' marker_publisher = rospy.Publisher(topic, MarkerArray) markerArray = MarkerArray() del_marker = Marker() del_marker.action = del_marker.DELETEALL del_marker.header.frame_id = "visualization" del_marker.id = 1 markerArray.markers.append(del_marker) for i in range(len(box_preds_lidar)): print(scores[i]) if scores[i] > 0.3: # result_string = str(labels[i])+','+str(scores[i])+','+str(box_preds_lidar[i][0])+ ','+ str(box_preds_lidar[i][1]) + ','+ str(math.sqrt(box_preds_lidar[i][1]*box_preds_lidar[i][1]+ box_preds_lidar[i][0]*box_preds_lidar[i][0])) +','+ str(box_preds_lidar[i][6])+'\n' # with open('20191121result_file.csv',mode ='a') as result_file: # print(result_string) # result_file.write(result_string) marker = Marker() marker.header.frame_id = "visualization" marker.type = marker.CUBE marker.action = marker.ADD marker.scale.x = box_preds_lidar[i][3] marker.scale.y = box_preds_lidar[i][4] marker.scale.z = box_preds_lidar[i][5] marker.color.a = 0.3 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.pose.orientation.x = 0 marker.pose.orientation.y = 0 marker.pose.orientation.z = math.sin(-box_preds_lidar[i][6] / 2) marker.pose.orientation.w = math.cos(-box_preds_lidar[i][6] / 2) marker.pose.position.x = box_preds_lidar[i][0] marker.pose.position.y = box_preds_lidar[i][1] marker.pose.position.z = box_preds_lidar[i][ 2] + box_preds_lidar[i][5] / 2.0 + offset marker.id = 10 + i marker.text = 'Car' marker.lifetime = rospy.Duration() markerArray.markers.append(marker) marker_publisher.publish(markerArray) topic = '/pointpillars/visualization/points' data.header.frame_id = "visualization" points_publisher = rospy.Publisher(topic, PointCloud2) points_publisher.publish(data) boundary = Marker() boundary.header.frame_id = "visualization" boundary.type = boundary.LINE_STRIP boundary.action = boundary.ADD boundary.scale.x = 0.5 boundary.color.a = 1.0 boundary.color.r = 1.0 boundary.color.g = 0.0 boundary.color.b = 0.0 boundary.pose.orientation.x = 0 boundary.pose.orientation.y = 0 boundary.pose.orientation.z = 0 boundary.pose.orientation.w = 1 corners = [0, -19.84, -2.5, 47.36, 19.84, 0.5] p1 = geometry_msgs.msg.Point(corners[0], corners[1], 0.) boundary.points.append(p1) p2 = geometry_msgs.msg.Point(corners[0], corners[4], 0.) boundary.points.append(p2) p3 = geometry_msgs.msg.Point(corners[3], corners[4], 0.) boundary.points.append(p3) p4 = geometry_msgs.msg.Point(corners[3], corners[1], 0.) boundary.points.append(p4) boundary.points.append(p1) boundary.id = 0 boundary.lifetime = rospy.Duration() topic = '/pointpillars/visualization/boundary' boundary_publisher = rospy.Publisher(topic, Marker) boundary_publisher.publish(boundary)
def publish_markers(self): offset_main_psi = -np.pi*0.70 offset_steer_psi = -np.pi*0.70 # create main frame marker marker_main_frame = Marker() marker_main_frame.header.frame_id = self.tf_steering_frame_name marker_main_frame.header.stamp = rospy.Time.now() marker_main_frame.id = 1 marker_main_frame.action = Marker.ADD marker_main_frame.type = Marker.MESH_RESOURCE marker_main_frame.mesh_resource = self.model_main_frame marker_main_frame.pose.position.x = 0.0 marker_main_frame.pose.position.y = 0.0 marker_main_frame.pose.position.z = 0.0 roll = 0.0 pitch = -self.bicycle_state.lean_phi yaw = self.bicycle_state.heading_psi + offset_main_psi quaternion = quaternion_from_euler(roll, pitch, yaw) marker_main_frame.pose.orientation.x = quaternion[0] marker_main_frame.pose.orientation.y = quaternion[1] marker_main_frame.pose.orientation.z = quaternion[2] marker_main_frame.pose.orientation.w = quaternion[3] marker_main_frame.scale.x = 1.0 marker_main_frame.scale.y = 1.0 marker_main_frame.scale.z = 1.0 marker_main_frame.color.a = self.alpha marker_main_frame.color.r = 1.0 marker_main_frame.color.g = 0.5 marker_main_frame.color.b = 0.0 self.pub_marker_main.publish(marker_main_frame) # create steering frame marker maker_steering_frame = Marker() maker_steering_frame.header.frame_id = self.tf_steering_frame_name maker_steering_frame.header.stamp = rospy.Time.now() maker_steering_frame.id = 1 maker_steering_frame.action = Marker.ADD maker_steering_frame.type = Marker.MESH_RESOURCE maker_steering_frame.mesh_resource = self.model_steer_frame maker_steering_frame.scale.x = 1.0 maker_steering_frame.scale.y = 1.0 maker_steering_frame.scale.z = 1.0 maker_steering_frame.pose.position.x = 0.0 maker_steering_frame.pose.position.y = 0.0 maker_steering_frame.pose.position.z = 0.0 roll = 0.0 pitch = -self.bicycle_state.lean_phi yaw = offset_steer_psi + self.bicycle_state.heading_psi + self.bicycle_state.steering_delta quaternion = quaternion_from_euler(roll, pitch, yaw) maker_steering_frame.pose.orientation.x = quaternion[0] maker_steering_frame.pose.orientation.y = quaternion[1] maker_steering_frame.pose.orientation.z = quaternion[2] maker_steering_frame.pose.orientation.w = quaternion[3] maker_steering_frame.color.a = self.alpha maker_steering_frame.color.r = 0.8 maker_steering_frame.color.g = 0.4 maker_steering_frame.color.b = 0.0 self.pub_marker_steering.publish(maker_steering_frame)
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: # Publish tracked people to /people_tracked topic and to rviz for person in self.people_tracked: leg_1 = person.leg_1 leg_2 = person.leg_2 if self.publish_occluded or leg_1.seen_in_current_scan or leg_2.seen_in_current_scan: # Get person's 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 = (leg_1.pos_x + leg_2.pos_x) / 2. ps.point.y = (leg_1.pos_y + leg_2.pos_y) / 2. 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 new_person.id = person.id_num people_tracked_msg.people.append(new_person) # publish rviz markers 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() - leg_1.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 for i in xrange( 2 ): # publish two markers per person: one for body and one for head marker.id = marker_id #person.id_num + 20000*i marker_id += 1 if i == 0: # cylinder for body shape 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 else: # 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 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) # 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)
def handle_periodic_publish(self, event): req = ReadDataRequest() req.perception_name = 'objects' req.query = '{}' req.data = ['~'] result = self.rd_srv(req) if not result.result: return vis_msg = MarkerArray() ret_data = json.loads(result.data) for obj in ret_data: br = tf2_ros.TransformBroadcaster() transformed_msg = TransformStamped() transformed_msg.header.stamp = rospy.Time.now() transformed_msg.header.frame_id = obj['frame_id'] transformed_msg.child_frame_id = obj['name'] transformed_msg.transform.translation.x = obj['xyz'][0] transformed_msg.transform.translation.y = obj['xyz'][1] transformed_msg.transform.translation.z = obj['xyz'][2] q = tf.transformations.quaternion_from_euler( obj['rpy'][0], obj['rpy'][1], obj['rpy'][2]) transformed_msg.transform.rotation.x = q[0] transformed_msg.transform.rotation.y = q[1] transformed_msg.transform.rotation.z = q[2] transformed_msg.transform.rotation.w = q[3] br.sendTransform(transformed_msg) marker = Marker() marker.header.frame_id = obj['frame_id'] marker.header.stamp = rospy.Time.now() marker.ns = obj['name'] marker.id = 0 obj_type = obj['geometry'].keys()[0] if obj_type == 'box': marker.type = Marker.CUBE elif obj_type == 'cylinder': marker.type = Marker.CYLINDER elif obj_type == 'sphere': marker.type = Marker.SPHERE marker.action = Marker.ADD marker.pose.position.x = obj['xyz'][0] marker.pose.position.y = obj['xyz'][1] marker.pose.position.z = obj['xyz'][2] orientation = tf.transformations.quaternion_from_euler( obj['rpy'][0], obj['rpy'][1], obj['rpy'][2]) 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.scale.x = obj['geometry'][obj_type][0] marker.scale.y = obj['geometry'][obj_type][1] marker.scale.z = obj['geometry'][obj_type][2] marker.color.r = obj['material']['color'][0] marker.color.g = obj['material']['color'][1] marker.color.b = obj['material']['color'][2] marker.color.a = obj['material']['color'][3] vis_msg.markers.append(marker) self.pub_vis_msg.publish(vis_msg)
def create_delta_simulation(): print "Creating simulation" markerArray = MarkerArray() width = 0.005 # Point of first dynamixel base1 = Marker() base1.header.frame_id = "/map" base1.type = base1.SPHERE base1.action = base1.ADD base1.scale.x = width base1.scale.y = width base1.scale.z = width base1.color.a = 1.0 base1.color.r = 1.0 base1.color.g = 0.0 base1.color.b = 0.0 base1.pose.orientation.w = 1.0 base1.pose.position.x = 0 base1.pose.position.y = - 0.06 * sqrt(2) base1.pose.position.z = 0 base1.id = 1 # Point of second dynamixel base2 = Marker() base2.header.frame_id = "/map" base2.type = base2.SPHERE base2.action = base2.ADD base2.scale.x = width base2.scale.y = width base2.scale.z = width base2.color.a = 1.0 base2.color.r = 0.0 base2.color.g = 1.0 base2.color.b = 0.0 base2.pose.orientation.w = 1.0 base2.pose.position.x = 0.06 base2.pose.position.y = 0.06 base2.pose.position.z = 0 base2.id = 2 # Point of third dynamixel base3 = Marker() base3.header.frame_id = "/map" base3.type = base3.SPHERE base3.action = base3.ADD base3.scale.x = width base3.scale.y = width base3.scale.z = width base3.color.a = 1.0 base3.color.r = 0.0 base3.color.g = 0.0 base3.color.b = 1.0 base3.pose.orientation.w = 1.0 base3.pose.position.x = - 0.06 base3.pose.position.y = 0.06 base3.pose.position.z = 0 base3.id = 3 # Arrow from first dynamixel to elbow arrow1 = Marker() arrow1.header.frame_id = "/map" arrow1.type = arrow1.ARROW arrow1.action = arrow1.ADD arrow1.scale.x = width arrow1.scale.y = width arrow1.color.a = 1.0 arrow1.color.r = 1.0 arrow1.color.g = 0.0 arrow1.color.b = 0.0 arrow1.points = [None]*2 arrow1.points[0] = Point(base1.pose.position.x, base1.pose.position.y, 0.0) arrow1.points[1] = Point(base1.pose.position.x, base1.pose.position.y, 0.08) arrow1.pose.orientation.w = 1.0 arrow1.id = 4 # Arrow from second dynamixel to elbow arrow2 = Marker() arrow2.header.frame_id = "/map" arrow2.type = arrow2.ARROW arrow2.action = arrow2.ADD arrow2.scale.x = width arrow2.scale.y = width arrow2.color.a = 1.0 arrow2.color.r = 0.0 arrow2.color.g = 1.0 arrow2.color.b = 0.0 arrow2.points = [None]*2 arrow2.points[0] = Point(base2.pose.position.x, base2.pose.position.y, 0.0) arrow2.points[1] = Point(base2.pose.position.x, base2.pose.position.y, 0.08) arrow2.pose.orientation.w = 1.0 arrow2.id = 5 # Arrow from third dynamixel to elbow arrow3 = Marker() arrow3.header.frame_id = "/map" arrow3.type = arrow3.ARROW arrow3.action = arrow3.ADD arrow3.scale.x = width arrow3.scale.y = width arrow3.color.a = 1.0 arrow3.color.r = 0.0 arrow3.color.g = 0.0 arrow3.color.b = 1.0 arrow3.points = [None]*2 arrow3.points[0] = Point(base3.pose.position.x, base3.pose.position.y, 0.0) arrow3.points[1] = Point(base3.pose.position.x, base3.pose.position.y, 0.08) arrow3.pose.orientation.w = 1.0 arrow3.id = 6 # Arrow from elbow to final for first dynamixel arrow4 = Marker() arrow4.header.frame_id = "/map" arrow4.type = arrow4.ARROW arrow4.action = arrow4.ADD arrow4.scale.x = width arrow4.scale.y = width arrow4.color.a = 1.0 arrow4.color.r = 1.0 arrow4.color.g = 0.0 arrow4.color.b = 0.0 arrow4.points = [None]*2 arrow4.points[0] = arrow1.points[1] arrow4.points[1] = arrow1.points[1] arrow4.points[1].z = arrow4.points[1].z + 0.2 # end to elbow dist arrow4.pose.orientation.w = 1.0 arrow4.id = 7 # Arrow from second dynamixel to elbow arrow5 = Marker() arrow5.header.frame_id = "/map" arrow5.type = arrow5.ARROW arrow5.action = arrow5.ADD arrow5.scale.x = width arrow5.scale.y = width arrow5.color.a = 1.0 arrow5.color.r = 0.0 arrow5.color.g = 1.0 arrow5.color.b = 0.0 arrow5.points = [None]*2 arrow5.points[0] = arrow2.points[1] arrow5.points[1] = arrow2.points[1] arrow5.points[1].z = arrow2.points[1].z + 0.2 arrow5.pose.orientation.w = 1.0 arrow5.id = 8 # Arrow from third dynamixel to elbow arrow6 = Marker() arrow6.header.frame_id = "/map" arrow6.type = arrow6.ARROW arrow6.action = arrow6.ADD arrow6.scale.x = width arrow6.scale.y = width arrow6.color.a = 1.0 arrow6.color.r = 0.0 arrow6.color.g = 0.0 arrow6.color.b = 1.0 arrow6.points = [None]*2 arrow6.points[0] = arrow3.points[1] arrow6.points[1] = arrow3.points[1] arrow6.points[1].z = arrow3.points[1].z + 0.2 arrow6.pose.orientation.w = 1.0 arrow6.id = 9 floor = Marker() floor.header.frame_id = "/map" floor.type = floor.CUBE floor.action = floor.ADD floor.scale.x = .2 floor.scale.y = .2 floor.scale.z = width floor.color.a = 1.0 # brown: 205-133-63 floor.color.r = 205.0 / 255.0 floor.color.g = 133.0 / 255.0 floor.color.b = 63.0 / 255.0 floor.pose.orientation.w = 1 floor.pose.position.x = 0 floor.pose.position.y = 0 floor.pose.position.z = -width floor.id = 10 # Append marker markerArray.markers.append(base1) markerArray.markers.append(base2) markerArray.markers.append(base3) markerArray.markers.append(arrow1) markerArray.markers.append(arrow2) markerArray.markers.append(arrow3) markerArray.markers.append(arrow4) markerArray.markers.append(arrow5) markerArray.markers.append(arrow6) markerArray.markers.append(floor) return markerArray
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 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" marker.color.r = track.colour[0] marker.color.g = track.colour[1] marker.color.b = track.colour[2] 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 person 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 = track.colour[0] # marker.color.g = track.colour[1] # marker.color.b = track.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 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 setupVisualization(dataset, args, selected_collection_key): """ Creates the necessary variables in a dictionary "dataset_graphics", which will be passed onto the visualization function """ # Create a python dictionary that will contain all the visualization related information graphics = { 'collections': {}, 'sensors': {}, 'pattern': {}, 'ros': {}, 'args': args } # Initialize ROS stuff rospy.init_node("calibrate") graphics['ros']['tf_broadcaster'] = tf.TransformBroadcaster() graphics['ros']['publisher_models'] = rospy.Publisher('~robot_meshes', MarkerArray, queue_size=0, latch=True) now = rospy.Time.now() # Parse xacro description file description_file, _, _ = uriReader( dataset['calibration_config']['description_file']) rospy.loginfo('Reading description file ' + description_file + '...') xml_robot = readXacroFile(description_file) pattern = dataset['calibration_config']['calibration_pattern'] # Create colormaps to be used for coloring the elements. Each collection contains a color, each sensor likewise. graphics['pattern']['colormap'] = cm.gist_rainbow( np.linspace(0, 1, pattern['dimension']['x'] * pattern['dimension']['y'])) graphics['collections']['colormap'] = cm.tab20b( np.linspace(0, 1, len(dataset['collections'].keys()))) for idx, collection_key in enumerate(sorted( dataset['collections'].keys())): graphics['collections'][str(collection_key)] = { 'color': graphics['collections']['colormap'][idx, :] } # color_map_sensors = cm.gist_rainbow(np.linspace(0, 1, len(dataset['sensors'].keys()))) # for idx, sensor_key in enumerate(sorted(dataset['sensors'].keys())): # dataset['sensors'][str(sensor_key)]['color'] = color_map_sensors[idx, :] # Create image publishers ---------------------------------------------------------- # We need to republish a new image at every visualization for collection_key, collection in dataset['collections'].items(): for sensor_key, sensor in dataset['sensors'].items(): if not collection['labels'][str(sensor_key)][ 'detected']: # not detected by sensor in collection continue if sensor['msg_type'] == 'Image': msg_type = sensor_msgs.msg.Image topic = dataset['calibration_config']['sensors'][sensor_key][ 'topic_name'] topic_name = '~c' + str(collection_key) + topic + '/labeled' graphics['collections'][collection_key][str(sensor_key)] = { 'publisher': rospy.Publisher(topic_name, msg_type, queue_size=0, latch=True) } msg_type = sensor_msgs.msg.CameraInfo topic_name = '~c' + str(collection_key) + '/' + str( sensor_key) + '/camera_info' graphics['collections'][collection_key][str(sensor_key)]['publisher_camera_info'] = \ rospy.Publisher(topic_name, msg_type, queue_size=0, latch=True) # Create Labeled Data publishers ---------------------------------------------------------- markers = MarkerArray() for collection_key, collection in dataset['collections'].items(): for sensor_key, sensor in dataset['sensors'].items(): if not collection['labels'][str(sensor_key)][ 'detected']: # not detected by sensor in collection continue if sensor[ 'msg_type'] == 'LaserScan': # -------- Publish the laser scan data ------------------------------ frame_id = genCollectionPrefix( collection_key, collection['data'][sensor_key]['header']['frame_id']) marker = Marker( header=Header(frame_id=frame_id, stamp=now), ns=str(collection_key) + '-' + str(sensor_key), id=0, frame_locked=True, type=Marker.POINTS, action=Marker.ADD, lifetime=rospy.Duration(0), pose=Pose(position=Point(x=0, y=0, z=0), orientation=Quaternion(x=0, y=0, z=0, w=1)), scale=Vector3(x=0.03, y=0.03, z=0), color=ColorRGBA( r=graphics['collections'][collection_key]['color'][0], g=graphics['collections'][collection_key]['color'][1], b=graphics['collections'][collection_key]['color'][2], a=1.0)) # Get laser points that belong to the chessboard (labelled) idxs = collection['labels'][sensor_key]['idxs'] rhos = [ collection['data'][sensor_key]['ranges'][idx] for idx in idxs ] thetas = [ collection['data'][sensor_key]['angle_min'] + collection['data'][sensor_key]['angle_increment'] * idx for idx in idxs ] for idx, (rho, theta) in enumerate(zip(rhos, thetas)): marker.points.append( Point(x=rho * math.cos(theta), y=rho * math.sin(theta), z=0)) markers.markers.append(copy.deepcopy(marker)) # Draw extrema points marker.ns = str(collection_key) + '-' + str(sensor_key) marker.type = Marker.SPHERE_LIST marker.id = 1 marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.a = 0.5 marker.points = [marker.points[0], marker.points[-1]] markers.markers.append(copy.deepcopy(marker)) # Draw detected edges marker.ns = str(collection_key) + '-' + str(sensor_key) marker.type = Marker.CUBE_LIST marker.id = 2 marker.scale.x = 0.05 marker.scale.y = 0.05 marker.scale.z = 0.05 marker.color.a = 0.5 marker.points = [] # Reset the list of marker points for edge_idx in collection['labels'][sensor_key][ 'edge_idxs']: # add edge points p = Point() p.x = rhos[edge_idx] * math.cos(thetas[edge_idx]) p.y = rhos[edge_idx] * math.sin(thetas[edge_idx]) p.z = 0 marker.points.append(p) markers.markers.append(copy.deepcopy(marker)) if sensor[ 'msg_type'] == 'PointCloud2': # -------- Publish the velodyne data ------------------------------ # Convert velodyne data on .json dictionary to ROS message type cloud_msg = message_converter.convert_dictionary_to_ros_message( "sensor_msgs/PointCloud2", collection['data'][sensor_key]) # Get LiDAR points that belong to the pattern idxs = collection['labels'][sensor_key]['idxs'] pc = ros_numpy.numpify(cloud_msg) points = np.zeros((pc.shape[0], 3)) points[:, 0] = pc['x'] points[:, 1] = pc['y'] points[:, 2] = pc['z'] frame_id = genCollectionPrefix( collection_key, collection['data'][sensor_key]['header']['frame_id']) marker = Marker( header=Header(frame_id=frame_id, stamp=now), ns=str(collection_key) + '-' + str(sensor_key), id=0, frame_locked=True, type=Marker.SPHERE_LIST, action=Marker.ADD, lifetime=rospy.Duration(0), pose=Pose(position=Point(x=0, y=0, z=0), orientation=Quaternion(x=0, y=0, z=0, w=1)), scale=Vector3(x=0.02, y=0.02, z=0.02), color=ColorRGBA( r=graphics['collections'][collection_key]['color'][0], g=graphics['collections'][collection_key]['color'][1], b=graphics['collections'][collection_key]['color'][2], a=0.4)) for idx in idxs: marker.points.append( Point(x=points[idx, 0], y=points[idx, 1], z=points[idx, 2])) markers.markers.append(copy.deepcopy(marker)) # Visualize LiDAR corner points marker = Marker( header=Header(frame_id=frame_id, stamp=now), ns=str(collection_key) + '-' + str(sensor_key) + '-limit_points', id=0, frame_locked=True, type=Marker.SPHERE_LIST, action=Marker.ADD, lifetime=rospy.Duration(0), pose=Pose(position=Point(x=0, y=0, z=0), orientation=Quaternion(x=0, y=0, z=0, w=1)), scale=Vector3(x=0.07, y=0.07, z=0.07), color=ColorRGBA( r=graphics['collections'][collection_key]['color'][0], g=graphics['collections'][collection_key]['color'][1], b=graphics['collections'][collection_key]['color'][2], a=0.8)) for pt in collection['labels'][sensor_key]['limit_points']: marker.points.append(Point(x=pt['x'], y=pt['y'], z=pt['z'])) markers.markers.append(copy.deepcopy(marker)) graphics['ros']['MarkersLabeled'] = markers graphics['ros']['PubLabeled'] = rospy.Publisher('~labeled_data', MarkerArray, queue_size=0, latch=True) # ----------------------------------------------------------------------------------------------------- # -------- Robot meshes # ----------------------------------------------------------------------------------------------------- # Check whether the robot is static, in the sense that all of its joints are fixed. If so, for efficiency purposes, # only one robot mesh (from the selected collection) is published. all_joints_fixed = True for joint in xml_robot.joints: if not joint.type == 'fixed': print('Robot has at least joint ' + joint.name + ' non fixed. Will render all collections') all_joints_fixed = False break markers = MarkerArray() if all_joints_fixed: # render a single robot mesh print('Robot has all joints fixed. Will render only collection ' + selected_collection_key) rgba = [.5, .5, .5, 1] # best color we could find m = urdfToMarkerArray(xml_robot, frame_id_prefix=genCollectionPrefix( selected_collection_key, ''), namespace=selected_collection_key, rgba=rgba) markers.markers.extend(m.markers) else: # render robot meshes for all collections for collection_key, collection in dataset['collections'].items(): # rgba = graphics['collections'][collection_key]['color'] # rgba[3] = 0.4 # change the alpha rgba = [.5, .5, .5, 0.7] # best color we could find m = urdfToMarkerArray(xml_robot, frame_id_prefix=genCollectionPrefix( collection_key, ''), namespace=collection_key, rgba=rgba) markers.markers.extend(m.markers) graphics['ros']['robot_mesh_markers'] = markers # ----------------------------------------------------------------------------------------------------- # -------- Publish the pattern data # ----------------------------------------------------------------------------------------------------- if dataset['calibration_config']['calibration_pattern'][ 'fixed']: # Draw single pattern for selected collection key frame_id = generateName( dataset['calibration_config']['calibration_pattern']['link'], prefix='c' + selected_collection_key) ns = str(selected_collection_key) markers = createPatternMarkers(frame_id, ns, selected_collection_key, now, dataset, graphics) else: # Draw a pattern per collection markers = MarkerArray() for idx, (collection_key, collection) in enumerate(dataset['collections'].items()): frame_id = generateName( dataset['calibration_config']['calibration_pattern']['link'], prefix='c' + collection_key) ns = str(collection_key) collection_markers = createPatternMarkers(frame_id, ns, collection_key, now, dataset, graphics) markers.markers.extend(collection_markers.markers) graphics['ros']['MarkersPattern'] = markers graphics['ros']['PubPattern'] = rospy.Publisher('~patterns', MarkerArray, queue_size=0, latch=True) # Create LaserBeams Publisher ----------------------------------------------------------- # This one is recomputed every time in the objective function, so just create the generic properties. markers = MarkerArray() for collection_key, collection in dataset['collections'].items(): for sensor_key, sensor in dataset['sensors'].items(): if not collection['labels'][sensor_key][ 'detected']: # chess not detected by sensor in collection continue if sensor['msg_type'] == 'LaserScan' or sensor[ 'msg_type'] == 'PointCloud2': frame_id = genCollectionPrefix( collection_key, collection['data'][sensor_key]['header']['frame_id']) marker = Marker( header=Header(frame_id=frame_id, stamp=rospy.Time.now()), ns=str(collection_key) + '-' + str(sensor_key), id=0, frame_locked=True, type=Marker.LINE_LIST, action=Marker.ADD, lifetime=rospy.Duration(0), pose=Pose(position=Point(x=0, y=0, z=0), orientation=Quaternion(x=0, y=0, z=0, w=1)), scale=Vector3(x=0.001, y=0, z=0), color=ColorRGBA( r=graphics['collections'][collection_key]['color'][0], g=graphics['collections'][collection_key]['color'][1], b=graphics['collections'][collection_key]['color'][2], a=1.0)) markers.markers.append(marker) graphics['ros']['MarkersLaserBeams'] = markers graphics['ros']['PubLaserBeams'] = rospy.Publisher('~LaserBeams', MarkerArray, queue_size=0, latch=True) # Create Miscellaneous MarkerArray ----------------------------------------------------------- markers = MarkerArray() # Text signaling the anchored sensor for _sensor_key, sensor in dataset['sensors'].items(): if _sensor_key == dataset['calibration_config']['anchored_sensor']: marker = Marker(header=Header(frame_id=str(_sensor_key), stamp=now), ns=str(_sensor_key), id=0, frame_locked=True, type=Marker.TEXT_VIEW_FACING, action=Marker.ADD, lifetime=rospy.Duration(0), pose=Pose(position=Point(x=0, y=0, z=0.2), orientation=Quaternion(x=0, y=0, z=0, w=1)), scale=Vector3(x=0.0, y=0.0, z=0.1), color=ColorRGBA(r=0.6, g=0.6, b=0.6, a=1.0), text='Anchored') markers.markers.append(marker) graphics['ros']['MarkersMiscellaneous'] = markers graphics['ros']['PubMiscellaneous'] = rospy.Publisher('~Miscellaneous', MarkerArray, queue_size=0, latch=True) # Publish only once in latched mode graphics['ros']['PubMiscellaneous'].publish( graphics['ros']['MarkersMiscellaneous']) return graphics
def __init__(self): rospy.init_node('revisualizer') self.rviz_pub = rospy.Publisher("visualization/state", visualization_msgs.Marker, queue_size=2) self.rviz_pub_t = rospy.Publisher("visualization/state_t", visualization_msgs.Marker, queue_size=2) self.rviz_pub_utils = rospy.Publisher("visualization/bus_voltage", visualization_msgs.Marker, queue_size=2) self.kill_server = InteractiveMarkerServer("interactive_kill") # text marker # TODO: Clean this up, there should be a way to set all of this inline self.surface_marker = visualization_msgs.Marker() self.surface_marker.type = self.surface_marker.TEXT_VIEW_FACING self.surface_marker.color = ColorRGBA(1, 1, 1, 1) self.surface_marker.scale.z = 0.1 self.depth_marker = visualization_msgs.Marker() self.depth_marker.type = self.depth_marker.TEXT_VIEW_FACING self.depth_marker.color = ColorRGBA(1.0, 1.0, 1.0, 1.0) self.depth_marker.scale.z = 0.1 # create marker for displaying current battery voltage self.low_battery_threshold = rospy.get_param('/battery/kill_voltage', 44.0) self.warn_battery_threshold = rospy.get_param('/battery/warn_voltage', 44.5) self.voltage_marker = visualization_msgs.Marker() self.voltage_marker.header.frame_id = "base_link" self.voltage_marker.lifetime = rospy.Duration(5) self.voltage_marker.ns = 'sub' self.voltage_marker.id = 22 self.voltage_marker.pose.position.x = -2.0 self.voltage_marker.scale.z = 0.2 self.voltage_marker.color.a = 1 self.voltage_marker.type = visualization_msgs.Marker.TEXT_VIEW_FACING # create an interactive marker to display kill status and change it self.need_kill_update = True self.kill_marker = InteractiveMarker() self.kill_marker.header.frame_id = "base_link" self.kill_marker.pose.position.x = -2.3 self.kill_marker.name = "kill button" kill_status_marker = Marker() kill_status_marker.type = Marker.TEXT_VIEW_FACING kill_status_marker.text = "UNKILLED" kill_status_marker.id = 55 kill_status_marker.scale.z = 0.2 kill_status_marker.color.a = 1.0 kill_button_control = InteractiveMarkerControl() kill_button_control.name = "kill button" kill_button_control.interaction_mode = InteractiveMarkerControl.BUTTON kill_button_control.markers.append(kill_status_marker) self.kill_server.insert(self.kill_marker, self.kill_buttton_callback) self.kill_marker.controls.append(kill_button_control) self.killed = False # connect kill marker to kill alarm self.kill_listener = AlarmListener("kill") self.kill_listener.add_callback(self.kill_alarm_callback) self.kill_alarm = AlarmBroadcaster("kill") # distance to bottom self.range_sub = rospy.Subscriber("dvl/range", RangeStamped, self.range_callback) # distance to surface self.depth_sub = rospy.Subscriber("depth", DepthStamped, self.depth_callback) # battery voltage self.voltage_sub = rospy.Subscriber("/bus_voltage", Float64, self.voltage_callback)
def publish(self, timestamp): pose = PoseStamped() pose.header.frame_id = self.target_frame pose.header.stamp = timestamp pose.pose.position.x = self.X[0, 0] pose.pose.position.y = self.X[1, 0] pose.pose.position.z = 0.0 Q = quaternion_from_euler(0, 0, self.X[2, 0]) pose.pose.orientation.x = Q[0] pose.pose.orientation.y = Q[1] pose.pose.orientation.z = Q[2] pose.pose.orientation.w = Q[3] self.pose_pub.publish(pose) ma = MarkerArray() marker = Marker() marker.header = pose.header marker.ns = "kf_uncertainty" marker.id = 5000 marker.type = Marker.CYLINDER marker.action = Marker.ADD marker.pose = pose.pose print 'diag(P): ' + str(numpy.diag(self.P)) marker.pose.position.z = -0.1 marker.scale.x = 3 * sqrt(self.P[0, 0]) marker.scale.y = 3 * sqrt(self.P[1, 1]) marker.scale.z = 0.1 marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 1.0 ma.markers.append(marker) for id in self.idx.iterkeys(): marker = Marker() marker.header.stamp = timestamp marker.header.frame_id = self.target_frame marker.ns = "landmark_kf" marker.id = id marker.type = Marker.CYLINDER marker.action = Marker.ADD l = self.idx[id] marker.pose.position.x = self.X[l, 0] marker.pose.position.y = self.X[l + 1, 0] marker.pose.position.z = -0.1 marker.pose.orientation.x = 0 marker.pose.orientation.y = 0 marker.pose.orientation.z = 1 marker.pose.orientation.w = 0 marker.scale.x = 0.2 #3*sqrt(self.P[l,l]) marker.scale.y = 0.2 #3*sqrt(self.P[l+1,l+1]); marker.scale.z = 0.1 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 = self.target_frame marker.ns = "landmark_kf" marker.id = 1000 + id marker.type = Marker.TEXT_VIEW_FACING marker.action = Marker.ADD marker.pose.position.x = self.X[l + 0, 0] marker.pose.position.y = self.X[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)
if __name__ == '__main__': # Initialize the node, as usual rospy.init_node('markers', argv=sys.argv) # Create a marker. Markers of all shapes share a common type. sphere = Marker() # Set the frame ID and type. The frame ID is the frame in which the position of the marker # is specified. The type is the shape of the marker, detailed on the wiki page. sphere.header.frame_id = '/base_link' sphere.type = sphere.SPHERE # Each marker has a unique ID number. If you have more than one marker that you want displayed at a # given time, then each needs to have a unique ID number. If you publish a new marker with the same # ID number and an existing marker, it will replace the existing marker with that ID number. sphere.id = 0 # Set the action. We can add, delete, or modify markers. sphere.action = sphere.ADD # These are the size parameters for the marker. The effect of these on the marker will vary by shape, # but, basically, they specify how big the marker along each of the axes of the coordinate frame named # in frame_id. sphere.scale.x = 0.5 sphere.scale.y = 0.5 sphere.scale.z = 0.5 # Color, as an RGB triple, from 0 to 1. sphere.color.r = 1.0 sphere.color.g = 0.0 sphere.color.b = 0.0