From cf8a6cd4e743553a3f8738f2e0386cbdaf536bad Mon Sep 17 00:00:00 2001 From: Tianji Date: Tue, 20 Aug 2024 21:48:03 +0000 Subject: [PATCH 1/7] demo version --- .../multi_robot_semantic_mapping_params.yaml | 30 +++-- ROAM-Planning/CMakeLists.txt | 2 +- ROAM-Planning/launch/planning_multi.launch | 4 +- .../multi_robot_exploration_params.yaml | 2 +- .../exploration/ga_multi_exploration_node.py | 112 +++++++++--------- 5 files changed, 77 insertions(+), 73 deletions(-) diff --git a/ROAM-Mapping/params/multi_robot_semantic_mapping_params.yaml b/ROAM-Mapping/params/multi_robot_semantic_mapping_params.yaml index 72e3add..373297b 100755 --- a/ROAM-Mapping/params/multi_robot_semantic_mapping_params.yaml +++ b/ROAM-Mapping/params/multi_robot_semantic_mapping_params.yaml @@ -2,9 +2,12 @@ octomap: octomap_topic: "/octomap_full" pointcloud_topic: "/$(arg name)/semantic_pcl" frame_id: "world" - resolution: 0.3 - max_range: 10.0 - raycast_range: 0.5 + # resolution: 0.3 + # max_range: 10.0 + # raycast_range: 0.5 + resolution: 0.5 + max_range: 15.0 + raycast_range: 10.0 clamping_thres_min: 0.006692851 clamping_thres_max: 0.993307149 occupancy_thres: 0.5 @@ -21,17 +24,18 @@ octomap: # Camera intrinsic matrix parameters camera: - fx: 65.24951330239097 - fy: 65.24951330239097 - cx: 80.0 - cy: 60.0 + fx: 320 + fy: 320 + cx: 320 + cy: 240 - width: 160 - height: 120 + width: 640 + height: 480 semantic_pcl: - color_image_topic: "/$(arg name)/camera/image" - semantic_image_topic: "/$(arg name)/camera/semantic_image" - depth_image_topic: "/$(arg name)/depth_camera/depth/image_rect_raw" + color_image_topic: "/$(arg name)/image" + semantic_image_topic: "/$(arg name)/colored_map" + depth_image_topic: "/$(arg name)/depth_image" point_type: 0 - frame_id: "$(arg name)/camera" + frame_id: "$(arg name)/camera_optic" + unit_conversion: 1.0 \ No newline at end of file diff --git a/ROAM-Planning/CMakeLists.txt b/ROAM-Planning/CMakeLists.txt index c49a8da..bada4f3 100644 --- a/ROAM-Planning/CMakeLists.txt +++ b/ROAM-Planning/CMakeLists.txt @@ -40,7 +40,7 @@ set(SOURCE_FILES src/cpp/src/exploration/util.cpp ) -set(EXPLORATION_LIB_DESTINATION "${CATKIN_DEVEL_PREFIX}/lib/python2.7/dist-packages") +set(EXPLORATION_LIB_DESTINATION "${CATKIN_DEVEL_PREFIX}/lib/python3/dist-packages") add_library(exploration_cpp SHARED ${PROJECT_SOURCE_DIR} ${SOURCE_FILES} src/cpp/src/exploration/python.cpp) target_link_libraries(exploration_cpp ${PYTHON_LIBRARIES}) set_target_properties(exploration_cpp PROPERTIES SUFFIX ".so" PREFIX "_" diff --git a/ROAM-Planning/launch/planning_multi.launch b/ROAM-Planning/launch/planning_multi.launch index 4e4d58c..338328b 100755 --- a/ROAM-Planning/launch/planning_multi.launch +++ b/ROAM-Planning/launch/planning_multi.launch @@ -12,11 +12,11 @@ - - --> + diff --git a/ROAM-Planning/params/multi_robot_exploration_params.yaml b/ROAM-Planning/params/multi_robot_exploration_params.yaml index 5c4b5be..35aaa10 100644 --- a/ROAM-Planning/params/multi_robot_exploration_params.yaml +++ b/ROAM-Planning/params/multi_robot_exploration_params.yaml @@ -20,7 +20,7 @@ planning: collision_check_period: 1 multi: - num_agents: 6 + num_agents: 3 horizon: 5 delta_q: 10.0 gamma_q: 0.01 diff --git a/ROAM-Planning/src/exploration/ga_multi_exploration_node.py b/ROAM-Planning/src/exploration/ga_multi_exploration_node.py index 2da202c..5a87b49 100755 --- a/ROAM-Planning/src/exploration/ga_multi_exploration_node.py +++ b/ROAM-Planning/src/exploration/ga_multi_exploration_node.py @@ -158,10 +158,10 @@ def __init__(self): print('Multi agent exploration agent \'{}\' initialized!'.format(self.robot_name)) def team_path_callback(self, team_path_msg): - + if team_path_msg.data[0] == self.agent_id: return - + if self.received_team_path_num == 0: self.consensus_delta = np.copy(team_path_msg.data[1:]) else: @@ -170,12 +170,12 @@ def team_path_callback(self, team_path_msg): self.received_team_path_num += 1 def need_plan_callback(self, need_plan_msg): - + self.need_plan[1:] = need_plan_msg.data[1:] if self.check_ready(): self.need_plan[self.agent_id] = 1 - + if np.mean(self.need_plan[1:]) >= self.should_plan_thresh: self.optimize_pose() else: @@ -187,7 +187,7 @@ def need_plan_callback(self, need_plan_msg): def optimize_pose(self): if self.is_planning: return - + print('Planning started for agent \'{}\'!'.format(self.robot_name)) self.is_planning = True self.consensus_delta = None @@ -200,7 +200,7 @@ def optimize_pose(self): if np.mean(self.need_plan[1:]) < self.need_plan_thresh: print('{}: Peer-pressured to stop planning!'.format(self.robot_name)) break - + if self.received_team_path_num > 0: unweighted_delta = self.consensus_delta - self.received_team_path_num * self.team_path[1:] self.team_path[1:] += self.weight_mask * unweighted_delta / (self.received_team_path_num + 1) @@ -217,14 +217,14 @@ def optimize_pose(self): cell_scores = self.compute_score(reachable_rc) team_grad[3 * self.horizon * (self.agent_id - 1) + 3 * i:\ 3 * self.horizon * (self.agent_id - 1) + 3 * (i + 1)] += self.compute_gradient(pose, cell_scores, reachable_rc) - + for agent in range(self.num_agents): t_init = 0 robot_collision_penalty = self.robot_colision_coeff if agent + 1 == self.agent_id: t_init = i + 1 robot_collision_penalty = 1 - + for t in range(t_init, self.horizon): other_pose = self.team_path[1 + 3 * self.horizon * agent + 3 * t:1 + 3 * self.horizon * agent + 3 * (t + 1)] dist = np.linalg.norm(pose[:2] - other_pose[:2]) @@ -237,13 +237,13 @@ def optimize_pose(self): 3 * self.horizon * (self.agent_id - 1) + 3 * i + 2] -= grad team_grad[3 * self.horizon * agent + 3 * t:\ 3 * self.horizon * agent + 3 * t + 2] += grad - + self.team_path[1:] += team_grad * self.adjusted_step_size / (iter_num + 1) self.team_path[3::3] = wrap_angles(self.team_path[3::3]) - + self.team_path_pub.publish(self.team_path) print('{}: {}-th iteration of pose optimization Finished! Gradient norm: {}'.format(self.robot_name, iter_num + 1, np.linalg.norm(team_grad * self.adjusted_step_size / (iter_num + 1)))) - + self.publish_path() self.is_planning = False @@ -323,13 +323,13 @@ def compute_frontier_path(self, robot_pose): if path_length > self.goal_check_radius: pixel_path = xy_to_rc(path, self.planning_map) try: - collision_inds = np.nonzero(self.planning_map.data[pixel_path[:, 0].astype(int), - pixel_path[:, 1].astype(int)] == Costmap.OCCUPIED)[0] - if collision_inds.shape[0] == 0: - path_list.append(path) - path_score_list.append(frontier_size[i] / (1 + path_length)) + collision_inds = np.nonzero(self.planning_map.data[pixel_path[:, 0].astype(int), + pixel_path[:, 1].astype(int)] == Costmap.OCCUPIED)[0] + if collision_inds.shape[0] == 0: + path_list.append(path) + path_score_list.append(frontier_size[i] / (1 + path_length)) except IndexError: - continue + continue best_path = None if len(path_list) == 0: @@ -406,12 +406,12 @@ def get_pose_from_tf(self, from_frame_id): no_transfrom = True while no_transfrom: try: - (translation, rotation) = self.tf_listener.lookupTransform(self.world_frame_id, - from_frame_id, - rospy.Time(0)) - no_transfrom = False + (translation, rotation) = self.tf_listener.lookupTransform(self.world_frame_id, + from_frame_id, + rospy.Time(0)) + no_transfrom = False except tf.LookupException or tf.ConnectivityException: - continue + continue euler = tf.transformations.euler_from_quaternion(rotation) return np.array([translation[0], translation[1], euler[2]]) @@ -468,12 +468,12 @@ def map_callback(self, occ_map_msg): if collision_inds.shape[0] > 0: print('{}: Collision ahead!'.format(self.robot_name)) self.collision_pub.publish(Empty()) - + self.is_planning = True - + rem_waypoint_inds = self.waypoint_inds[self.waypoint_inds > collision_inds[0]] rem_waypoints = self.curr_path[rem_waypoint_inds, :] - + robot_pose = self.get_pose_from_tf(self.robot_frame_id) self.footprint.draw_circumscribed(robot_pose, self.planning_map, self.draw_radius) full_path = robot_pose[None, :] @@ -483,31 +483,31 @@ def map_callback(self, occ_map_msg): if plan_success: full_path = np.vstack((full_path, partial_path)) self.waypoint_inds = np.concatenate((self.waypoint_inds, [full_path.shape[0] - 1])) - + if len(full_path.shape) > 1: - path_length = 0 - curr_pose = full_path[0] - for i, next_pose in enumerate(full_path[1:]): - path_length += np.linalg.norm(next_pose[:2] - curr_pose[:2]) - if path_length > self.max_path_length: - full_path = full_path[:i+1] - self.waypoint_inds = self.waypoint_inds[self.waypoint_inds < full_path.shape[0]] - break - else: - curr_pose = next_pose + path_length = 0 + curr_pose = full_path[0] + for i, next_pose in enumerate(full_path[1:]): + path_length += np.linalg.norm(next_pose[:2] - curr_pose[:2]) + if path_length > self.max_path_length: + full_path = full_path[:i+1] + self.waypoint_inds = self.waypoint_inds[self.waypoint_inds < full_path.shape[0]] + break + else: + curr_pose = next_pose self.curr_path = full_path self.goal = full_path[-1] - + path_msg = Path() path_msg.header.frame_id = self.world_frame_id - + if len(full_path.shape) == 1: self.add_waypoint(full_path, path_msg) else: for pose in full_path: self.add_waypoint(pose, path_msg) - + self.path_pub.publish(path_msg) self.is_planning = False @@ -657,12 +657,12 @@ def get_failsafe_path(self, state): if path_length > self.goal_check_radius: pixel_path = xy_to_rc(random_path, self.planning_map) try: - collision_inds = np.nonzero(self.planning_map.data[pixel_path[:, 0].astype(int), - pixel_path[:, 1].astype(int)] == Costmap.OCCUPIED)[0] - if collision_inds.shape[0] == 0: - path = random_path + collision_inds = np.nonzero(self.planning_map.data[pixel_path[:, 0].astype(int), + pixel_path[:, 1].astype(int)] == Costmap.OCCUPIED)[0] + if collision_inds.shape[0] == 0: + path = random_path except IndexError: - continue + continue rospy.sleep(0.2) return path @@ -719,22 +719,22 @@ def publish_path(self): collision_inds = np.nonzero(self.planning_map.data[pixel_path[:, 0].astype(int), pixel_path[:, 1].astype(int)] == Costmap.OCCUPIED)[0] if collision_inds.shape[0] == 0: - full_path = np.vstack((full_path, partial_path)) - self.waypoint_inds = np.concatenate((self.waypoint_inds, [full_path.shape[0] - 1])) + full_path = np.vstack((full_path, partial_path)) + self.waypoint_inds = np.concatenate((self.waypoint_inds, [full_path.shape[0] - 1])) except IndexError: continue if len(full_path.shape) > 1: - path_length = 0 - curr_pose = full_path[0] - for i, next_pose in enumerate(full_path[1:]): - path_length += np.linalg.norm(next_pose[:2] - curr_pose[:2]) - if path_length > self.max_path_length: - full_path = full_path[:i+1] - self.waypoint_inds = self.waypoint_inds[self.waypoint_inds < full_path.shape[0]] - break - else: - curr_pose = next_pose + path_length = 0 + curr_pose = full_path[0] + for i, next_pose in enumerate(full_path[1:]): + path_length += np.linalg.norm(next_pose[:2] - curr_pose[:2]) + if path_length > self.max_path_length: + full_path = full_path[:i+1] + self.waypoint_inds = self.waypoint_inds[self.waypoint_inds < full_path.shape[0]] + break + else: + curr_pose = next_pose self.curr_path = full_path self.goal = full_path[-1] From 22b451d92a2b10bfc21e23ab67356a97905c3e77 Mon Sep 17 00:00:00 2001 From: Tianji Date: Thu, 19 Sep 2024 07:00:43 +0000 Subject: [PATCH 2/7] demo --- .gitignore | 1 + .../multi_robot_semantic_mapping_params.yaml | 12 +- .../src/octomap_generator_semantic_ros.cpp | 3 +- .../multi_robot_exploration_params.yaml | 46 +++--- .../exploration/ga_multi_exploration_node.py | 146 +++++++++++++++--- .../src/footprints/footprint_points.py | 46 ++++++ 6 files changed, 204 insertions(+), 50 deletions(-) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..ba0430d --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +__pycache__/ \ No newline at end of file diff --git a/ROAM-Mapping/params/multi_robot_semantic_mapping_params.yaml b/ROAM-Mapping/params/multi_robot_semantic_mapping_params.yaml index 373297b..ccb19bb 100755 --- a/ROAM-Mapping/params/multi_robot_semantic_mapping_params.yaml +++ b/ROAM-Mapping/params/multi_robot_semantic_mapping_params.yaml @@ -5,7 +5,7 @@ octomap: # resolution: 0.3 # max_range: 10.0 # raycast_range: 0.5 - resolution: 0.5 + resolution: 0.2 max_range: 15.0 raycast_range: 10.0 clamping_thres_min: 0.006692851 @@ -16,11 +16,11 @@ octomap: psi: 1 phi: -0.3 publish_2d_map: true - min_ground_z: -1.5 - max_ground_z: 1.5 - consensus_weight: 0.8 - sub_timer: 10 - pub_timer: 10 + min_ground_z: 0.0 + max_ground_z: 0.7 + consensus_weight: 0.2 + sub_timer: 5 + pub_timer: 5 # Camera intrinsic matrix parameters camera: diff --git a/ROAM-Mapping/src/octomap_generator_semantic_ros.cpp b/ROAM-Mapping/src/octomap_generator_semantic_ros.cpp index 79dba9d..528481f 100644 --- a/ROAM-Mapping/src/octomap_generator_semantic_ros.cpp +++ b/ROAM-Mapping/src/octomap_generator_semantic_ros.cpp @@ -166,7 +166,8 @@ void SemanticOctomapGeneratorNode::publish2DOccupancyMap(const SemanticOctree* o { double node_z = it.getZ(); - double node_half_side = pow(it.getSize(), 1/3) / 2; + // double node_half_side = pow(it.getSize(), 1/3) / 2; + double node_half_side = it.getSize()/2; double top_side = node_z + node_half_side; double bottom_side = node_z - node_half_side; diff --git a/ROAM-Planning/params/multi_robot_exploration_params.yaml b/ROAM-Planning/params/multi_robot_exploration_params.yaml index 35aaa10..2ca9353 100644 --- a/ROAM-Planning/params/multi_robot_exploration_params.yaml +++ b/ROAM-Planning/params/multi_robot_exploration_params.yaml @@ -1,40 +1,38 @@ footprint: - type: 'tricky_circle' + type: 'jackal' angular_resolution: 0.01745 inflation_scale: 1.3 planning: - SE2_radius: 16.0 - max_iter: 5 - max_path_length: 100 - step_size: 0.001 - angular_range: 1.570796327 + SE2_radius: 2 # epsilon_max, radius of the SE2 space pose sample + max_iter: 10 # k_p, maximux iters of traj optimization + max_path_length: 100 # max length of the path in meters + step_size: 0.001 # scale factor of time varing step size of local update + angular_range: 1.570796327 # sample parameters for pose approx angular_samples: 32 num_orientation_samples: 8 - distance_coeff: 0.001 + distance_coeff: 0.01 # gamma_c tradeoff between informativeness and collision avoidance theta_coeff: 0.1 min_frontier_size: 30 - goal_check_radius: 2.0 - draw_radius: 15 + goal_check_radius: 0.3 # radius of goal check for viewpoint planner + draw_radius: 2 # radius to clear obstacles around the robot in pixels!!! map_topic: '/$(arg name)/occupancy_map_2D' collision_check_period: 1 multi: - num_agents: 3 - horizon: 5 - delta_q: 10.0 - gamma_q: 0.01 - robot_colision_coeff: 1000000 - epsilon: 1 - optimization_period: 0.1 - should_plan_threshold: 0.3 - need_plan_threshold: 0.2 - need_plan_period: 1.0 + num_agents: 2 + horizon: 5 # sampling waypoints on path + delta_q: 10.0 # d_q = F + SE2_radius + gamma_q: 0.01 # trade off between between self informativeness and overlap FOV penalty + robot_colision_coeff: 2 # penalty for robot planned traj repeat, significantly impact gradient + epsilon: 0.1 # consensus step size + should_plan_threshold: 0.3 # thresh_p i.e. threshold to start alg3 + need_plan_threshold: 0.2 # if less then this portion of the agents need to plan, all robots stop optimizing i.e. threshold to quit alg3 + # quit alg3 when some robots finish planning and start to approach the goal + need_plan_period: 1.0 # t_p navigation: - tau: 0.005 - alpha: 0.999 - radius: 2.0 - dist: 1.0 - safety_dist: 1.0 + tau: 0.005 # sleep time for navigator to publish the next waypoint + dist: 0.3 # dist to determine if the robot has reached the waypoint + safety_dist: 1.0 # peer safe distance publish_odom: true diff --git a/ROAM-Planning/src/exploration/ga_multi_exploration_node.py b/ROAM-Planning/src/exploration/ga_multi_exploration_node.py index 5a87b49..6c83554 100755 --- a/ROAM-Planning/src/exploration/ga_multi_exploration_node.py +++ b/ROAM-Planning/src/exploration/ga_multi_exploration_node.py @@ -13,26 +13,55 @@ from nav_msgs.msg import OccupancyGrid from nav_msgs.msg import Path -from std_msgs.msg import Empty +from std_msgs.msg import Empty, Header from geometry_msgs.msg import PoseStamped from rospy.numpy_msg import numpy_msg from rospy_tutorials.msg import Floats - +import os from utilities.frontier_utils import extract_frontiers, cleanup_map_for_planning -from footprints.footprint_points import get_tricky_circular_footprint, get_tricky_oval_footprint +from footprints.footprint_points import get_tricky_circular_footprint, get_tricky_oval_footprint, get_jackal_footprint from footprints.footprints import CustomFootprint from planners.astar_cpp import oriented_astar, get_astar_angles from utilities.util import rc_to_xy, wrap_angles, xy_to_rc from mapping.costmap import Costmap from utilities.sensor_utils import bresenham2d, bresenham2d_with_intensities +def map_visulize(image_path, res = 1, origin = np.array([0, 0]), goal = None, radius = None): + if isinstance(image_path, str): + img = cv2.imread(image_path, cv2.IMREAD_GRAYSCALE) + if img is None: + raise FileNotFoundError(f'Image file not found: {image_path}') + elif isinstance(image_path, np.ndarray): + img = image_path + else: + raise ValueError('Invalid image path') + img_height, img_width = img.shape + img = np.flipud(img) + map_width = img_width * res + map_height = img_height * res + fig, ax = plt.subplots() + extent = [origin[0], origin[0] + map_width, origin[1], origin[1] + map_height] + ax.imshow(img, cmap='gray', extent=extent, origin='lower') + ax.set_xlabel('X (meters)') + ax.set_ylabel('Y (meters)') + ax.set_title('Map') + if goal is not None and radius is not None: + ax.add_patch(plt.Circle(goal, radius, color='r', alpha=0.3)) + # plt.show() + return ax class GradAscentMultiExplorationAgent: def __init__(self): self.robot_name = '/' + rospy.get_param("~agent_name", 'husky') footprint_type = rospy.get_param(self.robot_name + '/footprint/type') - + if not os.path.exists('/root/ws/map'): + os.makedirs('/root/ws/map') + files = os.listdir('/root/ws/map') + for f in files: + os.remove('/root/ws/map/' + f) + self.filter_obstacles_flag = False + self.map_counter = 0 if footprint_type == 'tricky_circle': footprint_points = get_tricky_circular_footprint() elif footprint_type == 'tricky_oval': @@ -44,6 +73,8 @@ def __init__(self): footprint_radius * np.array([np.cos(rotation_angles), np.sin(rotation_angles)]).T elif footprint_type == 'pixel': footprint_points = np.array([[0., 0.]]) + elif footprint_type == 'jackal': + footprint_points = get_jackal_footprint() else: footprint_points = None assert False and "footprint type specified not supported." @@ -151,9 +182,12 @@ def __init__(self): self.vectorized_rc = None self.vectorized_xy = None self.eval_cells = None - self.kernel_cleanup = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) - self.kernel_erosion = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) - + self.kernel_cleanup = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3)) + self.kernel_erosion = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3)) + # self.kernel_cleanup = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) + # self.kernel_erosion = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) + self.plan_map_pub1 = rospy.Publisher(self.robot_name + "/PLANMAP1", OccupancyGrid, queue_size = 1, latch=True) + self.plan_map_pub = rospy.Publisher(self.robot_name + "/PLANMAP", OccupancyGrid, queue_size = 1, latch=True) rospy.sleep(1) print('Multi agent exploration agent \'{}\' initialized!'.format(self.robot_name)) @@ -188,7 +222,7 @@ def optimize_pose(self): if self.is_planning: return - print('Planning started for agent \'{}\'!'.format(self.robot_name)) + # print('IN OPTIMIZE_POSE Planning started for agent \'{}\'!'.format(self.robot_name)) self.is_planning = True self.consensus_delta = None self.received_team_path_num = 0 @@ -248,6 +282,7 @@ def optimize_pose(self): self.is_planning = False def make_init_plan(self): + print('{}: LOOKING FOR FRONTIERS!'.format(self.robot_name)) if self.raytrace_precomputed is None: print('{}: Initializing ray-tracing!'.format(self.robot_name)) max_range = rospy.get_param(self.robot_name + '/octomap/max_range') @@ -286,13 +321,15 @@ def make_init_plan(self): rospy.sleep(0.1) self.planning_map.data = np.array(self.planning_map.data) - self.footprint.draw_circumscribed(robot_pose, self.planning_map, self.draw_radius) + # self.footprint.draw_circumscribed(robot_pose, self.planning_map, self.draw_radius) path = self.compute_frontier_path(robot_pose) if path.shape[0] >= self.horizon: + # print('length of path before sampling: ', path.shape[0]) path_inds = np.linspace(0, path.shape[0], self.horizon, endpoint=False).astype(int) path_sampled = path[path_inds] else: + # print('length of path: ', path.shape[0]) path_sampled = np.zeros((self.horizon, 3)) path_sampled[:path.shape[0]] = path path_sampled[path.shape[0]:] = path[-1] @@ -307,11 +344,14 @@ def compute_frontier_path(self, robot_pose): robot_pose_forward = robot_pose.copy() robot_pose_forward[0] = robot_pose[0] + 1 * np.cos(robot_pose[2]) robot_pose_forward[1] = robot_pose[1] + 1 * np.sin(robot_pose[2]) - + + # print("%d frontiers found!" % len(frontiers)) if frontiers is not None: for i, f in enumerate(frontiers): if np.linalg.norm(robot_pose_forward[:2] - f[:2]) < 1: continue + # print("FINDING PATH TO FRONTIER: ", f) + # why do we need to set 127 as obstacles plan_success, path = oriented_astar(start=robot_pose_forward, occupancy_map=self.planning_map, footprint=self.footprint, @@ -417,6 +457,11 @@ def get_pose_from_tf(self, from_frame_id): return np.array([translation[0], translation[1], euler[2]]) def check_ready(self): + ''' + return true only when robot has map, + not planning, + haven't reach current goal + ''' if self.planning_map is None: return False @@ -434,7 +479,17 @@ def check_ready(self): return False def map_callback(self, occ_map_msg): + ''' + using new map to check if planned path has collision, if does, + find the nearest collision point and find new path to waypoints after it + do this check only when we have path generated before or from other func and currently not planning + ''' + # print('{}: New map received! MAP CALLBACK START'.format(self.robot_name)) if not self.is_planning: + plan_map_msg = OccupancyGrid() + plan_map_msg.header = occ_map_msg.header + plan_map_msg.info = occ_map_msg.info + occupancy_map = 255 - np.array(occ_map_msg.data, dtype=np.uint8) occupancy_map = occupancy_map.reshape((occ_map_msg.info.height, occ_map_msg.info.width)) @@ -447,10 +502,22 @@ def map_callback(self, occ_map_msg): resolution = occ_map_msg.info.resolution self.planning_map = Costmap(occupancy_map, resolution, origin) - - self.planning_map = cleanup_map_for_planning(occupancy_map=self.planning_map, kernel=self.kernel_cleanup, filter_obstacles=True) + # CHECK HERE + self.planning_map = cleanup_map_for_planning(occupancy_map=self.planning_map, kernel=self.kernel_cleanup, filter_obstacles=self.filter_obstacles_flag) self.planning_map.data = cv2.erode(self.planning_map.data, self.kernel_erosion, iterations=1) - + tmp_occ_map = self.planning_map.data.copy() + # map_visulize(tmp_occ_map, res=self.planning_map.resolution, origin=self.planning_map.origin) + # plt.savefig('/root/ws/map/map%d.png'%self.map_counter) + # plt.clf() + self.map_counter += 1 + tmp_occ_map_int = np.zeros_like(tmp_occ_map, dtype=np.int8) + tmp_occ_map_int[tmp_occ_map == Costmap.UNEXPLORED] = -1 + tmp_occ_map_int[tmp_occ_map == Costmap.FREE] = 0 + tmp_occ_map_int[tmp_occ_map == Costmap.OCCUPIED] = 100 + tmp_occ_map_int = np.flipud(tmp_occ_map_int) + plan_map_msg.data = tmp_occ_map_int.flatten().tolist() + self.plan_map_msg = plan_map_msg + self.plan_map_pub.publish(plan_map_msg) check_collision = False curr_time = rospy.get_time() if (curr_time - self.collision_check_timer) > self.collision_check_period: @@ -458,19 +525,22 @@ def map_callback(self, occ_map_msg): check_collision = True if self.curr_path is not None and check_collision: + # print("Start checking collision!") pixel_path = xy_to_rc(self.curr_path, self.planning_map) - try: + # find out the index of collision of current path collision_inds = np.nonzero(self.planning_map.data[pixel_path[:, 0].astype(int),pixel_path[:, 1].astype(int)] == Costmap.OCCUPIED)[0] except IndexError: return if collision_inds.shape[0] > 0: - print('{}: Collision ahead!'.format(self.robot_name)) + # print("collision_inds: ", collision_inds) + # print('{}: Collision ahead!'.format(self.robot_name)) self.collision_pub.publish(Empty()) self.is_planning = True + # print("waypoint_inds: ", self.waypoint_inds) rem_waypoint_inds = self.waypoint_inds[self.waypoint_inds > collision_inds[0]] rem_waypoints = self.curr_path[rem_waypoint_inds, :] @@ -478,8 +548,11 @@ def map_callback(self, occ_map_msg): self.footprint.draw_circumscribed(robot_pose, self.planning_map, self.draw_radius) full_path = robot_pose[None, :] self.waypoint_inds = np.array([0], dtype=int) + # print("rem_waypoints: ", rem_waypoint_inds) for pose in rem_waypoints: - plan_success, partial_path = oriented_astar(start=full_path[-1], occupancy_map=self.planning_map, footprint=self.footprint, obstacle_values=[Costmap.OCCUPIED], epsilon=1, goal=pose) + # print("Replan to waypoint: ", pose) + plan_success, partial_path = oriented_astar(start=full_path[-1], occupancy_map=self.planning_map, footprint=self.footprint, + obstacle_values=[Costmap.OCCUPIED], epsilon=1, goal=pose) if plan_success: full_path = np.vstack((full_path, partial_path)) self.waypoint_inds = np.concatenate((self.waypoint_inds, [full_path.shape[0] - 1])) @@ -507,6 +580,7 @@ def map_callback(self, occ_map_msg): else: for pose in full_path: self.add_waypoint(pose, path_msg) + print("Revised path generated!") self.path_pub.publish(path_msg) self.is_planning = False @@ -640,7 +714,9 @@ def find_frontiers(self, state): return frontier_goals, frontier_size def get_failsafe_path(self, state): + # rospy.loginfo("111111111ENTERED FAILSAFE PATH") path = None + self.plan_map_pub.publish(self.plan_map_msg) while path is None: random_start, start_sample_success = self.sample_free_pose() random_goal, goal_sample_success = self.sample_free_pose() @@ -657,6 +733,7 @@ def get_failsafe_path(self, state): if path_length > self.goal_check_radius: pixel_path = xy_to_rc(random_path, self.planning_map) try: + # also, why check again here? collision_inds = np.nonzero(self.planning_map.data[pixel_path[:, 0].astype(int), pixel_path[:, 1].astype(int)] == Costmap.OCCUPIED)[0] if collision_inds.shape[0] == 0: @@ -664,7 +741,10 @@ def get_failsafe_path(self, state): except IndexError: continue rospy.sleep(0.2) - + assert path is not None, "Fail-safe path generation failed!" + print(self.robot_name+" Fail-safe path generated!") + # data1 = self.planning_map.data.copy() + # data2 = path.copy() return path def sample_free_pose(self): @@ -706,7 +786,31 @@ def publish_path(self): robot_pose = self.get_pose_from_tf(self.robot_frame_id) full_path = robot_pose[None, :] self.waypoint_inds = np.array([0], dtype=int) + tmp_occ_map = self.planning_map.data.copy() + plan_map_msg = OccupancyGrid() + plan_map_msg.header = Header() + plan_map_msg.header.stamp = rospy.Time.now() + plan_map_msg.header.frame_id = self.world_frame_id + plan_map_msg.info.resolution = self.planning_map.resolution + plan_map_msg.info.width = self.planning_map.data.shape[1] + plan_map_msg.info.height = self.planning_map.data.shape[0] + plan_map_msg.info.origin.position.x = self.planning_map.origin[0] + plan_map_msg.info.origin.position.y = self.planning_map.origin[1] + tmp_occ_map_int = np.zeros_like(tmp_occ_map, dtype=np.int8) + tmp_occ_map_int[tmp_occ_map == Costmap.UNEXPLORED] = -1 + tmp_occ_map_int[tmp_occ_map == Costmap.FREE] = 0 + tmp_occ_map_int[tmp_occ_map == Costmap.OCCUPIED] = 100 + tmp_occ_map_int = np.flipud(tmp_occ_map_int) + plan_map_msg.data = tmp_occ_map_int.flatten().tolist() + self.plan_map_msg = plan_map_msg + self.plan_map_pub1.publish(plan_map_msg) + map_visulize(self.planning_map.data, self.planning_map.resolution, self.planning_map.origin) + plt.savefig('/root/ws/map/map%d.png'%self.map_counter) + plt.clf() + self.map_counter += 1 + # plt.show() for pose in path: + print('waypoint: ', pose) plan_success, partial_path = oriented_astar(start=full_path[-1], occupancy_map=self.planning_map, footprint=self.footprint, @@ -714,6 +818,7 @@ def publish_path(self): epsilon=1, goal=pose) if plan_success: + # Why we check the collision here when plan_success is True? Plan scale in cpp function is default to 1 pixel_path = xy_to_rc(partial_path, self.planning_map) try: collision_inds = np.nonzero(self.planning_map.data[pixel_path[:, 0].astype(int), @@ -721,9 +826,12 @@ def publish_path(self): if collision_inds.shape[0] == 0: full_path = np.vstack((full_path, partial_path)) self.waypoint_inds = np.concatenate((self.waypoint_inds, [full_path.shape[0] - 1])) + else: + raise ValueError('Collision detected in the path!') except IndexError: + print('IndexError11111111111111111111111111111111111') continue - + # assert full_path.shape[0] > 1, "Path generation failed!" if len(full_path.shape) > 1: path_length = 0 curr_pose = full_path[0] @@ -751,7 +859,7 @@ def publish_path(self): self.path_pub.publish(path_msg) - rospy.loginfo("{}: New path published!".format(self.robot_name)) + rospy.loginfo("{}: Brand New path published!".format(self.robot_name)) def main(args): diff --git a/ROAM-Planning/src/footprints/footprint_points.py b/ROAM-Planning/src/footprints/footprint_points.py index 640b5ec..871e7a6 100644 --- a/ROAM-Planning/src/footprints/footprint_points.py +++ b/ROAM-Planning/src/footprints/footprint_points.py @@ -54,3 +54,49 @@ def get_tricky_oval_footprint(): [1303.15, -286.54], [1341.34, -118.37]] ) / 1000. + +def generate_clockwise_square(x_size, y_size, points_per_side): + """ + Generates points on a clockwise square with the given parameters + :param x_size float: size in the x axis + :param y_size float: size in the y axis + :param points_per_side int: number of points to generate per side + :return array(N, 2)[float]: points defining the square + """ + y_range = np.linspace(-y_size / 2, y_size / 2, points_per_side, endpoint=True) + x_range = np.linspace(-x_size / 2, x_size / 2, points_per_side, endpoint=True) + y_range = np.vstack(([np.zeros_like(y_range)], [y_range])).T + x_range = np.vstack(([[x_range], np.zeros_like(x_range)])).T + + back_points = y_range + x_range[0, :] + + front_points = y_range + x_range[-1, :] + zero_ind = np.argmin(np.abs(front_points[:, 1])) + front_first_half = front_points[zero_ind:, :] + front_last_half = front_points[:zero_ind] + + left_points = x_range + y_range[-1, :] + + right_points = x_range + y_range[0, :] + right_points = right_points[::-1, :] + + # points = np.vstack((back_points, front_points, left_points, right_points)) + points = np.vstack((front_first_half, right_points, back_points, left_points, front_last_half)) + return points + + +def get_jackal_footprint(points_per_side=11, debug=False): + """ + Gives an estimate of the ClearPath jackal robot footprint + :param points_per_side int: number of points per side to discretize + :param debug bool: show plots? + :return array(N, 2)[float]: points defining the footprint + """ + assert points_per_side % 2 != 0 and "Must be odd, so 0 is generated" + length = .508 + width = .430 + points = generate_clockwise_square(length, width, points_per_side) + if debug: + plt.scatter(points[:, 0], points[:, 1]) + plt.show() + return points \ No newline at end of file From 631361542070066649886fffc42709153e3ab34b Mon Sep 17 00:00:00 2001 From: Tianji Date: Sat, 21 Sep 2024 00:20:36 +0000 Subject: [PATCH 3/7] remove debug map plot --- .../exploration/ga_multi_exploration_node.py | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/ROAM-Planning/src/exploration/ga_multi_exploration_node.py b/ROAM-Planning/src/exploration/ga_multi_exploration_node.py index 6c83554..944fffe 100755 --- a/ROAM-Planning/src/exploration/ga_multi_exploration_node.py +++ b/ROAM-Planning/src/exploration/ga_multi_exploration_node.py @@ -47,7 +47,6 @@ def map_visulize(image_path, res = 1, origin = np.array([0, 0]), goal = None, ra ax.set_title('Map') if goal is not None and radius is not None: ax.add_patch(plt.Circle(goal, radius, color='r', alpha=0.3)) - # plt.show() return ax class GradAscentMultiExplorationAgent: @@ -55,11 +54,11 @@ def __init__(self): self.robot_name = '/' + rospy.get_param("~agent_name", 'husky') footprint_type = rospy.get_param(self.robot_name + '/footprint/type') - if not os.path.exists('/root/ws/map'): - os.makedirs('/root/ws/map') - files = os.listdir('/root/ws/map') - for f in files: - os.remove('/root/ws/map/' + f) + # if not os.path.exists('/root/ws/map'): + # os.makedirs('/root/ws/map') + # files = os.listdir('/root/ws/map') + # for f in files: + # os.remove('/root/ws/map/' + f) self.filter_obstacles_flag = False self.map_counter = 0 if footprint_type == 'tricky_circle': @@ -804,9 +803,9 @@ def publish_path(self): plan_map_msg.data = tmp_occ_map_int.flatten().tolist() self.plan_map_msg = plan_map_msg self.plan_map_pub1.publish(plan_map_msg) - map_visulize(self.planning_map.data, self.planning_map.resolution, self.planning_map.origin) - plt.savefig('/root/ws/map/map%d.png'%self.map_counter) - plt.clf() + # map_visulize(self.planning_map.data, self.planning_map.resolution, self.planning_map.origin) + # plt.savefig('/root/ws/map/map%d.png'%self.map_counter) + # plt.clf() self.map_counter += 1 # plt.show() for pose in path: From 8b9d4143fc996850713251a72ad264b3fea812aa Mon Sep 17 00:00:00 2001 From: Tianji Date: Tue, 1 Oct 2024 08:46:16 +0000 Subject: [PATCH 4/7] add metric params --- .../multi_robot_semantic_mapping_params.yaml | 2 +- .../multi_robot_exploration_params.yaml | 15 ++-- .../exploration/ga_multi_exploration_node.py | 81 +++---------------- .../navigation/simple_path_navigation_node.py | 36 +++++---- 4 files changed, 43 insertions(+), 91 deletions(-) diff --git a/ROAM-Mapping/params/multi_robot_semantic_mapping_params.yaml b/ROAM-Mapping/params/multi_robot_semantic_mapping_params.yaml index ccb19bb..92653e7 100755 --- a/ROAM-Mapping/params/multi_robot_semantic_mapping_params.yaml +++ b/ROAM-Mapping/params/multi_robot_semantic_mapping_params.yaml @@ -5,7 +5,7 @@ octomap: # resolution: 0.3 # max_range: 10.0 # raycast_range: 0.5 - resolution: 0.2 + resolution: 0.3 max_range: 15.0 raycast_range: 10.0 clamping_thres_min: 0.006692851 diff --git a/ROAM-Planning/params/multi_robot_exploration_params.yaml b/ROAM-Planning/params/multi_robot_exploration_params.yaml index 2ca9353..5e7b032 100644 --- a/ROAM-Planning/params/multi_robot_exploration_params.yaml +++ b/ROAM-Planning/params/multi_robot_exploration_params.yaml @@ -1,7 +1,7 @@ footprint: type: 'jackal' angular_resolution: 0.01745 - inflation_scale: 1.3 + inflation_scale: 1.4 planning: SE2_radius: 2 # epsilon_max, radius of the SE2 space pose sample @@ -15,7 +15,10 @@ planning: theta_coeff: 0.1 min_frontier_size: 30 goal_check_radius: 0.3 # radius of goal check for viewpoint planner - draw_radius: 2 # radius to clear obstacles around the robot in pixels!!! + draw_radius: 0.4 # radius to clear obstacles around the robot in meters + clean_kernel_size: 0.8 # size of the mask for closing unknown space in meters + erode_kernel_size: 0.8 # size of the mask for eroding free sapce in meters + median_filter_enable: false # enable median filter for pepper noise, only enable if your map is fine enough to speed up planning map_topic: '/$(arg name)/occupancy_map_2D' collision_check_period: 1 @@ -24,15 +27,15 @@ planning: horizon: 5 # sampling waypoints on path delta_q: 10.0 # d_q = F + SE2_radius gamma_q: 0.01 # trade off between between self informativeness and overlap FOV penalty - robot_colision_coeff: 2 # penalty for robot planned traj repeat, significantly impact gradient + robot_colision_coeff: 5 # penalty for robot planned similar view point, significantly impact gradient epsilon: 0.1 # consensus step size - should_plan_threshold: 0.3 # thresh_p i.e. threshold to start alg3 - need_plan_threshold: 0.2 # if less then this portion of the agents need to plan, all robots stop optimizing i.e. threshold to quit alg3 + should_plan_threshold: 0.4 # thresh_p i.e. threshold to start alg3 + need_plan_threshold: 0.3 # if less then this portion of the agents need to plan, all robots stop optimizing i.e. threshold to quit alg3 # quit alg3 when some robots finish planning and start to approach the goal need_plan_period: 1.0 # t_p navigation: tau: 0.005 # sleep time for navigator to publish the next waypoint dist: 0.3 # dist to determine if the robot has reached the waypoint - safety_dist: 1.0 # peer safe distance + safety_dist: 0.1 # peer safe distance publish_odom: true diff --git a/ROAM-Planning/src/exploration/ga_multi_exploration_node.py b/ROAM-Planning/src/exploration/ga_multi_exploration_node.py index 944fffe..2994df3 100755 --- a/ROAM-Planning/src/exploration/ga_multi_exploration_node.py +++ b/ROAM-Planning/src/exploration/ga_multi_exploration_node.py @@ -17,7 +17,6 @@ from geometry_msgs.msg import PoseStamped from rospy.numpy_msg import numpy_msg from rospy_tutorials.msg import Floats -import os from utilities.frontier_utils import extract_frontiers, cleanup_map_for_planning from footprints.footprint_points import get_tricky_circular_footprint, get_tricky_oval_footprint, get_jackal_footprint from footprints.footprints import CustomFootprint @@ -52,15 +51,8 @@ def map_visulize(image_path, res = 1, origin = np.array([0, 0]), goal = None, ra class GradAscentMultiExplorationAgent: def __init__(self): self.robot_name = '/' + rospy.get_param("~agent_name", 'husky') - footprint_type = rospy.get_param(self.robot_name + '/footprint/type') - # if not os.path.exists('/root/ws/map'): - # os.makedirs('/root/ws/map') - # files = os.listdir('/root/ws/map') - # for f in files: - # os.remove('/root/ws/map/' + f) - self.filter_obstacles_flag = False - self.map_counter = 0 + self.filter_obstacles_flag = rospy.get_param(self.robot_name + '/planning/median_filter_enable') if footprint_type == 'tricky_circle': footprint_points = get_tricky_circular_footprint() elif footprint_type == 'tricky_oval': @@ -135,7 +127,7 @@ def __init__(self): self.waypoint_inds = None self.goal = None self.goal_check_radius = rospy.get_param(self.robot_name + '/planning/goal_check_radius') - self.draw_radius = rospy.get_param(self.robot_name + '/planning/draw_radius') + self.draw_radius = int(np.round(rospy.get_param(self.robot_name + '/planning/draw_radius')/rospy.get_param(self.robot_name + '/octomap/resolution'))) self.map_sub = rospy.Subscriber(rospy.get_param(self.robot_name + '/planning/map_topic'), OccupancyGrid, self.map_callback) self.collision_check_timer = rospy.get_time() @@ -181,11 +173,12 @@ def __init__(self): self.vectorized_rc = None self.vectorized_xy = None self.eval_cells = None - self.kernel_cleanup = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3)) - self.kernel_erosion = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3)) - # self.kernel_cleanup = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) - # self.kernel_erosion = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) - self.plan_map_pub1 = rospy.Publisher(self.robot_name + "/PLANMAP1", OccupancyGrid, queue_size = 1, latch=True) + clean_kernel_size = int(round(rospy.get_param(self.robot_name + '/planning/clean_kernel_size')\ + / rospy.get_param(self.robot_name + '/octomap/resolution'))) + erode_kernel_size = int(round(rospy.get_param(self.robot_name + '/planning/erode_kernel_size')\ + / rospy.get_param(self.robot_name + '/octomap/resolution'))) + self.kernel_cleanup = cv2.getStructuringElement(cv2.MORPH_RECT, (clean_kernel_size, clean_kernel_size)) + self.kernel_erosion = cv2.getStructuringElement(cv2.MORPH_RECT, (erode_kernel_size, erode_kernel_size)) self.plan_map_pub = rospy.Publisher(self.robot_name + "/PLANMAP", OccupancyGrid, queue_size = 1, latch=True) rospy.sleep(1) print('Multi agent exploration agent \'{}\' initialized!'.format(self.robot_name)) @@ -220,8 +213,6 @@ def need_plan_callback(self, need_plan_msg): def optimize_pose(self): if self.is_planning: return - - # print('IN OPTIMIZE_POSE Planning started for agent \'{}\'!'.format(self.robot_name)) self.is_planning = True self.consensus_delta = None self.received_team_path_num = 0 @@ -281,6 +272,9 @@ def optimize_pose(self): self.is_planning = False def make_init_plan(self): + ''' + Initialize team path for all robots by planning to frontiers or failsafe path + ''' print('{}: LOOKING FOR FRONTIERS!'.format(self.robot_name)) if self.raytrace_precomputed is None: print('{}: Initializing ray-tracing!'.format(self.robot_name)) @@ -320,15 +314,13 @@ def make_init_plan(self): rospy.sleep(0.1) self.planning_map.data = np.array(self.planning_map.data) - # self.footprint.draw_circumscribed(robot_pose, self.planning_map, self.draw_radius) + self.footprint.draw_circumscribed(robot_pose, self.planning_map, self.draw_radius) path = self.compute_frontier_path(robot_pose) if path.shape[0] >= self.horizon: - # print('length of path before sampling: ', path.shape[0]) path_inds = np.linspace(0, path.shape[0], self.horizon, endpoint=False).astype(int) path_sampled = path[path_inds] else: - # print('length of path: ', path.shape[0]) path_sampled = np.zeros((self.horizon, 3)) path_sampled[:path.shape[0]] = path path_sampled[path.shape[0]:] = path[-1] @@ -343,14 +335,10 @@ def compute_frontier_path(self, robot_pose): robot_pose_forward = robot_pose.copy() robot_pose_forward[0] = robot_pose[0] + 1 * np.cos(robot_pose[2]) robot_pose_forward[1] = robot_pose[1] + 1 * np.sin(robot_pose[2]) - - # print("%d frontiers found!" % len(frontiers)) if frontiers is not None: for i, f in enumerate(frontiers): if np.linalg.norm(robot_pose_forward[:2] - f[:2]) < 1: continue - # print("FINDING PATH TO FRONTIER: ", f) - # why do we need to set 127 as obstacles plan_success, path = oriented_astar(start=robot_pose_forward, occupancy_map=self.planning_map, footprint=self.footprint, @@ -459,7 +447,7 @@ def check_ready(self): ''' return true only when robot has map, not planning, - haven't reach current goal + and is close enough to the goal or has no goal ''' if self.planning_map is None: return False @@ -483,7 +471,6 @@ def map_callback(self, occ_map_msg): find the nearest collision point and find new path to waypoints after it do this check only when we have path generated before or from other func and currently not planning ''' - # print('{}: New map received! MAP CALLBACK START'.format(self.robot_name)) if not self.is_planning: plan_map_msg = OccupancyGrid() plan_map_msg.header = occ_map_msg.header @@ -501,14 +488,9 @@ def map_callback(self, occ_map_msg): resolution = occ_map_msg.info.resolution self.planning_map = Costmap(occupancy_map, resolution, origin) - # CHECK HERE self.planning_map = cleanup_map_for_planning(occupancy_map=self.planning_map, kernel=self.kernel_cleanup, filter_obstacles=self.filter_obstacles_flag) self.planning_map.data = cv2.erode(self.planning_map.data, self.kernel_erosion, iterations=1) tmp_occ_map = self.planning_map.data.copy() - # map_visulize(tmp_occ_map, res=self.planning_map.resolution, origin=self.planning_map.origin) - # plt.savefig('/root/ws/map/map%d.png'%self.map_counter) - # plt.clf() - self.map_counter += 1 tmp_occ_map_int = np.zeros_like(tmp_occ_map, dtype=np.int8) tmp_occ_map_int[tmp_occ_map == Costmap.UNEXPLORED] = -1 tmp_occ_map_int[tmp_occ_map == Costmap.FREE] = 0 @@ -524,7 +506,6 @@ def map_callback(self, occ_map_msg): check_collision = True if self.curr_path is not None and check_collision: - # print("Start checking collision!") pixel_path = xy_to_rc(self.curr_path, self.planning_map) try: # find out the index of collision of current path @@ -533,13 +514,10 @@ def map_callback(self, occ_map_msg): return if collision_inds.shape[0] > 0: - # print("collision_inds: ", collision_inds) - # print('{}: Collision ahead!'.format(self.robot_name)) self.collision_pub.publish(Empty()) self.is_planning = True - # print("waypoint_inds: ", self.waypoint_inds) rem_waypoint_inds = self.waypoint_inds[self.waypoint_inds > collision_inds[0]] rem_waypoints = self.curr_path[rem_waypoint_inds, :] @@ -547,9 +525,7 @@ def map_callback(self, occ_map_msg): self.footprint.draw_circumscribed(robot_pose, self.planning_map, self.draw_radius) full_path = robot_pose[None, :] self.waypoint_inds = np.array([0], dtype=int) - # print("rem_waypoints: ", rem_waypoint_inds) for pose in rem_waypoints: - # print("Replan to waypoint: ", pose) plan_success, partial_path = oriented_astar(start=full_path[-1], occupancy_map=self.planning_map, footprint=self.footprint, obstacle_values=[Costmap.OCCUPIED], epsilon=1, goal=pose) if plan_success: @@ -713,9 +689,7 @@ def find_frontiers(self, state): return frontier_goals, frontier_size def get_failsafe_path(self, state): - # rospy.loginfo("111111111ENTERED FAILSAFE PATH") path = None - self.plan_map_pub.publish(self.plan_map_msg) while path is None: random_start, start_sample_success = self.sample_free_pose() random_goal, goal_sample_success = self.sample_free_pose() @@ -732,7 +706,6 @@ def get_failsafe_path(self, state): if path_length > self.goal_check_radius: pixel_path = xy_to_rc(random_path, self.planning_map) try: - # also, why check again here? collision_inds = np.nonzero(self.planning_map.data[pixel_path[:, 0].astype(int), pixel_path[:, 1].astype(int)] == Costmap.OCCUPIED)[0] if collision_inds.shape[0] == 0: @@ -742,8 +715,6 @@ def get_failsafe_path(self, state): rospy.sleep(0.2) assert path is not None, "Fail-safe path generation failed!" print(self.robot_name+" Fail-safe path generated!") - # data1 = self.planning_map.data.copy() - # data2 = path.copy() return path def sample_free_pose(self): @@ -785,29 +756,6 @@ def publish_path(self): robot_pose = self.get_pose_from_tf(self.robot_frame_id) full_path = robot_pose[None, :] self.waypoint_inds = np.array([0], dtype=int) - tmp_occ_map = self.planning_map.data.copy() - plan_map_msg = OccupancyGrid() - plan_map_msg.header = Header() - plan_map_msg.header.stamp = rospy.Time.now() - plan_map_msg.header.frame_id = self.world_frame_id - plan_map_msg.info.resolution = self.planning_map.resolution - plan_map_msg.info.width = self.planning_map.data.shape[1] - plan_map_msg.info.height = self.planning_map.data.shape[0] - plan_map_msg.info.origin.position.x = self.planning_map.origin[0] - plan_map_msg.info.origin.position.y = self.planning_map.origin[1] - tmp_occ_map_int = np.zeros_like(tmp_occ_map, dtype=np.int8) - tmp_occ_map_int[tmp_occ_map == Costmap.UNEXPLORED] = -1 - tmp_occ_map_int[tmp_occ_map == Costmap.FREE] = 0 - tmp_occ_map_int[tmp_occ_map == Costmap.OCCUPIED] = 100 - tmp_occ_map_int = np.flipud(tmp_occ_map_int) - plan_map_msg.data = tmp_occ_map_int.flatten().tolist() - self.plan_map_msg = plan_map_msg - self.plan_map_pub1.publish(plan_map_msg) - # map_visulize(self.planning_map.data, self.planning_map.resolution, self.planning_map.origin) - # plt.savefig('/root/ws/map/map%d.png'%self.map_counter) - # plt.clf() - self.map_counter += 1 - # plt.show() for pose in path: print('waypoint: ', pose) plan_success, partial_path = oriented_astar(start=full_path[-1], @@ -817,7 +765,6 @@ def publish_path(self): epsilon=1, goal=pose) if plan_success: - # Why we check the collision here when plan_success is True? Plan scale in cpp function is default to 1 pixel_path = xy_to_rc(partial_path, self.planning_map) try: collision_inds = np.nonzero(self.planning_map.data[pixel_path[:, 0].astype(int), @@ -828,9 +775,7 @@ def publish_path(self): else: raise ValueError('Collision detected in the path!') except IndexError: - print('IndexError11111111111111111111111111111111111') continue - # assert full_path.shape[0] > 1, "Path generation failed!" if len(full_path.shape) > 1: path_length = 0 curr_pose = full_path[0] diff --git a/ROAM-Planning/src/navigation/simple_path_navigation_node.py b/ROAM-Planning/src/navigation/simple_path_navigation_node.py index c648be9..dd6fcc4 100755 --- a/ROAM-Planning/src/navigation/simple_path_navigation_node.py +++ b/ROAM-Planning/src/navigation/simple_path_navigation_node.py @@ -12,7 +12,7 @@ from nav_msgs.msg import Path, Odometry from std_msgs.msg import Empty -from geometry_msgs.msg import Pose, PoseStamped +from geometry_msgs.msg import Pose, PoseStamped, Twist class PathNavigation: @@ -35,6 +35,7 @@ def __init__(self): self.collision_sub = message_filters.Subscriber(self.robot_name + '/planner/collision', Empty, queue_size = 15) self.collision_sub.registerCallback(self.collision_callback) + self.cmd_vel_pub = rospy.Publisher(self.robot_name + '/cmd_vel', Twist, queue_size = 15) self.path = None self.position_cmd_pub = rospy.Publisher(self.robot_name + "/planner/position_cmd", PoseStamped, queue_size = 15) @@ -54,18 +55,19 @@ def __init__(self): peer_collision = False for i in range(num_agents): - if self.robot_name[-1] == str(i + 1): - continue - peer_frame_id = self.robot_name[:-1] + str(i + 1) + '/base_link' # Assuming the robot frame is named as 'robot_i/base_link' - peer_pose = self.get_pose_from_tf(peer_frame_id) - if np.linalg.norm(np.array([next_pose.position.x - peer_pose[0], next_pose.position.y - peer_pose[1]])) <= safety_dist: - peer_collision = True - rospy.loginfo("{}: Possible close call! Stopping for deconflicting...".format(self.robot_name)) - break + if self.robot_name[-1] == str(i + 1): + continue + peer_frame_id = self.robot_name[:-1] + str(i + 1) + '/base_link' # Assuming the robot frame is named as 'robot_i/base_link' + peer_pose = self.get_pose_from_tf(peer_frame_id) + if np.linalg.norm(np.array([next_pose.position.x - peer_pose[0], next_pose.position.y - peer_pose[1]])) <= safety_dist: + peer_collision = True + rospy.loginfo("{}: Possible close call! Stopping for deconflicting...".format(self.robot_name)) + break if peer_collision: - rospy.sleep(0.5) - continue + self.cmd_vel_pub.publish(Twist()) + rospy.sleep(0.5) + continue position_cmd_msg = PoseStamped() position_cmd_msg.pose = next_pose @@ -81,6 +83,8 @@ def __init__(self): self.path = self.path[1:] else: self.path = None + else: + self.cmd_vel_pub.publish(Twist()) rospy.sleep(tau) def get_pose_from_tf(self, from_frame_id): @@ -109,12 +113,12 @@ def get_tf(self, from_frame_id): no_transfrom = True while no_transfrom: try: - (translation, rotation) = self.tf_listener.lookupTransform(self.world_frame_id, - from_frame_id, - rospy.Time(0)) - no_transfrom = False + (translation, rotation) = self.tf_listener.lookupTransform(self.world_frame_id, + from_frame_id, + rospy.Time(0)) + no_transfrom = False except tf.LookupException: - continue + continue return (translation, rotation) def path_callback(self, path_msg): From 77495e352bc09cafad5c7ab913831b1b8397bdcb Mon Sep 17 00:00:00 2001 From: Tianji Date: Wed, 2 Oct 2024 23:26:06 -0700 Subject: [PATCH 5/7] update node for race condition --- .../multi_robot_exploration_params.yaml | 10 ++--- .../exploration/ga_multi_exploration_node.py | 37 ++++--------------- 2 files changed, 13 insertions(+), 34 deletions(-) diff --git a/ROAM-Planning/params/multi_robot_exploration_params.yaml b/ROAM-Planning/params/multi_robot_exploration_params.yaml index 5e7b032..156a44a 100644 --- a/ROAM-Planning/params/multi_robot_exploration_params.yaml +++ b/ROAM-Planning/params/multi_robot_exploration_params.yaml @@ -15,22 +15,22 @@ planning: theta_coeff: 0.1 min_frontier_size: 30 goal_check_radius: 0.3 # radius of goal check for viewpoint planner - draw_radius: 0.4 # radius to clear obstacles around the robot in meters - clean_kernel_size: 0.8 # size of the mask for closing unknown space in meters + draw_radius: 0.2 # radius to clear obstacles around the robot in meters + clean_kernel_size: 0.6 # size of the mask for closing unknown space in meters erode_kernel_size: 0.8 # size of the mask for eroding free sapce in meters median_filter_enable: false # enable median filter for pepper noise, only enable if your map is fine enough to speed up planning map_topic: '/$(arg name)/occupancy_map_2D' collision_check_period: 1 multi: - num_agents: 2 + num_agents: 4 horizon: 5 # sampling waypoints on path delta_q: 10.0 # d_q = F + SE2_radius gamma_q: 0.01 # trade off between between self informativeness and overlap FOV penalty robot_colision_coeff: 5 # penalty for robot planned similar view point, significantly impact gradient epsilon: 0.1 # consensus step size - should_plan_threshold: 0.4 # thresh_p i.e. threshold to start alg3 - need_plan_threshold: 0.3 # if less then this portion of the agents need to plan, all robots stop optimizing i.e. threshold to quit alg3 + should_plan_threshold: 0.1 # thresh_p i.e. threshold to start alg3 + need_plan_threshold: 0.1 # if less then this portion of the agents need to plan, all robots stop optimizing i.e. threshold to quit alg3 # quit alg3 when some robots finish planning and start to approach the goal need_plan_period: 1.0 # t_p diff --git a/ROAM-Planning/src/exploration/ga_multi_exploration_node.py b/ROAM-Planning/src/exploration/ga_multi_exploration_node.py index 2994df3..13ca6b4 100755 --- a/ROAM-Planning/src/exploration/ga_multi_exploration_node.py +++ b/ROAM-Planning/src/exploration/ga_multi_exploration_node.py @@ -25,28 +25,6 @@ from mapping.costmap import Costmap from utilities.sensor_utils import bresenham2d, bresenham2d_with_intensities -def map_visulize(image_path, res = 1, origin = np.array([0, 0]), goal = None, radius = None): - if isinstance(image_path, str): - img = cv2.imread(image_path, cv2.IMREAD_GRAYSCALE) - if img is None: - raise FileNotFoundError(f'Image file not found: {image_path}') - elif isinstance(image_path, np.ndarray): - img = image_path - else: - raise ValueError('Invalid image path') - img_height, img_width = img.shape - img = np.flipud(img) - map_width = img_width * res - map_height = img_height * res - fig, ax = plt.subplots() - extent = [origin[0], origin[0] + map_width, origin[1], origin[1] + map_height] - ax.imshow(img, cmap='gray', extent=extent, origin='lower') - ax.set_xlabel('X (meters)') - ax.set_ylabel('Y (meters)') - ax.set_title('Map') - if goal is not None and radius is not None: - ax.add_patch(plt.Circle(goal, radius, color='r', alpha=0.3)) - return ax class GradAscentMultiExplorationAgent: def __init__(self): @@ -70,6 +48,14 @@ def __init__(self): footprint_points = None assert False and "footprint type specified not supported." + clean_kernel_size = int(round(rospy.get_param(self.robot_name + '/planning/clean_kernel_size')\ + / rospy.get_param(self.robot_name + '/octomap/resolution'))) + erode_kernel_size = int(round(rospy.get_param(self.robot_name + '/planning/erode_kernel_size')\ + / rospy.get_param(self.robot_name + '/octomap/resolution'))) + self.kernel_cleanup = cv2.getStructuringElement(cv2.MORPH_RECT, (clean_kernel_size, clean_kernel_size)) + self.kernel_erosion = cv2.getStructuringElement(cv2.MORPH_RECT, (erode_kernel_size, erode_kernel_size)) + self.plan_map_pub = rospy.Publisher(self.robot_name + "/PLANMAP", OccupancyGrid, queue_size = 1, latch=True) + self.footprint = CustomFootprint(footprint_points=footprint_points, angular_resolution=np.pi / 4, inflation_scale=rospy.get_param(self.robot_name + '/footprint/inflation_scale')) @@ -173,13 +159,6 @@ def __init__(self): self.vectorized_rc = None self.vectorized_xy = None self.eval_cells = None - clean_kernel_size = int(round(rospy.get_param(self.robot_name + '/planning/clean_kernel_size')\ - / rospy.get_param(self.robot_name + '/octomap/resolution'))) - erode_kernel_size = int(round(rospy.get_param(self.robot_name + '/planning/erode_kernel_size')\ - / rospy.get_param(self.robot_name + '/octomap/resolution'))) - self.kernel_cleanup = cv2.getStructuringElement(cv2.MORPH_RECT, (clean_kernel_size, clean_kernel_size)) - self.kernel_erosion = cv2.getStructuringElement(cv2.MORPH_RECT, (erode_kernel_size, erode_kernel_size)) - self.plan_map_pub = rospy.Publisher(self.robot_name + "/PLANMAP", OccupancyGrid, queue_size = 1, latch=True) rospy.sleep(1) print('Multi agent exploration agent \'{}\' initialized!'.format(self.robot_name)) From a66fe0ba20709e6773f33aac8f67039e679136a4 Mon Sep 17 00:00:00 2001 From: Tianji Date: Tue, 15 Oct 2024 08:14:49 +0000 Subject: [PATCH 6/7] set better parameter for sim --- .../multi_robot_exploration_params.yaml | 47 +++++++++---------- 1 file changed, 23 insertions(+), 24 deletions(-) diff --git a/ROAM-Planning/params/multi_robot_exploration_params.yaml b/ROAM-Planning/params/multi_robot_exploration_params.yaml index 156a44a..238c495 100644 --- a/ROAM-Planning/params/multi_robot_exploration_params.yaml +++ b/ROAM-Planning/params/multi_robot_exploration_params.yaml @@ -4,38 +4,37 @@ footprint: inflation_scale: 1.4 planning: - SE2_radius: 2 # epsilon_max, radius of the SE2 space pose sample - max_iter: 10 # k_p, maximux iters of traj optimization - max_path_length: 100 # max length of the path in meters - step_size: 0.001 # scale factor of time varing step size of local update - angular_range: 1.570796327 # sample parameters for pose approx + SE2_radius: 2 + max_iter: 10 + max_path_length: 15.0 + step_size: 0.001 + angular_range: 1.570796327 angular_samples: 32 num_orientation_samples: 8 - distance_coeff: 0.01 # gamma_c tradeoff between informativeness and collision avoidance + distance_coeff: 0.01 theta_coeff: 0.1 min_frontier_size: 30 - goal_check_radius: 0.3 # radius of goal check for viewpoint planner - draw_radius: 0.2 # radius to clear obstacles around the robot in meters - clean_kernel_size: 0.6 # size of the mask for closing unknown space in meters - erode_kernel_size: 0.8 # size of the mask for eroding free sapce in meters - median_filter_enable: false # enable median filter for pepper noise, only enable if your map is fine enough to speed up planning + goal_check_radius: 0.3 + draw_radius: 0.2 + clean_kernel_size: 0.6 + erode_kernel_size: 0.8 + median_filter_enable: false map_topic: '/$(arg name)/occupancy_map_2D' collision_check_period: 1 multi: - num_agents: 4 - horizon: 5 # sampling waypoints on path - delta_q: 10.0 # d_q = F + SE2_radius - gamma_q: 0.01 # trade off between between self informativeness and overlap FOV penalty - robot_colision_coeff: 5 # penalty for robot planned similar view point, significantly impact gradient - epsilon: 0.1 # consensus step size - should_plan_threshold: 0.1 # thresh_p i.e. threshold to start alg3 - need_plan_threshold: 0.1 # if less then this portion of the agents need to plan, all robots stop optimizing i.e. threshold to quit alg3 - # quit alg3 when some robots finish planning and start to approach the goal - need_plan_period: 1.0 # t_p + num_agents: 2 + horizon: 10 + delta_q: 10.0 + gamma_q: 0.01 + robot_colision_coeff: 5 + epsilon: 0.1 + should_plan_threshold: 0.6 + need_plan_threshold: 0.6 + need_plan_period: 0.05 navigation: - tau: 0.005 # sleep time for navigator to publish the next waypoint - dist: 0.3 # dist to determine if the robot has reached the waypoint - safety_dist: 0.1 # peer safe distance + tau: 0.005 + dist: 0.2 + safety_dist: 0.1 publish_odom: true From bb7c1386e4f9a98371d813b8f956450ac3aad1d4 Mon Sep 17 00:00:00 2001 From: Tianji Date: Tue, 15 Oct 2024 08:26:44 +0000 Subject: [PATCH 7/7] remove devel map topic --- .../exploration/ga_multi_exploration_node.py | 30 +++++-------------- 1 file changed, 7 insertions(+), 23 deletions(-) diff --git a/ROAM-Planning/src/exploration/ga_multi_exploration_node.py b/ROAM-Planning/src/exploration/ga_multi_exploration_node.py index 13ca6b4..8d6072c 100755 --- a/ROAM-Planning/src/exploration/ga_multi_exploration_node.py +++ b/ROAM-Planning/src/exploration/ga_multi_exploration_node.py @@ -9,8 +9,6 @@ import cv2 import numpy as np -import matplotlib.pyplot as plt - from nav_msgs.msg import OccupancyGrid from nav_msgs.msg import Path from std_msgs.msg import Empty, Header @@ -251,9 +249,6 @@ def optimize_pose(self): self.is_planning = False def make_init_plan(self): - ''' - Initialize team path for all robots by planning to frontiers or failsafe path - ''' print('{}: LOOKING FOR FRONTIERS!'.format(self.robot_name)) if self.raytrace_precomputed is None: print('{}: Initializing ray-tracing!'.format(self.robot_name)) @@ -423,11 +418,6 @@ def get_pose_from_tf(self, from_frame_id): return np.array([translation[0], translation[1], euler[2]]) def check_ready(self): - ''' - return true only when robot has map, - not planning, - and is close enough to the goal or has no goal - ''' if self.planning_map is None: return False @@ -445,15 +435,7 @@ def check_ready(self): return False def map_callback(self, occ_map_msg): - ''' - using new map to check if planned path has collision, if does, - find the nearest collision point and find new path to waypoints after it - do this check only when we have path generated before or from other func and currently not planning - ''' if not self.is_planning: - plan_map_msg = OccupancyGrid() - plan_map_msg.header = occ_map_msg.header - plan_map_msg.info = occ_map_msg.info occupancy_map = 255 - np.array(occ_map_msg.data, dtype=np.uint8) occupancy_map = occupancy_map.reshape((occ_map_msg.info.height, @@ -475,9 +457,12 @@ def map_callback(self, occ_map_msg): tmp_occ_map_int[tmp_occ_map == Costmap.FREE] = 0 tmp_occ_map_int[tmp_occ_map == Costmap.OCCUPIED] = 100 tmp_occ_map_int = np.flipud(tmp_occ_map_int) - plan_map_msg.data = tmp_occ_map_int.flatten().tolist() - self.plan_map_msg = plan_map_msg - self.plan_map_pub.publish(plan_map_msg) + # plan_map_msg = OccupancyGrid() + # plan_map_msg.header = occ_map_msg.header + # plan_map_msg.info = occ_map_msg.info + # plan_map_msg.data = tmp_occ_map_int.flatten().tolist() + # self.plan_map_msg = plan_map_msg + # self.plan_map_pub.publish(plan_map_msg) check_collision = False curr_time = rospy.get_time() if (curr_time - self.collision_check_timer) > self.collision_check_period: @@ -487,7 +472,6 @@ def map_callback(self, occ_map_msg): if self.curr_path is not None and check_collision: pixel_path = xy_to_rc(self.curr_path, self.planning_map) try: - # find out the index of collision of current path collision_inds = np.nonzero(self.planning_map.data[pixel_path[:, 0].astype(int),pixel_path[:, 1].astype(int)] == Costmap.OCCUPIED)[0] except IndexError: return @@ -736,7 +720,7 @@ def publish_path(self): full_path = robot_pose[None, :] self.waypoint_inds = np.array([0], dtype=int) for pose in path: - print('waypoint: ', pose) + # print('waypoint: ', pose) plan_success, partial_path = oriented_astar(start=full_path[-1], occupancy_map=self.planning_map, footprint=self.footprint,