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)