def marker_publisher(self, max=None): self.markers_msg = visualization_msgs.MarkerArray() stamp = self.get_clock().now().to_msg() msg = ROSMessage.line_marker(0, stamp, self.path, [0.1, 1.0, 1.0], "path") self.markers_msg.markers = [msg] self.markers_pub.publish(self.markers_msg)
def Lanecb(msg): global pub rate = rospy.Rate(10) MarkerArr = viz.MarkerArray() for i in range(len(msg.waypoints)): Marker = viz.Marker() Marker.header.frame_id = "map" Marker.header.stamp = rospy.Time.now() Marker.type = 0 Marker.action = 0 Marker.scale.x = 0.8 Marker.scale.y = 0.15 Marker.scale.z = 0.15 Marker.color.r = 0.1 Marker.color.g = 1.0 Marker.color.b = 0.0 Marker.color.a = 1.0 Marker.pose = msg.waypoints[i].pose.pose Marker.id = i MarkerArr.markers.append(Marker) if pub is not None: pub.publish(MarkerArr) rate.sleep()
def create_viz_markers(waypoints): marray= viz_msgs.MarkerArray() for w in waypoints: m_arrow = create_arrow(w) m_text = create_text(w) marray.markers.append(m_arrow) marray.markers.append(m_text) return marray
def __init__(self): super().__init__('MainNode') #Publishers: self.markers_pub = self.create_publisher( visualization_msgs.MarkerArray, 'marker', 3000) self.markers_path_pub = self.create_publisher( visualization_msgs.Marker, 'path_markers', 3000) self.pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'pcd', 10) self.coverable_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'coverable_pcd', 100) self.visited_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'visited_pcd', 100) self.visited_ground_pcd_pub = self.create_publisher( sensor_msgs.PointCloud2, 'visited_ground_pcd', 100) self.traversable_pcd_pub = self.create_publisher( sensor_msgs.PointCloud2, 'traversable_pcd', 100) self.inaccessible_pcd_pub = self.create_publisher( sensor_msgs.PointCloud2, 'inaccessible_pcd', 100) self.path_pub = self.create_publisher(nav_msgs.Path, 'path', 10) with open(FILE, 'rb') as cached_pcd_file: cache_data = pickle.load(cached_pcd_file) all_results = deepcopy(cache_data) path = list(filter(lambda x: x["ns"] == PATH_NS, all_results))[0]["path"] #Create Environment environment = PointCloudEnvironment( self.print, "garage_terrain_assessment.dictionary", "garage.pcd", False) point_cloud = environment.full_pcd traversable_point_cloud = environment.traversable_pcd coverable_point_cloud = environment.coverable_pcd motion_planner = MotionPlanner(self.print, traversable_point_cloud) self.pcd_pub.publish( point_cloud.point_cloud(point_cloud.points, 'my_frame')) self.coverable_pcd_pub.publish( coverable_point_cloud.point_cloud(coverable_point_cloud.points, 'my_frame')) self.traversable_pcd_pub.publish( traversable_point_cloud.point_cloud(traversable_point_cloud.points, 'my_frame')) current_max = 0 while current_max < len(path): self.markers_msg = visualization_msgs.MarkerArray() stamp = self.get_clock().now().to_msg() msg = ROSMessage.line_marker(0, stamp, path[0:current_max], [0.1, 1.0, 1.0], PATH_NS) self.markers_msg.markers = [msg] current_max += 10 self.markers_pub.publish(self.markers_msg)
def publishTagMarkers(tags, publisher): """Republishes the tag_markers topic""" SHIFT = 0.05 h = std_msgs.Header(stamp=rospy.Time.now(), frame_id='map') ca = std_msgs.ColorRGBA(r=1, a=1) cb = std_msgs.ColorRGBA(g=1, a=1) ct = std_msgs.ColorRGBA(b=1, a=1) sa = geometry_msgs.Vector3(x=0.1, y=0.5, z=0.5) sb = geometry_msgs.Vector3(x=0.1, y=0.25, z=0.25) st = geometry_msgs.Vector3(x=1, y=1, z=1) ma = visualization_msgs.MarkerArray() ma.markers.append( visualization_msgs.Marker( header=h, id=-1, action=visualization_msgs.Marker.DELETEALL)) for i, t in enumerate(tags): th = t['th_deg'] * math.pi / 180. q = tf.transformations.quaternion_from_euler(0, 0, th) q = geometry_msgs.Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) ma.markers.append( visualization_msgs.Marker( header=h, id=i * 3, type=visualization_msgs.Marker.CUBE, action=visualization_msgs.Marker.ADD, pose=geometry_msgs.Pose( position=geometry_msgs.Point(x=t['x'], y=t['y']), orientation=q), scale=sa, color=ca)) ma.markers.append( visualization_msgs.Marker( header=h, id=i * 3 + 1, type=visualization_msgs.Marker.CUBE, action=visualization_msgs.Marker.ADD, pose=geometry_msgs.Pose( position=geometry_msgs.Point( x=t['x'] + SHIFT * math.cos(th), y=t['y'] + SHIFT * math.sin(th)), orientation=q), scale=sb, color=cb)) ma.markers.append( visualization_msgs.Marker( header=h, id=i * 3 + 2, type=visualization_msgs.Marker.TEXT_VIEW_FACING, action=visualization_msgs.Marker.ADD, pose=geometry_msgs.Pose( position=geometry_msgs.Point(x=t[1], y=t[2], z=0.5), orientation=q), scale=st, color=ct, text=str(t['id']))) publisher.publish(ma)
def marker_publisher(self, max=None): self.markers_msg = visualization_msgs.MarkerArray() for idx, marker in enumerate(self.markers[0:max]): stamp = self.get_clock().now().to_msg() msg = ROSMessage.point_marker(self.last_id, stamp, marker["point"], marker["color"], str(idx)) self.markers_msg.markers.append(msg) self.last_id += 1 self.markers_pub.publish(self.markers_msg)
def markcb(self, msg): self.count += 1 if self.count%FREQ_DIV == 0: # update all of the marker paths for m in msg.markers: if m.id not in self.paths.keys() and m.type == VM.Marker.SPHERE: self.paths[m.id] = Path(m) elif m.id in self.paths.keys(): self.paths[m.id].update_path(m) # now we can assemble a MarkerArray and publish: ma = VM.MarkerArray() ma.markers = [a.marker for a in self.paths.values()] self.path_pub.publish(ma) return
def visualize(self, samples, costs): marker_array = vm.MarkerArray() for i, (sample, cost) in enumerate(zip(samples, costs)): if sample is None: continue origin = np.array([0., 0., 0.]) angle = 0. for t in xrange(len(sample.get_U())): marker = vm.Marker() marker.id = i * len(sample.get_U()) + t marker.header.frame_id = '/map' marker.type = marker.ARROW marker.action = marker.ADD speed = sample.get_U(t=t, sub_control='cmd_vel')[0] / 5. steer = sample.get_U(t=t, sub_control='cmd_steer')[0] angle += (steer - 50.) / 100. * (np.pi / 2) new_origin = origin + [ speed * np.cos(angle), speed * np.sin(angle), 0. ] marker.points = [ gm.Point(*origin.tolist()), gm.Point(*new_origin.tolist()) ] origin = new_origin marker.lifetime = rospy.Duration() marker.scale.x = 0.05 marker.scale.y = 0.1 marker.scale.z = 0.1 if cost == min(costs): rgba = (0., 1., 0., 1.) marker.scale.x *= 2 marker.scale.y *= 2 marker.scale.z *= 2 else: rgba = list(cm.hot(1 - cost)) rgba[-1] = 0.5 marker.color.r, marker.color.g, marker.color.b, marker.color.a = rgba marker_array.markers.append(marker) # for id in xrange(marker.id+1, int(1e4)): # marker_array.markers.append(vm.Marker(id=id, action=marker.DELETE)) self.debug_cost_probcoll_pub.publish(marker_array)
def marker_publisher(self, max=None): self.markers_msg = visualization_msgs.MarkerArray() self.last_id = 0 for idx, path in enumerate([]): stamp = self.get_clock().now().to_msg() if path.get("path", False) is False: path["path"] = self.prev_file[idx]["path"] msg = ROSMessage.line_marker(self.last_id, stamp, path["path"], [0.1,1.0,1.0], path["ns"]) self.markers_msg.markers.append(msg) self.last_id += 1 self.markers_pub.publish(self.markers_msg)
def run(self): while not rospy.is_shutdown(): if self.last_msg: rospy.loginfo('Press enter to publish') raw_input() current_msg = vis_msg.MarkerArray() current_msg.markers = self.last_msg.markers[ self.current_start:self.current_start + self.n] for marker in current_msg.markers: marker.lifetime = rospy.Duration(0) marker.header.stamp = rospy.Time() self.pub.publish(current_msg) self._update() print "published" print "{0} out of {1} published".format( self.current_start + self.n, len(self.last_msg.markers))
def send_transforms(self, event): tnow = rospy.Time.now() mlist = [] for i, con in enumerate(self.controllers): quat = con.quat if self.running_flag: pos = con.act_pos con.update_marker_pose(pos) else: pos = con.simpos con.update_marker_pose(pos) self.br.sendTransform(pos, quat, tnow, con.conframe, SIMWF) con.marker.header.stamp = tnow mlist.append(con.marker) ma = VM.MarkerArray() ma.markers = mlist self.marker_pub.publish(ma) return
def send_joint_states(self, event): tnow = rospy.Time.now() # first send the transform: quat = tuple(tf.transformations.quaternion_from_euler( self.sys.get_config('TorsoPsi').q, self.sys.get_config('TorsoTheta').q, self.sys.get_config('TorsoPhi').q, 'rzyx')) point = tuple(( self.sys.get_config('TorsoX').q, self.sys.get_config('TorsoY').q, self.sys.get_config('TorsoZ').q )) self.br.sendTransform(point, quat, tnow, 'torso', 'world') self.js.header.stamp = tnow self.joint_pub.publish(self.js) # send transforms for i in 4,5: #hands self.send_single_transform(i, tnow) if self.shoulders_bool: for i in 2,3: #shoulders self.send_single_transform(i, tnow) else: #body self.send_single_transform(1, tnow) if self.legs_bool: for i in 6,7: #legs self.send_single_transform(i, tnow) # update the constraint visualization: mlist = [] for c in self.con_vis: c.update_points() mlist.append(c.marker) ma = VM.MarkerArray() ma.markers = mlist self.con_pub.publish(ma)
def main(): # Initialization rospy.init_node('objrec_spoofer') # Get spoofer parameters frame_id = rospy.get_param('~frame_id','/world') publish_period = rospy.get_param('~publish_period',1.0) publish_rate = rospy.Rate(1.0/publish_period) # Get the model info models = rospy.get_param('~models') stl_uris = {} for model in models: stl_uris[model] = rospy.get_param('~stl_uris/'+model) # Get objects from rosparam spoofed_objects = rospy.get_param('~spoofed_objects') # Create the publishers obj_pub = rospy.Publisher('/recognized_objects',objrec_msgs.RecognizedObjects) obj_marker_pub = rospy.Publisher('/recognized_objects_markers',visualization_msgs.MarkerArray) while not rospy.is_shutdown(): # Initialize objects message objects_msg = objrec_msgs.RecognizedObjects() objects_msg.header.stamp = rospy.Time.now() objects_msg.header.frame_id = frame_id # Initialize marker array message marker_array = visualization_msgs.MarkerArray() marker_id = 0 for obj in spoofed_objects: # Construct and populate an object message pss_msg = objrec_msgs.PointSetShape() pss_msg.label = obj['label'] pss_msg.confidence = obj['confidence'] pss_msg.pose = geometry_msgs.Pose(geometry_msgs.Point(*obj['position']), geometry_msgs.Quaternion(*obj['orientation'])) # Store the message objects_msg.objects.append(pss_msg) # Construct and populate a marker message marker = visualization_msgs.Marker() marker.header = objects_msg.header marker.ns = "objrec" marker.type = visualization_msgs.Marker.MESH_RESOURCE marker.action = visualization_msgs.Marker.ADD marker.lifetime = rospy.Duration(publish_period) marker.scale = geometry_msgs.Vector3(0.001, 0.001, 0.001) marker.color = std_msgs.ColorRGBA(1.0, 0.2, 0.4, 0.5) marker.id = marker_id marker.pose = pss_msg.pose marker.mesh_resource = stl_uris[obj['label']] # Store the message marker_array.markers.append(marker) marker_id += 1 # Publish the object information obj_pub.publish(objects_msg) # Publish the markers obj_marker_pub.publish(marker_array) publish_rate.sleep()
def __init__(self): super().__init__('MainNode') #Publishers: self.markers_pub = self.create_publisher( visualization_msgs.MarkerArray, 'marker', 3000) self.markers_path_pub = self.create_publisher( visualization_msgs.Marker, 'path_markers', 3000) self.pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'pcd', 10) self.coverable_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'coverable_pcd', 100) self.visited_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'visited_pcd', 100) self.visited_ground_pcd_pub = self.create_publisher( sensor_msgs.PointCloud2, 'visited_ground_pcd', 100) self.traversable_pcd_pub = self.create_publisher( sensor_msgs.PointCloud2, 'traversable_pcd', 100) self.inaccessible_pcd_pub = self.create_publisher( sensor_msgs.PointCloud2, 'inaccessible_pcd', 100) self.path_pub = self.create_publisher(nav_msgs.Path, 'path', 10) #Create Environment environment = MapEnvironment(self.print, "simple_open.dictionary", "src/exjobb/maps/map_simple_open.png", 0.015) point_cloud = environment.full_pcd traversable_point_cloud = environment.traversable_pcd coverable_point_cloud = environment.coverable_pcd motion_planner = MotionPlanner(self.print, traversable_point_cloud) TIME_LIMIT = 400 #start_point = np.array([5,0.55,0]) #spiral start_point = np.array([0.55, 0.7, 0]) goal_coverage = 1 paths_markers = [] #Get CPP path current = "BA*" current_param = { 'angle_offset': 0, 'step_size': 0.5, 'visited_threshold': 0.375 } if current == "BA*": cpp = BAstar(self.print, motion_planner, coverable_point_cloud, time_limit=TIME_LIMIT, parameters=current_param) if current == "Inward Spiral": cpp = Spiral(self.print, motion_planner, coverable_point_cloud, time_limit=TIME_LIMIT, parameters=current_param) self.pcd_pub.publish( point_cloud.point_cloud(point_cloud.points, 'my_frame')) self.coverable_pcd_pub.publish( coverable_point_cloud.point_cloud(coverable_point_cloud.points, 'my_frame')) self.traversable_pcd_pub.publish( traversable_point_cloud.point_cloud(traversable_point_cloud.points, 'my_frame')) path = cpp.get_cpp_path(start_point, goal_coverage=goal_coverage) self.print(cpp.print_stats(path)) current_max = 0 coverable_point_cloud.covered_points_idx = np.array([]) while current_max < len(path): self.markers_msg = visualization_msgs.MarkerArray() stamp = self.get_clock().now().to_msg() msg = ROSMessage.line_marker(0, stamp, path[0:current_max], [0.1, 1.0, 1.0], "path") self.markers_msg.markers = [msg] current_max += 2 new_path = path[current_max - 2:current_max] self.markers_pub.publish(self.markers_msg) #if current_max > 2: coverable_point_cloud.visit_path(new_path) visited_ground_points_pcd = coverable_point_cloud.get_covered_points_as_pcd( ) self.visited_ground_pcd_pub.publish(visited_ground_points_pcd)
def __init__(self): super().__init__('MainNode') #Publishers: self.markers_pub = self.create_publisher(visualization_msgs.MarkerArray, 'marker', 3000) self.markers_path_pub = self.create_publisher(visualization_msgs.Marker, 'path_markers', 3000) self.pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'pcd', 10) self.coverable_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'coverable_pcd', 100) self.visited_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'visited_pcd', 100) self.visited_ground_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'visited_ground_pcd', 100) self.traversable_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'traversable_pcd', 100) self.inaccessible_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'inaccessible_pcd', 100) self.path_pub = self.create_publisher(nav_msgs.Path, 'path', 10) with open(FILE, 'rb') as cached_pcd_file: cache_data = pickle.load(cached_pcd_file) all_results = deepcopy(cache_data) param = list(filter(lambda x: x["ns"] == PATH_NS, all_results))[0]["param"] cpp_name = list(filter(lambda x: x["ns"] == PATH_NS, all_results))[0]["cpp"] #Create Environment environment = PointCloudEnvironment(self.print, "garage_terrain_assessment.dictionary", "garage.pcd", False) point_cloud = environment.full_pcd traversable_point_cloud = environment.traversable_pcd coverable_point_cloud = environment.coverable_pcd motion_planner = MotionPlanner(self.print, traversable_point_cloud) TIME_LIMIT = 400 goal_coverage = 0.97 start_point = np.array([28.6, -6.7, -10.3]) #garage #start_point = np.array([-53.7, 54.2, -2.7]) #bridge #start_point = np.array([-20.7, 43, -1]) #cross if cpp_name == "BA*": cpp = BAstar(self.print, motion_planner, coverable_point_cloud, time_limit=TIME_LIMIT, parameters = param) if cpp_name == "Inward Spiral": cpp = Spiral(self.print, motion_planner, coverable_point_cloud, time_limit=TIME_LIMIT, parameters = param) if cpp_name == "Sampled BA*": cpp = RandomBAstar3(self.print, motion_planner, coverable_point_cloud, time_limit=TIME_LIMIT, parameters = param) path = cpp.get_cpp_path(start_point, goal_coverage=goal_coverage) all_segments = cpp.all_segments all_movements = cpp.all_movements markers = cpp.points_to_mark self.pcd_pub.publish(point_cloud.point_cloud(point_cloud.points, 'my_frame')) self.coverable_pcd_pub.publish(coverable_point_cloud.point_cloud(coverable_point_cloud.points, 'my_frame')) self.traversable_pcd_pub.publish(traversable_point_cloud.point_cloud(traversable_point_cloud.points, 'my_frame')) self.markers_msg = visualization_msgs.MarkerArray() self.last_id = 0 ANIMATION = True if not ANIMATION: #Only markers for idx, marker in enumerate(markers): stamp = self.get_clock().now().to_msg() msg = ROSMessage.point_marker(self.last_id, stamp, marker["point"], marker["color"], "markers") self.markers_msg.markers.append(msg) self.last_id += 1 #Add segments coverable_point_cloud.covered_points_idx = np.array([]) for idx, segment in enumerate(all_segments): if len(segment.path) == 0: #all_movements.pop(idx) continue stamp = self.get_clock().now().to_msg() msg = ROSMessage.line_marker(self.last_id, stamp, segment.path, [0.1,1.0,1.0], "segment"+str(idx)) self.markers_msg.markers.append(msg) self.last_id += 1 coverable_point_cloud.visit_path(segment.path) if ANIMATION: visited_ground_points_pcd = coverable_point_cloud.get_covered_points_as_pcd() self.visited_ground_pcd_pub.publish(visited_ground_points_pcd) self.markers_pub.publish(self.markers_msg) time.sleep(1) #Add Movements for idx, move in enumerate(all_movements): stamp = self.get_clock().now().to_msg() msg = ROSMessage.line_marker(self.last_id, stamp, move.path, [1.0,0.5,0.5], "move"+str(idx)) self.markers_msg.markers.append(msg) self.last_id += 1 visited_ground_points_pcd = coverable_point_cloud.get_covered_points_as_pcd() self.visited_ground_pcd_pub.publish(visited_ground_points_pcd) self.markers_pub.publish(self.markers_msg) return current_max = 0 while current_max < len(path): self.markers_msg = visualization_msgs.MarkerArray() stamp = self.get_clock().now().to_msg() msg = ROSMessage.line_marker(0, stamp, path[0:current_max], [0.1,1.0,1.0], PATH_NS) self.markers_msg.markers = [msg] current_max += 10 self.markers_pub.publish(self.markers_msg)