def manually_add_obstacle(self, points_set, obstacle_id, movability,
robot_polygon):
obstacle = Obstacle.from_points_make_polygon(points_set, self.info,
self.frame_id,
self.robot_metadata,
obstacle_id, movability)
# Raise exception if obstacle in intersection with robot
if robot_polygon is not None and obstacle.polygon.intersects(
robot_polygon):
raise IntersectionWithRobotException
# Raise exception if any of the obstacle's points are out of map bounds
for map_point in obstacle.map_points_set:
if not Utils._is_in_matrix(map_point[0], map_point[1],
len(self.static_occ_grid),
len(self.static_occ_grid[0])):
raise ObstacleOutOfBoundsException
# Raise exception if obstacle intersects another obstacle or the static obstacles
if self.is_obstacle_intersecting_others(obstacle):
raise IntersectionWithObstaclesException
self.obstacles.update({obstacle.obstacle_id: obstacle})
self.compute_merged_occ_grid()
all_push_poses.header = self.static_map.header
all_push_poses.header.stamp = rospy.Time.now()
for obstacle_id, obstacle in self.multilayered_map.obstacles.items():
for push_pose in obstacle.push_poses:
all_push_poses.poses = all_push_poses.poses + [push_pose.pose]
Utils.publish_once(self.push_poses_pub, all_push_poses)
if __name__ == '__main__':
rospy.init_node('map_manager')
map_manager = MapManager(0.05, 1.0, "/map", "/simulated/global_occupancy_grid", "/simulated/all_push_poses")
# This obstacle with these specific coordinates can only be inserted in the 01 map
obstacle1 = Obstacle.from_points_make_polygon(Utils.map_coords_to_real_coords(
{(7, 1), (8, 1), (7, 6), (8, 6)},
map_manager.multilayered_map.info.resolution),
map_manager.multilayered_map.info,
map_manager.map_frame,
map_manager.robot_metadata,
1,
True)
map_manager.manually_add_obstacle(obstacle1)
# print(obstacle1.robot_inflated_obstacle["matrix"])
# print(obstacle1.obstacle_matrix["matrix"])
#
# # TEST
# print(map_manager.multilayered_map.info)
# print(map_manager.multilayered_map.robot_metadata)
# print(map_manager.multilayered_map.frame_id)
print(map_manager.multilayered_map.static_occ_grid)
print(map_manager.multilayered_map.static_obstacles_positions)
