def __init__(self): self.ogm = OccupancyGridManager('/map', subscribe_to_updates=True) self.pub = rospy.Publisher('/closest_cell_cost', PointStamped, queue_size=1) # Can use 'Publish Point' in Rviz rospy.Subscriber('/clicked_point', PointStamped, self.point_cb)
class OGMTesterCellCost(object): def __init__(self): self.ogm = OccupancyGridManager('/map', subscribe_to_updates=True) self.pub = rospy.Publisher('/closest_cell_cost', PointStamped, queue_size=1) # Can use 'Publish Point' in Rviz rospy.Subscriber('/clicked_point', PointStamped, self.point_cb) def point_cb(self, msg): cost = self.ogm.get_cost_from_world_x_y(msg.point.x, msg.point.y) rospy.loginfo("Cost at (x, y): {}, {} is {}".format( msg.point.x, msg.point.y, cost)) costmap_x, costmap_y = self.ogm.get_costmap_x_y( msg.point.x, msg.point.y) rospy.loginfo("Which is in costmap coords (x, y): {}, {}".format( costmap_x, costmap_y)) # Publish a PointStamped of the closest cell over 99 cost (black in map) closest_cell_x, closest_cell_y, cost = self.ogm.get_closest_cell_over_cost( costmap_x, costmap_y, 99, 9) if closest_cell_x != -1: p = PointStamped() p.header.frame_id = self.ogm.reference_frame p.point.x, p.point.y = self.ogm.get_world_x_y( closest_cell_x, closest_cell_y) self.pub.publish(p) def run(self): rospy.spin()
def __init__(self): rospy.init_node('OGM_tester') self.ogm = OccupancyGridManager( '/wheelchair/move_base/global_costmap/costmap', subscribe_to_updates=True) self.last_costmap = None self.point_pub = rospy.Publisher('/safe_point', PointStamped, queue_size=1) rospy.Subscriber('/clicked_point', PointStamped, self.point_cb)
class OGMTester(object): def __init__(self): self.ogm = OccupancyGridManager('/map', subscribe_to_updates=True) # Can use 'Publish Point' in Rviz rospy.Subscriber('/clicked_point', PointStamped, self.point_cb) def point_cb(self, msg): cost = self.ogm.get_cost_from_world_x_y(msg.point.x, msg.point.y) rospy.loginfo("Cost at (x, y): {}, {} is {}".format( msg.point.x, msg.point.y, cost)) costmap_x, costmap_y = self.ogm.get_costmap_x_y( msg.point.x, msg.point.y) rospy.loginfo("Which is in costmap coords (x, y): {}, {}".format( costmap_x, costmap_y)) def run(self): rospy.spin()
class OGMtester(object): def __init__(self): rospy.init_node('OGM_tester') self.ogm = OccupancyGridManager( '/wheelchair/move_base/global_costmap/costmap', subscribe_to_updates=True) self.last_costmap = None self.point_pub = rospy.Publisher('/safe_point', PointStamped, queue_size=1) rospy.Subscriber('/clicked_point', PointStamped, self.point_cb) def point_cb(self, msg): #print(self.ogm.origin) cost = self.ogm.get_cost_from_world_x_y(msg.point.x, msg.point.y) rospy.loginfo("cost {};{} is {}".format(int(msg.point.x), int(msg.point.y), cost)) if cost > 50: costmap_x, costmap_y = self.ogm.get_costmap_x_y( msg.point.x, msg.point.y) safe_x, safe_y, safe_cost = self.ogm.get_closest_cell_under_cost( costmap_x, costmap_y, 50, 10) rospy.loginfo("{} {} {}".format(safe_x, safe_y, safe_cost)) if safe_cost != -1 and safe_x != -1 and safe_y != -1: safe_point = msg safe_point.point.x, safe_point.point.y = self.ogm.get_world_x_y( safe_x, safe_y) safe_point.point.z = 0.2 self.point_pub.publish(safe_point) def run(self): rospy.spin()
def __init__(self): self.ogm = OccupancyGridManager('/map', subscribe_to_updates=True) # Can use 'Publish Point' in Rviz rospy.Subscriber('/clicked_point', PointStamped, self.point_cb)
rospy.sleep(2.0) rospy.loginfo("Done") rp = RosPack() pkg_path = rp.get_path('occupancy_grid_python') global_costmap_msg = cPickle.load( open(pkg_path + '/data/global_costmap.pickle')) local_costmap_msg = cPickle.load( open(pkg_path + '/data/local_costmap.pickle')) map_msg = cPickle.load(open(pkg_path + '/data/map.pickle')) global_costmap_pub.publish(global_costmap_msg) local_costmap_pub.publish(local_costmap_msg) map_pub.publish(map_msg) rospy.loginfo("Done publishing example data") ogm = OccupancyGridManager('/move_base/global_costmap/costmap', subscribe_to_updates=False) wx1, wy1 = ogm.get_world_x_y(0, 0) print("world from costmap coords 0 0: ") print((wx1, wy1)) print("Which, the origin from ogm is:") print(ogm.origin) cx1, cy1 = ogm.get_costmap_x_y(wx1, wy1) print("Costmap from world: ") print((cx1, cy1)) cx2, cy2 = ogm.get_costmap_x_y(0.0, 0.0) print("costmap from world coords 0 0: ") print((cx2, cy2)) wx2, wy2 = ogm.get_world_x_y(cx2, cy2) print("back to world: ")
occgrid.data = [i for i in range(0, 20)] # [[ 0 1 2 3 4 5 6 7 8 9] # [10 11 12 13 14 15 16 17 18 19]] # but interpreted in Rviz costmap as # [10 11 12 13 14 15 16 17 18 19] # [ 0 1 2 3 4 5 6 7 8 9] # Initialize publishers... rospy.sleep(3.0) map_pub.publish(occgrid) rospy.sleep(1.0) ogm = OccupancyGridManager('/map', subscribe_to_updates=False) for y in range(2): # row for x in range(10): # column if ogm.is_in_gridmap(x, y): cost = ogm.get_cost_from_costmap_x_y(x, y) print("x: {}, y: {} is in gridmap (cost: {})".format( x, y, cost)) else: print("x: {}, y: {} is NOT in gridmap".format(x, y)) if ogm.is_in_gridmap(ogm.width - 1, ogm.height - 1): print("ogm.width - 1: {}, ogm.height - 1: {} is in gridmap.".format( ogm.width - 1, ogm.height - 1)) else: print(