def callback(data):
global ranges
global thetasens
print 'camera'
thetasens = np.linspace(angle_min, angle_max, 640)
ranges = []
for a in range(0, 639):
ranges.append(data.ranges[a])
mymap = OccupancyGrid(header=Header(seq=0,
stamp=rospy.Time.now(),
frame_id="map"),
info=MapMetaData(width=100,
height=100,
resolution=0.1,
map_load_time=rospy.Time.now()))
mymap_thresh = OccupancyGrid(header=Header(seq=0,
stamp=rospy.Time.now(),
frame_id="map"),
info=MapMetaData(
width=100,
height=100,
resolution=0.1,
map_load_time=rospy.Time.now()))
k = -2
for i in range(0, 100):
m = -2
for j in range(0, 100):
test = inverse_range_sensor_model(m, k, positionx, positiony, yaw,
ranges)
if test != 0:
occupancy[i][j] = occupancy[i][j] + test
mymap.data.append(logodds(occupancy[i][j]) * 100)
if logodds(occupancy[i][j]) > 0.75:
mymap_thresh.data.append(100)
elif logodds(occupancy[i][j]) < 0.25:
mymap_thresh.data.append(0)
else:
mymap_thresh.data.append(50)
else:
mymap.data.append(logodds(occupancy[i][j]) * 100)
if logodds(occupancy[i][j]) > 0.75:
mymap_thresh.data.append(100)
elif logodds(occupancy[i][j]) < 0.25:
mymap_thresh.data.append(0)
else:
mymap_thresh.data.append(50)
m = m + 0.1
k = k + 0.1
publisher = rospy.Publisher('/map', OccupancyGrid, queue_size=1)
publisher2 = rospy.Publisher('/map2', OccupancyGrid, queue_size=1)
rate = rospy.Rate(10)
publisher.publish(mymap)
publisher2.publish(mymap_thresh)
def combine_maps(self, map1, map2, map1_metadata, map2_metadata):
"""
The algo: stitch map2 into map1, using the resolution of map1
1. Get xmin/max, ymin/ymax of both maps.
2. Build two empty occupancy grids of the correct shape.
3. Put each map into these occupancy grids (scale map2)
4. Combine maps with max.
"""
metadata_out = MapMetaData()
m1_r = map1_metadata.resolution
m2_r = map2_metadata.resolution
m1_xmin = map1_metadata.origin.position.x
m1_ymin = map1_metadata.origin.position.y
m2_xmin = map2_metadata.origin.position.x
m2_ymin = map2_metadata.origin.position.y
m1_w = map1_metadata.width
m1_h = map1_metadata.height
m2_w = map2_metadata.width
m2_h = map2_metadata.height
m1_xmax = m1_xmin + m1_r * m1_w
m2_xmax = m2_xmin + m2_r * m2_w
m1_ymax = m1_ymin + m1_r * m1_h
m2_ymax = m2_ymin + m2_r * m2_h
map2_2_map1_scaling = m2_r / m1_r
metadata_out.resolution = map1_metadata.resolution
metadata_out.origin.position.x = min(m1_xmin, m2_xmin)
metadata_out.origin.position.y = min(m1_ymin, m2_ymin)
out_xmax = max(m1_xmax, m2_xmax)
out_ymax = max(m1_ymax, m2_ymax)
metadata_out.width = int((out_xmax - metadata_out.origin.position.x) /
metadata_out.resolution) + 1
metadata_out.height = int((out_ymax - metadata_out.origin.position.y) /
metadata_out.resolution) + 1
map2_scaled = rescale(map2,
map2_2_map1_scaling,
multichannel=False,
anti_aliasing=False)
map1_acc = np.zeros([metadata_out.width, metadata_out.height])
map2_acc = np.zeros([metadata_out.width, metadata_out.height])
m1_start_x = int((m1_xmin - metadata_out.origin.position.x) /
metadata_out.resolution)
m1_start_y = int((m1_ymin - metadata_out.origin.position.y) /
metadata_out.resolution)
m2_start_x = int((m2_xmin - metadata_out.origin.position.x) /
metadata_out.resolution)
m2_start_y = int((m2_ymin - metadata_out.origin.position.y) /
metadata_out.resolution)
map1_acc[m1_start_x:m1_start_x + map1.shape[0],
m1_start_y:m1_start_y + map1.shape[1]] = np.copy(map1)
map2_acc[m2_start_y:m2_start_y + map2_scaled.shape[0],
m2_start_x:m2_start_x +
map2_scaled.shape[1]] = np.copy(map2_scaled)
out = np.maximum(map1_acc, map2_acc)
return out, metadata_out
def __init__(self,
res,
width,
height,
starting_pose,
topic_name='/search_grid'):
self.meta_data = MapMetaData()
# Resolution is m/cell. Width is X, height is Y.
self.meta_data.resolution = res # rospy.get_param("map_resolution")
self.meta_data.width = width # rospy.get_param("map_width")
self.meta_data.height = height # rospy.get_param("map_height")
# Starting Position
self.mid_x = -starting_pose.x
self.mid_y = -starting_pose.y
p = Point(x=-starting_pose.x * res, y=-starting_pose.y * res, z=0)
q = Quaternion(*tf.transformations.quaternion_from_euler(
0, 0, starting_pose.theta))
self.meta_data.origin = Pose(position=p, orientation=q)
rospy.Service('/reset_occ_grid', Empty, self.reset_grid)
# Create array of -1's of the correct size
self.occ_grid = np.zeros(
(self.meta_data.height, self.meta_data.width)) - 1
self.searched = np.zeros((self.meta_data.height, self.meta_data.width))
self.markers = np.zeros((self.meta_data.height, self.meta_data.width))
self.occ_grid_pub = rospy.Publisher(topic_name,
OccupancyGrid,
queue_size=1)
self.searcher = Searcher(self.searched, res, [self.mid_x, self.mid_y])
def __call__(self, msg):
costmap = self._handler.costmap()
if costmap is not None:
cells = costmap.cells
rows, cols = cells.shape
cells = cv2.resize(cells, (rows / self._visualization_scale,
cols / self._visualization_scale))
origin = costmap.origin / self._visualization_scale
resolution = costmap.resolution * self._visualization_scale
msg = OccupancyGrid()
msg.header.frame_id = "map"
msg.header.stamp = costmap.stamp
msg.info = MapMetaData(
map_load_time=rospy.Time.now(),
resolution=resolution,
width=cells.shape[0],
height=cells.shape[1],
origin=Pose(position=Point(origin[1] * resolution,
origin[0] * resolution, 0),
orientation=Quaternion(0, 0, 0, 1)))
data = cells.flatten()
data_out = np.full(data.shape, 100)
mask = data <= 0.99
data_out[mask] = (data[mask] * 98 + 0.5).astype(int)
msg.data = data_out
try:
self._pub.publish(msg)
except rospy.ROSException as e:
rospy.logdebug("GridMap2DListener: {}".format(e))
def __init__(self):
self.map_data = MapMetaData()
self.trace_pub = rospy.Publisher('/raytrace_output',
occupancy_update,
queue_size=5)
self.meta_sub = rospy.Subscriber('/map_meta_data',
MapMetaData,
callback=self.NewMetaData)
self.vis_pub = rospy.Publisher('/raytrace_cloud',
PointCloud,
queue_size=5)
self.pose_sub = rospy.Subscriber('/ekf_est',
PoseStamped,
callback=self.pose_callback)
self.laser_sub = rospy.Subscriber('/scan',
LaserScan,
callback=self.laser_callback)
self.tf_listener = tf.TransformListener()
#time.sleep(10.0)
self.pose = None
self.have_pose = False
self.scan = None
self.have_scan = False
self.recieved_data = False
def __init__(self):
rospy.init_node('camera_observed_area_tracker')
rospy.loginfo('Camera observed area tracker node started')
self.pose_converted = PoseConverted()
self.pose = Pose()
self.map = [[]]
self.map_info = MapMetaData()
self.map_camera_seen_initialised = False
self.map_camera = [[]]
self.map_camera_seen_seq = 0
self.robot_x_old = 0
self.robot_y_old = 0
self.robot_yaw_old = 0
# --- Publisher ---
self.pub_seen_map = rospy.Publisher('camera_seen_map',
OccupancyGrid,
queue_size=1)
# --- Subscriber ---
self.pose_subscriber = rospy.Subscriber('simple_odom_pose', CustomPose,
self._handle_pose_update)
self.map_sub = rospy.Subscriber('map', OccupancyGrid,
self._handle_map_update)
self._setup_camera_seen_masks()
rospy.loginfo("--- ready ---")
rospy.spin()
def __init__(self):
rospy.on_shutdown(self._shutdown)
rospy.init_node('tag_manager')
self.abs_file_path = rospy.get_param(
"~tag_file",
default=
"/home/daniel/catkin_ws/src/tag_manager/StoredTag/tags.store")
self.tag_list = []
self.tag_detection_radius = 10
self._load_tags()
self.map_info = MapMetaData()
self.markerArray = MarkerArray()
# --- Publisher ---
self.marker_publisher = rospy.Publisher('visualization_marker_array',
MarkerArray,
queue_size=100)
# --- Services ---
self.check_tag_known_service = rospy.Service(
'check_tag_known', CheckTagKnown, self._handle_check_tag_known)
self.add_tag_service = rospy.Service('add_tag', AddTag,
self._handle_add_tag)
self.get_tags_service = rospy.Service('get_tags', GetTags,
self._handle_get_tags)
self._setup()
rospy.spin()
def __init__(self, map_name):
self.name = map_name
self.raw_msg = OccupancyGrid() # OccupancyGrid message goes here
self.map_meta = MapMetaData() # MapMetaData
self.map = np.array([])
self.map_im = np.array([])
self.roi = np.array([])
self.frontiers = []
self.poi = []
self.debug_markers = []
self.map_is_loaded = False
# ROI
self.roi_origin = Pose()
self.roi_max = Point()
self.roi_min = Point()
# HDBSCAN
self.clusterer = hdbscan.HDBSCAN(min_cluster_size=50)
# parameters
self.occupancy_thresh = 50 # % probability of occupancy before considering as occupied.
self.map_roi_boundary = 5 # extra cells to keep around known map
# Plotting stuff
# sns.set_context('poster')
# sns.set_color_codes()
# main map window
cv2.namedWindow('map_im', cv2.WINDOW_NORMAL)
def publishMap():
meta = MapMetaData()
meta.width = 10
meta.height = 10
meta.resolution = 0.2
for c in range(-1,100):
map = OccupancyGrid()
map.header.frame_id = 'map'
for i in range(0,100):
val = c + i
if val > 100:
val = val - 101
map.data.append(val)
map.info = meta
mapPub.publish(map)
publishGrid()
def getOccupancyGrid(matrix, scale):
stamp = rospy.Time.now()
oc = OccupancyGrid()
oc.header.frame_id = HEADER_FRAME
oc.header.stamp = stamp
# The time at which the map was loaded
load_time = stamp
# The origin of the map [m, m, rad]. This is the real-world pose of the
# cell (0,0) in the map.
origin = Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 0))
matrix = matrix[::-1]
width = len(matrix[0])
height = len(matrix)
oc.info = MapMetaData(load_time, scale, width, height, origin)
for row in range(height):
for col in range(width):
c = matrix[row][col]
if c == 0:
oc.data.append(100)
elif c == 1:
oc.data.append(0)
else:
oc.data.append(50)
return oc, width * scale, height * scale
def __init__(self):
#Declare the variables required for the program
self.map = OccupancyGrid()
self.metadata = MapMetaData()
self.pose = PoseWithCovarianceStamped()
self.point = Point()
self.droneArray = []
self.NUMBER_OF_ILLETERATIONS = 1000
self.pheromonePath = []
rospy.loginfo("Starting aco_ros...")
##########################Drone Objects#############################
self.myTopics = [[]] #Expect a double array in rospyPubList
self.myTopics = rospy.get_published_topics()
matching = [element for element in self.myTopics if "/droneLaser" in element]
matching.sort()
droneNumber = len(matching) #Match the amount of drones to the amount of laserscans we found
#rospy.loginfo(str(droneNumber))
#################Subscribers for maps ##############################
rospy.Subscriber("/map",OccupancyGrid, self.callForMap)
#rospy.Subscriber("/map_metadata",MapMetaData,self.callForMetadata)
rospy.Subscriber("/initialpose",PoseWithCovarianceStamped, self.callFinalPose)
rospy.Subscriber("/goal",Point,self.callPoint)
####################################################Run calcuations
if (droneNumber != 0):
i = 1 #counter
while (i != droneNumber): #Instantciate an array of drone Objects
self.droneArray[i] = Drone(i,"drone_"+str(i),True,False,self.point,"/myDrone") #Set drone name and set the sensors that work at the moment
rospy.loginfo("Found drone" + str(i))
i += 1
self.main()
else:
rospy.loginfo("No drones to spawn!")
def __init__(self):
rospy.init_node('turtlebot_state_machine', anonymous=True)
self.mode = Mode.EXPLORE
# current nav cmd
self.cmd_nav = Pose2D()
# map
self.map = OccupancyGrid()
self.map_meta_data = MapMetaData()
self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx, self.kdy, self.v_max, self.om_max)
self.pose_controller = PoseController(0., 0., 0., self.v_max, self.om_max)
self.heading_controller = HeadingController(self.kp_th, self.om_max)
# publishers
self.map_pub = rospy.Publisher('/map', OccupancyGrid, queue_size=10)
self.map_meta_data_pub = rospy.Publisher('/map_meta_data', MapMetaData, queue_size=10)
self.cmd_nav_pub = rospy.Publisher('/cmd_nav',Pose2D, queue_size=10)
# subscribers
#rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
#rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
#rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)
print "finished init"
def __init__(self):
# Initialize an empty message
self.data = -np.ones((1, 1))
self.info = MapMetaData()
self.info.width = 1
self.info.height = 1
# Initialize space for the dilated grid
self.grid_dilated = np.zeros((1, 1), dtype=np.int8)
# Publish a list of occupied points
self.occupied_pub = rospy.Publisher(self.OCCUPIED_TOPIC,
Marker,
queue_size=1)
# Publish to the map
self.map_pub = rospy.Publisher(self.CARTOGRAPHER_DILATED_TOPIC,
numpy_msg(OccupancyGrid),
queue_size=1)
# Subscribe to the map
self.map_sub = rospy.Subscriber(self.CARTOGRAPHER_TOPIC,
numpy_msg(OccupancyGrid),
self.map_cb,
queue_size=1)
def heightmap_msg(heightmap, metadata):
"""
Build the heightmap from the given numpy file and metadata
For now, say that each elem in occupancy grid = 1cm
"""
msg = OccupancyGrid()
msg.header.frame_id = '/map'
hmap_int = (heightmap * 100).astype(np.uint8)
msg.data = hmap_int.flatten()
msg.info = MapMetaData(resolution=metadata['resoluion'],
width=metadata['width'],
height=metadata['height'],
origin=Pose(
position=Point(
x=metadata['origin']['position']['x'],
y=metadata['origin']['position']['y'],
z=metadata['origin']['position']['z'],
),
orientation=Quaternion(
x=metadata['origin']['orientation']['x'],
y=metadata['origin']['orientation']['y'],
z=metadata['origin']['orientation']['z'],
w=metadata['origin']['orientation']['w'],
)))
assert len(msg.data) == msg.info.width * msg.info.height, 'BAD'
return msg
def __init__(self):
rospy.init_node('explorer', anonymous=True)
self.is_navigating = False
self.is_searching_unknown_space = False
self.robot_pose_available = False
self.robot_radius = 1
self.blowUpCellNum = 3
self.map_info = MapMetaData()
self.map_complete = False
self.map_complete_print = False
self.map_camera_complete = False
self.all_maps_complete_printed = False
self.waypoints = []
self.waypointsAvailable = False
self.pose = Pose()
self.pose_converted = PoseConverted()
self.folder = rospy.get_param(
"~map_folder",
default=
"/catkin_ws/src/turtlebot3/turtlebot3_navigation/maps/mymap/")
# --- Subscriber ---
rospy.loginfo('setup subscriber')
self.pose_subscriber = rospy.Subscriber('simple_odom_pose', CustomPose,
self._handle_update_pose)
# --- Service wait ---
rospy.loginfo('wait find_unkown_service')
rospy.wait_for_service('find_unkown_service')
rospy.loginfo('wait find_unseen_service')
rospy.wait_for_service('find_unseen_service')
# --- service connect ---
rospy.loginfo('setup services')
self.find_unknown_service = rospy.ServiceProxy('find_unkown_service',
FindUnknown)
self.find_unseen_service = rospy.ServiceProxy('find_unseen_service',
FindUnseen)
# --- Action server connect ---
rospy.loginfo('action client')
self.client = actionlib.SimpleActionClient('path_drive_server',
PathDriveAction)
self.client.wait_for_server()
rospy.loginfo('setup')
self._setup()
rospy.loginfo('ready')
self.rate = rospy.Rate(20)
def __init__(self, **kwargs):
assert all('_' + key in self.__slots__ for key in kwargs.keys()), \
'Invalid arguments passed to constructor: %s' % \
', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__))
from std_msgs.msg import Header
self.header = kwargs.get('header', Header())
from nav_msgs.msg import MapMetaData
self.info = kwargs.get('info', MapMetaData())
self.data = array.array('b', kwargs.get('data', []))
def __init__(self):
rospy.init_node('PRM_node')
# Initial pose as the start location, and goal (stored as strings)
self.start = None
self.goal = None
# Map
self.map = None
self.map_meta_data = MapMetaData()
# Robot radii -NOT diameter. Added in buffer
self.robot_width = .45 + .1
self.robot_height = .3 + .1
# List of robot points for checking for valid points
self.robot_points = []
# Gain for how much to weigh orientation
#self.c = .1
# How many verticies do we want to randomly generate
self.K = 10000
# How far away can we connect points (in cells)
self.max_rad = 25
self.far = 10
# graph is a dictionary
# each key is the pose (string form)
# each entry contains a list of keys that it has edges with
self.g = {}
self.blank_graph = {}
# Path object
self.path = Path()
# For processing the PRM points
self.first = True
# For drawing the PRM in RViz
self.prm_points = PointCloud()
# Publishers
self.path_pub = rospy.Publisher('/path', Path, queue_size=1)
self.prm_pub = rospy.Publisher('/prm', PointCloud, queue_size=1)
self.start_goal_pub = rospy.Publisher('/start_goal',
Odometry,
queue_size=1)
# Subscribers
self.map_sub = rospy.Subscriber('/map', OccupancyGrid, self.setMap)
self.pose_sub = rospy.Subscriber('/robot0/odom', Odometry,
self.setPose)
self.goal_sub = rospy.Subscriber('/goal', Pose2D, self.createPlan)
rospy.spin()
def numpyToOcc(grid, origMsg): msg = OccupancyGrid() msg.header.frame_id = "map" msg.data = grid.ravel() msg.info = MapMetaData() msg.info.height = grid.shape[0] msg.info.width = grid.shape[1] msg.info.resolution = 0.05 msg.info.origin = origMsg.info.origin return msg
def numpyToOcc(self, grid):
msg = OccupancyGrid()
msg.header.frame_id = "base_link"
msg.data = grid.ravel()
msg.info = MapMetaData()
msg.info.height = self.h
msg.info.width = self.w
msg.info.resolution = self.res
msg.info.origin = Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1))
return msg
def numpyToOcc(grid, res): msg = OccupancyGrid() msg.header.frame_id = "map" msg.data = grid.ravel() msg.info = MapMetaData() msg.info.height = grid.shape[0] msg.info.width = grid.shape[1] msg.info.resolution = res msg.info.origin = Pose(Point(0, 0, 0), Quaternion(0,0,0,1)) return msg
def __init__(self, image_path=None):
height = int(rospy.get_param("~grid_height", 800))
width = int(rospy.get_param("~grid_width", 800))
resolution = rospy.get_param("~grid_resolution", .3)
ogrid_topic = rospy.get_param("~grid_topic", "/ogrid")
self.grid_drawer = DrawGrid(height, width, image_path)
self.ogrid_pub = rospy.Publisher(ogrid_topic, OccupancyGrid, queue_size=1)
m = MapMetaData()
m.resolution = resolution
m.width = width
m.height = height
pos = np.array([-width * resolution / 2, -height * resolution / 2, 0])
quat = np.array([0, 0, 0, 1])
m.origin = Pose()
m.origin.position.x, m.origin.position.y = pos[:2]
self.map_meta_data = m
rospy.Timer(rospy.Duration(1), self.pub_grid)
def __init__(self):
rospy.init_node('mapping_node')
# Mapping Constants
self.origin = Pose()
self.origin.position.x = -25.
self.origin.position.y = -25.
self.origin.orientation.w = 1.
self.map_info = MapMetaData()
self.map_info.map_load_time = rospy.Time.now()
self.map_info.resolution = .1
self.map_info.height = 1000
self.map_info.width = 1000
self.map_info.origin = self.origin
# Sensor Model
self.alpha = 0.9
self.beta = 0.7
# Transform(s)
# sensor to robot
self.T_sensor = np.array([[np.cos(0), -1*np.sin(0), 0.27],
[np.sin(0), np.cos(0), 0.0],
[0.0, 0.0, 1.0]])
# robot to world
self.T_robot = np.array([[np.cos(0), -1*np.sin(0), 0.0],
[np.sin(0), np.cos(0), 0.0],
[0.0, 0.0, 1]])
# Occupancy grid variable (array)
# occ_grid is a numpy matrix
# Prefill with 0.5 for unknown occupancy
self.occ_grid = np.repeat(np.matrix(np.repeat(.5, self.map_info.width)), self.map_info.height, axis=0)
# ros_occ_grid is a ROS OccupancyGrid instance
self.ros_occ_grid = OccupancyGrid()
self.ros_occ_grid.info = self.map_info
# Publishers
self.map_pub = rospy.Publisher('/map', OccupancyGrid, queue_size=1)
# Subscribers
# pose_sub and scan_sub are synchronized.
# when the updateMap callback is executed it receives a message from both.
pose_sub = message_filters.Subscriber('/groundtruth', Odometry)
scan_sub = message_filters.Subscriber('/r2d2/laser/scan', LaserScan)
ts = message_filters.TimeSynchronizer([pose_sub, scan_sub], 10)
ts.registerCallback(self.updateMap)
rospy.spin()
def start():
# Initialize publisher and node
pub = rospy.Publisher('map', OccupancyGrid, queue_size=1)
rospy.init_node('occupancy_publish')
# Create empty map
map_generated = np.zeros((2000, 2000, 3), np.uint8)
resolution = 0.01
# Draw lines on the map
points = parse_points(file_name='room_318.txt')
points_normilized = [(np.float32(x[0] / resolution),
np.float32(x[1] / resolution)) for x in points]
font = cv2.FONT_HERSHEY_SIMPLEX
lines = [[0, 1], [1, 23], [23, 22], [22, 21], [21, 20], [20, 19], [19, 18],
[18, 17], [17, 11], [11, 13], [12, 13], [12, 10], [10, 9], [9, 0],
[4, 6], [6, 14], [14, 16], [16, 8], [8, 2], [2, 3], [3, 15],
[15, 5], [5, 4]]
# for i in range(len(points_normilized)-1):
# cv2.putText(map_generated,str(i),points_normilized[i], font, 1, (200,255,155), 2, cv2.LINE_AA)
# cv2.line(map_generated, points_normilized[i], points_normilized[i+1], (255, 255, 255), thickness=1)
for i in range(len(lines)):
# cv2.putText(map_generated,str(i),points_normilized[i], font, 1, (200,255,155), 2, cv2.LINE_AA)
cv2.line(map_generated,
points_normilized[lines[i][0]],
points_normilized[lines[i][1]], (255, 255, 255),
thickness=1)
map_generated = cv2.flip(map_generated, 0)
map_generated = cv2.flip(map_generated, 1)
map_generated_gray = cv2.cvtColor(map_generated, cv2.COLOR_BGR2GRAY)
# cv2.namedWindow('image', cv2.WINDOW_NORMAL)
# cv2.imshow('image', map_generated_gray)
# cv2.waitKey(delay=0)
print(map_generated_gray)
# Convert map in appropriate form
map_generated_flat = list(map_generated_gray.flatten())
map_generated_flat[:] = [x / 255.0 for x in map_generated_flat]
map_generated_flat[:] = [x * 100.0 for x in map_generated_flat]
map_width, map_height = map_generated_gray.shape
# Create an Occupancy Grid message
map_msg = OccupancyGrid(info=MapMetaData(width=map_width,
height=map_height,
resolution=resolution),
data=map_generated_flat)
# ROS working loop
rate = rospy.Rate(10)
while not rospy.is_shutdown():
pub.publish(map_msg)
rate.sleep()
def get_new_occupancy_grid(self): from nav_msgs.msg import OccupancyGrid from nav_msgs.msg import MapMetaData from roslib.msg import Header from geometry_msgs.msg import Pose from geometry_msgs.msg import Point from geometry_msgs.msg import Quaternion import random r = random.Random(1234) return OccupancyGrid(Header(0, rospy.Time(0,0), "/map"), MapMetaData(rospy.Time(123,456), 0.1, 100, 100, Pose(Point(1.23, 4.56, 7.89), Quaternion(0,0,0,1))), [r.randint(-1,100) for x in xrange(0,1000)])
def init_grid_publisher(self):
self.grid_publisher = rospy.Publisher('/indoor/occupancy_grid_topic',
OccupancyGrid,
queue_size=10)
# OccupancyGrid documentation:
# http://docs.ros.org/melodic/api/nav_msgs/html/msg/MapMetaData.html
occ_grid_msg = OccupancyGrid()
rate = rospy.Rate(self.rate)
while not rospy.is_shutdown():
m = MapMetaData() # Grid metadata
m.resolution = self.res # Grid resolution
m.width = self.x_lim[1] - self.x_lim[0] # Grid width in world CS
m.height = self.y_lim[1] - self.y_lim[
0] # Grid height in worlds CS
m.origin = Pose(
) # The grid origin in world CS (there is no need for indoor navigation)
occ_grid_msg.info = m
occ_grid_msg.header.stamp = rospy.Time.now()
occ_grid_msg.header.frame_id = "/indoor/occupancy_grid"
# Convert the matrix from 2D fload64 to 1D int8 list
occ_grid_msg.data = list(
np.asarray(self.matrix.flatten(), dtype=np.int8))
# Publish the message
self.grid_publisher.publish(occ_grid_msg)
rate.sleep()
def get_ros_message(self):
header = Header()
header.seq = 0
header.stamp = rospy.Time.now()
header.frame_id = '/odom'
meta_data = MapMetaData()
meta_data.resolution = self._resolution
meta_data.width = self._values.shape[X]
meta_data.height = self._values.shape[Y]
p = Pose()
p.position.x = self._origin[X]
p.position.y = self._origin[Y]
p.position.z = 0.01
p.orientation.x = 0.
p.orientation.y = 0.
p.orientation.z = 0.
p.orientation.w = 1.
meta_data.origin = p
grid_msg = ros_nav.OccupancyGrid(header=header,
info=meta_data,
data=[])
for row in self._values:
for v in row:
grid_msg.data.append(int(v * (100 / OCCUPIED)))
return grid_msg
def getROSOccupancyMap(self, resolution):
# First figure out how large the occupancy grid needs to be in cells
widthInCells = math.ceil(self.extent[0] / resolution))
heightInCells = int(math.ceil(self.extent[1] / resolution))
# Create the occupancy grid object
rosMAP = OccupancyGrid()
# Assign the map meta data
rosMap.info = MapMetaData()
rosMAP.info.resolution = resolution
rosMAP.info.width = uint(widthInCells)
rosMAP.info.height = uint(heightInCells)
rosMAP.info.pose = new Pose()
ROSMAP(widthInCells, heightInCells, resolution)
# First add a border. This stops STDR falling off the edge of the world.
# Don't have time to work out what the proper Python iterator is for this.
for y in range(heightInCells):
occupancyGrid.grid[0][y] = 1
occupancyGrid.grid[widthInCells-1][y] = 1
for x in range(1, widthInCells-1):
occupancyGrid.grid[x][0] = 1
occupancyGrid.grid[x][heightInCells] = 1
# Rasterise into cells. This can be SLOW
print 'Rasterising workspace into cells'
lastPercentPrint = 10
print 'Finished 0.0%'
for x in range(widthInCells):
percentProgress = math.floor(100.0 * x / widthInCells)
if (percentProgress > lastPercentPrint):
print 'Finished ' + str(percentProgress) + '%'
lastPercentPrint = lastPercentPrint + 10
for y in range(heightInCells):
xl = x * resolution + 1e-3
xr = xl + resolution - 2e-3
yb = y * resolution + 1e-3
yt = yb + resolution - 2e-3
cellPolygon = Polygon(((xl, yb), (xr, yb), (xr, yt), (xl, yt), (xl, yb)))
if self.polygonInFreeSpace(cellPolygon, ignoreBorder = True) == True:
occupancyGrid.grid[x][y] = 0
else:
occupancyGrid.grid[x][y] = 1
return occupancyGrid
def test_add_map_and_set_active(self):
im = Image.new('L', (800, 800))
d = ImageDraw.Draw(im)
d.rectangle(((100, 100), (600, 600)), fill=100)
d.rectangle(((300, 300), (350, 350)), fill=60)
d.rectangle(((200, 200), (400, 400)), fill=0)
b = BytesIO()
im.save(fp=b, format='PNG')
png_map_bytes = b.getvalue()
map_info = MapMetaData(map_load_time=rospy.Time(0),
resolution=0.02,
width=800,
height=800,
origin=Pose(position=Point(x=-10, y=-10, z=0),
orientation=Quaternion(x=0,
y=0,
z=0,
w=1)))
map_name = 'test_map'
add_map_srv = rospy.ServiceProxy('/map_manager/add_map', AddMap)
add_map_srv.wait_for_service(timeout=10)
request = AddMapRequest(
map=Map(info=MapInfo(name=map_name, meta_data=map_info),
markers=[
Marker(name='marker_{}'.format(i),
pose=Pose(position=Point(x=i, y=i),
orientation=Quaternion(w=1)))
for i in range(10)
]),
occupancy_grid=CompressedImage(format='png', data=png_map_bytes),
map_data=[1, 2, 3, 4])
rospy.loginfo('Adding a Map')
response = add_map_srv.call(request)
self.assertIsInstance(response, AddMapResponse)
self.assertTrue(response.success, msg=response.message)
set_map_srv = rospy.ServiceProxy('/map_manager/set_active_map',
SetActiveMap)
set_map_srv.wait_for_service(timeout=10)
rospy.loginfo('Setting active map')
response = set_map_srv.call(SetActiveMapRequest(map_name=map_name))
self.assertIsInstance(response, SetActiveMapResponse)
self.assertTrue(response.success, msg=response.message)
md = rospy.wait_for_message('/map_manager/map_data',
UInt8MultiArray,
timeout=10) # type: UInt8MultiArray
md_d = struct.unpack('<%sb' % len(md.data), md.data)
self.assertTrue(
all(int(a) == int(b) for a, b in zip(md_d, request.map_data)))
def slam_map(data):
#slam_msg = OccupancyGrid()
nav_pixel_msg = MapMetaData()
resolution = data.info.resolution
#print("Resolution:",resolution)
width = nav_pixel_msg.width
height = nav_pixel_msg.height
# for i in range(len(slam_msg.data)):
x = width * resolution + resolution / 2
y = height * resolution + resolution / 2
def __init__(self):
rospy.init_node('turtlebot', anonymous=True)
# initialize a publisher for occupancy grid
self.occupancy_pub = rospy.Publisher('/map',
OccupancyGrid,
queue_size=10)
# rospy.loginfo("got into initializaytion")
self.occupancy_grid = OccupancyGrid()
metadata = MapMetaData()
# metadata.map_load_time = None
metadata.resolution = resolution = 0.1
metadata.width = 250
metadata.height = 250
pos = np.array([
-metadata.width * resolution / 2,
-metadata.height * resolution / 2, 0
])
metadata.origin = Pose()
metadata.origin.position.x, metadata.origin.position.y = pos[:2]
self.conversion_m_to_xy = metadata.width / 30 + 10
#self.conversion_m_to_y = self.
self.map_metadata = metadata
self.occupancy_grid.info = self.map_metadata
self.occupancy_grid.data = np.ones(
metadata.height * metadata.width).astype(int) * O_0
self.laser_data = None
self.position = None
self.orientation = None
self.covariance = None
# initialize a subscriber to receive the estimated pose and the map
rospy.Subscriber("/scan", LaserScan, self.laser_callback)
# initialize a subscriber to receiver estimated posiition from the kalman filter output
rospy.Subscriber('/indoor_pos', PoseWithCovarianceStamped,
self.IPS_callback)
def expandMap():
global expandedMap
global expandedMap2
global expandedWidth
expandedMap = copy.deepcopy(grid)
print "Expanded is %f" % len(grid)
print "expanded width %f height %f" % (width, height)
for i in range(0, height):
for j in range(0, width):
if (grid[j + (width * i)].intensity >= 100):
for k in range(j - int(round(expandBuffer / res)),
j + int(round(expandBuffer / res)) + 1):
for l in range(i - int(round(expandBuffer / res)),
i + int(round(expandBuffer / res)) + 1):
if (k > 0 and k < width and l > 0 and l < height):
#print "Trying index %f k %f l %f" % (k + (width * l),k,l)
expandedMap[k + (width * l)].intensity = 100
expandedMap2 = []
added = False
block = False
for i in range(0, int(height / (expandedGridRes / res))):
for j in range(0, int(width / (expandedGridRes / res))):
block = False
for y in range(0, int(expandedGridRes / res)):
for x in range(0, int(expandedGridRes / res)):
if (expandedMap[int(i * width * (expandedGridRes / res) +
y * width + j *
(expandedGridRes / res) + x)].intensity
> expandThreshold):
block = True
if (block):
expandedMap2.append(
Node(j * expandedGridRes + gridOrigin.position.x,
i * expandedGridRes + gridOrigin.position.y, 100))
else:
expandedMap2.append(
Node(j * expandedGridRes + gridOrigin.position.x,
i * expandedGridRes + gridOrigin.position.y, 0))
newWidth = int(width / (expandedGridRes / res))
newHeight = int(height / (expandedGridRes / res))
expandedWidth = newWidth
ocGrid = OccupancyGrid()
pub_map = []
meta = MapMetaData()
meta.map_load_time = rospy.Time.now()
meta.width = newWidth
meta.height = newHeight
meta.resolution = expandedGridRes
for next in expandedMap2:
pub_map.append(next.intensity)
ocGrid.header.frame_id = '/map'
ocGrid.header.stamp = rospy.Time.now()
ocGrid.info = meta
ocGrid.info.origin = gridOrigin
ocGrid.data = pub_map
expanded_pub.publish(ocGrid)
def locateFrontierPt(
self, laserscanIn, mapIn
): # escapePath is a bool that deternimes if this is a escape routine
myScan = LaserScan()
myLocation = Pose()
mymap = OccupancyGrid()
myLocation = MapMetaData()
mymap = mapIn
myScan = laserscanIn
myLocation = mymap.info.origin
width = mymap.info.width
densityOccupation = []
densityClusters = []
# Generate a radius of density around the robot
for point in range(0, 667 - 1, 1):
rospy.loginfo(len(myScan.ranges))
x_other = myLocation.position.x + myScan.ranges[point] * math.cos(
self.angles + myLocation.orientation.z)
y_other = myLocation.position.y + myScan.ranges[point] * math.sin(
self.angles + myLocation.orientation.z)
#rospy.loginfo(myScan.ranges[point])
point = x_other + width * y_other
#rospy.loginfo(point)
if (not math.isinf(point) and not math.isnan(point)):
densityOccupation.append(int(point))
self.angles += 0.35982
# Locate a midpoint of density
#rospy.loginfo(densityOccupation)
for mapPoint in densityOccupation:
if (mymap.data[mapPoint] > 0):
densityClusters.append(0)
else:
densityClusters.append(1)
#rospy.loginfo(densityClusters)
points = []
for point in densityClusters: # Convert all possible "walkable" points into real points instead of map points.
if (point == 1):
myPoint = self.convertMapArrayIntoXY(point, width)
points.append(myPoint)
#rospy.loginfo(len(points))
if (
len(points) < 100
): # We want to verify that we can walk foward if we can possibly can do so.
return "Blocked", False
else:
return points, self.firstStart # Returns an array of points