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(