def setUpClass(cls):
# build a square room
map_data = np.zeros((400, 400), dtype=np.int8)
map_data[...] = -1
map_data[150:251, 150:251] = 100
map_data[151:250, 151:250] = 0
msg = ros_numpy.msgify(OccupancyGrid, map_data)
msg.info.resolution = 0.1
msg.info.origin = Pose(position=Point(x=-20, y=-20))
cls.map = mcl.Map(msg)
def _make_local_map(self, point):
#target frame is map_frame, source frame is laser_link
self.map_info.origin.position = point.point
self.ros_map.info = self.map_info
self.ros_map.data[...] = -1
mcl_local_map = mcl.Map(self.ros_map)
mcl_local_map.grid[:, :] = 100
### TODO: figure out faster serialization issue
map_msg = msgify(numpy_msg(OccupancyGrid),
mcl_local_map.grid,
info=self.map_info)
map_msg.header = point.header
map_msg.header.frame_id = 'map'
return map_msg
def _make_local_map(self, scan):
#target frame is map_frame, source frame is laser_link
trans = self.tf_buffer.lookup_transform(
target_frame=self.map_frame,
source_frame=scan.header.frame_id,
time=scan.header.stamp,
timeout=rospy.Duration(5))
pos = trans.transform.translation
orient = trans.transform.rotation
# transform from base to map
transform = np.dot(
transformations.translation_matrix([pos.x, pos.y, pos.z]),
transformations.quaternion_matrix(
[orient.x, orient.y, orient.z, orient.w]))
self.map_info.origin.position.x = pos.x - self.map_info.resolution * self.width / 2.0
self.map_info.origin.position.y = pos.y - self.map_info.resolution * self.height / 2.0
self.ros_map.info = self.map_info
self.ros_map.data[...] = -1
mcl_local_map = mcl.Map(self.ros_map)
mask = (scan.ranges < scan.range_max) & (scan.ranges > scan.range_min)
angles = np.arange(scan.angle_min, scan.angle_max,
scan.angle_increment)
x = scan.ranges[mask] * np.cos(angles[mask])
y = scan.ranges[mask] * np.sin(angles[mask])
# set the last component to zero to ignore translation
ii, jj = mcl_local_map.index_at(np.vstack(
(x, y, np.zeros(np.sum(mask)), np.ones(np.sum(mask)))).T,
world_to_map=transform)
ok = ((jj >= 0) & (jj < self.width) & (ii >= 0) & (ii < self.height))
ii = ii[ok]
jj = jj[ok]
mcl_local_map.grid[ii, jj] = 100
### TODO: figure out faster serialization issue
map_msg = msgify(numpy_msg(OccupancyGrid),
mcl_local_map.grid,
info=self.map_info)
map_msg.header = scan.header
map_msg.header.frame_id = 'map'
return map_msg
def sub_map(self, map):
"""
We need the global map in order to set up the coordinate frame
correctly for the local map
"""
rospy.loginfo("received map")
self.mcl_map = mcl.Map(map)
self.map_frame = map.header.frame_id
self.map_info_glob = map.info
self.width = 200
self.height = 200
ros_map = numpy_msg(OccupancyGrid)()
ros_map.header.frame_id = 'map'
ros_map.info = deepcopy(map.info)
ros_map.info.width = self.width
ros_map.info.height = self.height
ros_map.info.origin.position.x = -1 * ros_map.info.resolution * self.height / 2.0
ros_map.info.origin.position.y = -1 * ros_map.info.resolution * self.width / 2.0
self.map_info = ros_map.info
ros_map.data = np.ones(self.width * self.height, dtype=np.int8) * -1
self.ros_map = ros_map
def sub_map(self, map):
rospy.loginfo('Map recieved')
self.map = mcl.Map(map)
def sub_localmap(self, map):
if self.map is not None:
self.map.merge_transient(mcl.Map(map))
def sub_map(self, map):
self.map = mcl.Map(map)