diff --git a/.gitmodules b/.gitmodules index 2eae769..a9e49ff 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,8 +1,16 @@ [submodule "turtlebot3_simulations"] path = turtlebot3_simulations url = https://github.com/NESTLab/turtlebot3_simulations.git - branch = noetic-devel + branch = changesForRLTesting [submodule "coms"] path = coms url = https://github.com/NESTLab/coms.git branch = main +[submodule "slam_gmapping"] + path = slam_gmapping + url = git@github.com:tberg1234/slam_gmapping.git + branch = melodic-devel +[submodule "frontier_rl"] + path = frontier_rl + url = git@github.com:tberg1234/Frontier_ML.git + branch = fullObs diff --git a/Dockerfile b/Dockerfile old mode 100644 new mode 100755 index 70b3e20..8da5c3f --- a/Dockerfile +++ b/Dockerfile @@ -70,13 +70,17 @@ RUN git clone https://github.com/ilpincy/argos3-kheperaiv.git; \ RUN sudo apt-get update; \ sudo apt-get upgrade -y; \ sudo apt-get install -y \ + apt-utils \ python-is-python3 \ ros-noetic-octomap \ ros-noetic-octomap-msgs \ iproute2 \ ros-noetic-catkin \ python3-catkin-tools \ - libtf2-ros-dev + libtf2-ros-dev \ + ros-noetic-global-planner ; + +RUN pip install gym tensorflow ; # Add ROS dependent scripts RUN rosdep init; \ @@ -87,6 +91,11 @@ RUN rosdep init; \ echo "export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib/argos3:/opt/ros/noetic/lib" >> ~/.bashrc; \ echo "export ARGOS_PLUGIN_PATH=$HOME/catkin_ws/src/argos_bridge/ros_lib_links" >> ~/.bashrc; \ echo "export ARGOS_PLUGIN_PATH=$ARGOS_PLUGIN_PATH:$HOME/catkin_ws/devel/lib" >> ~/.bashrc + +COPY . /root/catkin_ws/src + EXPOSE 8080 +CMD ["chmod a+rwx /root -R"] + CMD ["/root/catkin_ws/src/entrypoint.sh"] diff --git a/Metrics/Run2/Distance_vs_Time_For_tb3_0.png b/Metrics/Run2/Distance_vs_Time_For_tb3_0.png new file mode 100644 index 0000000..4e985fb Binary files /dev/null and b/Metrics/Run2/Distance_vs_Time_For_tb3_0.png differ diff --git a/Metrics/Run2/Percent_Explored_vs_Distance_For_tb3_0.png b/Metrics/Run2/Percent_Explored_vs_Distance_For_tb3_0.png new file mode 100644 index 0000000..971e142 Binary files /dev/null and b/Metrics/Run2/Percent_Explored_vs_Distance_For_tb3_0.png differ diff --git a/Metrics/Run2/Percent_Explored_vs_Time_For_tb3_0.png b/Metrics/Run2/Percent_Explored_vs_Time_For_tb3_0.png new file mode 100644 index 0000000..c4c25a3 Binary files /dev/null and b/Metrics/Run2/Percent_Explored_vs_Time_For_tb3_0.png differ diff --git a/Metrics/Run2/Screenshot from 2021-12-11 17-33-07.png b/Metrics/Run2/Screenshot from 2021-12-11 17-33-07.png new file mode 100644 index 0000000..88a6e72 Binary files /dev/null and b/Metrics/Run2/Screenshot from 2021-12-11 17-33-07.png differ diff --git a/Metrics/Run3/Distance_vs_Time_For_tb3_0.png b/Metrics/Run3/Distance_vs_Time_For_tb3_0.png new file mode 100644 index 0000000..5c673fd Binary files /dev/null and b/Metrics/Run3/Distance_vs_Time_For_tb3_0.png differ diff --git a/Metrics/Run3/Percent_Explored_vs_Distance_For_tb3_0.png b/Metrics/Run3/Percent_Explored_vs_Distance_For_tb3_0.png new file mode 100644 index 0000000..a0fd71a Binary files /dev/null and b/Metrics/Run3/Percent_Explored_vs_Distance_For_tb3_0.png differ diff --git a/Metrics/Run3/Percent_Explored_vs_Time_For_tb3_0.png b/Metrics/Run3/Percent_Explored_vs_Time_For_tb3_0.png new file mode 100644 index 0000000..7faeb98 Binary files /dev/null and b/Metrics/Run3/Percent_Explored_vs_Time_For_tb3_0.png differ diff --git a/docker-compose-image.yaml b/docker-compose-image.yaml new file mode 100755 index 0000000..ab314ae --- /dev/null +++ b/docker-compose-image.yaml @@ -0,0 +1,21 @@ +version: '2' +services: + novnc: + image: tberg1234/2122_multidroneindoorslam:latest + environment: + # Adjust to your screen size + - DISPLAY_WIDTH=1366 + - DISPLAY_HEIGHT=768 + - RUN_XTERM=no + cap_add: + - NET_ADMIN + devices: + - "/dev/net/tun:/dev/net/tun" + - /dev/null:/dev/null + privileged: true + ports: + - "8080:8080" + networks: + - x11 +networks: + x11: diff --git a/frontier_rl b/frontier_rl new file mode 160000 index 0000000..9c9e0f7 --- /dev/null +++ b/frontier_rl @@ -0,0 +1 @@ +Subproject commit 9c9e0f7a958601cdacc9ca1249cb38aad6c05f7d diff --git a/multi_map_ros/rviz/multi_robot.rviz b/multi_map_ros/rviz/multi_robot.rviz index 8d67d7d..03f9d62 100644 --- a/multi_map_ros/rviz/multi_robot.rviz +++ b/multi_map_ros/rviz/multi_robot.rviz @@ -7,6 +7,9 @@ Panels: - /RobotModel1 - /TF1/Frames1 - /Map1 + - /GridCells1 + - /GridCells2 + - /Marker1 Splitter Ratio: 0.594406008720398 Tree Height: 719 - Class: rviz/Selection @@ -167,6 +170,32 @@ Visualization Manager: Unreliable: false Use Timestamp: false Value: true + - Alpha: 1 + Class: rviz/GridCells + Color: 25; 255; 0 + Enabled: true + Name: GridCells + Queue Size: 10 + Topic: /selected_frontier + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/GridCells + Color: 246; 22; 226 + Enabled: true + Name: GridCells + Queue Size: 10 + Topic: /selected_center + Unreliable: false + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /visualization_marker + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 diff --git a/multi_map_ros/src/multi_map_ros/Evaluation/Exploration/src/exploration_eval.py b/multi_map_ros/src/multi_map_ros/Evaluation/Exploration/src/exploration_eval.py new file mode 100755 index 0000000..f08c55b --- /dev/null +++ b/multi_map_ros/src/multi_map_ros/Evaluation/Exploration/src/exploration_eval.py @@ -0,0 +1,162 @@ +#!/usr/bin/env python3.8 +import math + +import rospy +import numpy as np +import copy +from scipy import signal as sig + +from std_msgs.msg import Header +from nav_msgs.msg import OccupancyGrid, GridCells, Odometry +from geometry_msgs.msg import Point, PoseStamped +from tf import TransformListener + +import matplotlib.pyplot as plt + +class exploration_eval: + def __init__(self): + rospy.init_node('exploration_eval') + self.robot_namespace = '/tb3_0' + self.mapSub = rospy.Subscriber(self.robot_namespace+'/map', OccupancyGrid, self.map_callback) + self.odomSub = rospy.Subscriber(self.robot_namespace+'/odom', Odometry, self.odom_callback) + # self.odomPub = rospy.Publisher('/distance_travelled', Odometry, queue_size=10) + self.timer = rospy.Timer(rospy.Duration(5), self.timer_callback) + + self.start_time = rospy.get_time() #rospy.get_rostime() + + self.total_distance = 0.0 + self.previous_x = 0 + self.previous_y = 0 + self.first_run = True + + self.time_record = [] + self.percent_of_map_covered_record = [] + self.total_distance_travelled_record = [] + self.percent_of_map_covered_time = [] + self.total_distance_travelled_time = [] + + self.map = OccupancyGrid() + self.odom = Odometry() + + self.files_written = False + + + def map_callback(self, msg): + + self.map = msg + + # data = np.asarray(msg.data) + # values = [0, 101] + # freq = np.histogram(data, bins=[-np.inf] + values + [np.inf])[0] / data.size + # # print(freq) + # + # # uniques, counts = np.unique(data, return_counts=True) + # # percentages = dict(zip(uniques, counts / len(data))) + # # print(percentages) + # + # # total_percent_explored = percentages.get(1) + percentages.get(0) + # + # total_percent_explored = freq[1] + # + # print("total Percent explored: " + str(total_percent_explored)) + # + # self.percent_of_map_covered_record.append(total_percent_explored) + # self.percent_of_map_covered_time.append(rospy.get_time() - self.start_time) + + + def odom_callback(self, msg): + + self.odom = msg + + # if (self.first_run): + # self.previous_x = msg.pose.pose.position.x + # self.previous_y = msg.pose.pose.position.y + # x = msg.pose.pose.position.x + # y = msg.pose.pose.position.y + # print(msg) + # d_increment = math.sqrt(((x - self.previous_x) * (x - self.previous_x)) + ((y - self.previous_y) * (y - self.previous_y))) + # self.total_distance = self.total_distance + d_increment + # print("Total distance traveled is: " + str(self.total_distance)) + # # self.odomPub.publish(data) + # self.previous_x = msg.pose.pose.position.x + # self.previous_y = msg.pose.pose.position.y + # self.first_run = False + # + # self.total_distance_travelled_record.append(self.total_distance) + # self.total_distance_travelled_time.append(rospy.get_time() - self.start_time) + + + def timer_callback(self, timer_info): + + data = np.asarray(self.map.data) + odom = self.odom + + values = [0, 101] + freq = np.histogram(data, bins=[-np.inf] + values + [np.inf])[0] / data.size + + # print(freq) + # + # uniques, counts = np.unique(data, return_counts=True) + # percentages = dict(zip(uniques, counts / len(data))) + # print(percentages) + + total_percent_explored = freq[1]*100 + + print("Total Percent explored: " + str(total_percent_explored)) + + self.percent_of_map_covered_record.append(total_percent_explored) + + if (self.first_run): + self.start_time = rospy.get_time() + self.previous_x = odom.pose.pose.position.x + self.previous_y = odom.pose.pose.position.y + x = odom.pose.pose.position.x + y = odom.pose.pose.position.y + d_increment = math.sqrt(((x - self.previous_x) * (x - self.previous_x)) + ((y - self.previous_y) * (y - self.previous_y))) + self.total_distance = self.total_distance + d_increment + print("Total distance traveled is: " + str(self.total_distance)) + self.previous_x = odom.pose.pose.position.x + self.previous_y = odom.pose.pose.position.y + self.first_run = False + + self.total_distance_travelled_record.append(self.total_distance) + + time_diff = rospy.get_time() - self.start_time + print("Time: " + str(time_diff)) + print("----------------------------------------------------------") + self.time_record.append(time_diff) + + if time_diff > 1200 and not self.files_written: + rospy.loginfo("Writing Eval Graphs to Files") + plt.plot(self.time_record,self.total_distance_travelled_record) + plt.xlabel('Time (s)') + plt.ylabel('Total Distance Travelled (m)') + + plt.savefig('Distance_vs_Time_For_%s.png' % self.robot_namespace.replace('/','')) + + plt.clf() + + plt.plot(self.time_record, self.percent_of_map_covered_record) + plt.xlabel('Time (s)') + plt.ylabel('Percent of Map Explored') + + plt.savefig('Percent_Explored_vs_Time_For_%s.png' % self.robot_namespace.replace('/','')) + + plt.clf() + + plt.plot(self.total_distance_travelled_record, self.percent_of_map_covered_record) + plt.xlabel('Total Distance Travelled (m)') + plt.ylabel('Percent of Map Explored') + + plt.savefig('Percent_Explored_vs_Distance_For_%s.png' % self.robot_namespace.replace('/','')) + + self.files_written = True + + rospy.signal_shutdown("Finished Evaluation") + + +if __name__ == '__main__': + node = exploration_eval() + + while not rospy.is_shutdown(): + pass \ No newline at end of file diff --git a/multi_map_ros/src/multi_map_ros/FrontierSelection/scripts/alg_testing.py b/multi_map_ros/src/multi_map_ros/FrontierSelection/scripts/alg_testing.py new file mode 100755 index 0000000..c2d3e8e --- /dev/null +++ b/multi_map_ros/src/multi_map_ros/FrontierSelection/scripts/alg_testing.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3.8 +import copy + +import numpy as np + +if __name__ == '__main__': + gridCells = list() + cell_threshold = 3 + gridCells.append((0, 0, 0)) + gridCells.append((1, 0, 0)) + gridCells.append((2, 0, 0)) + gridCells.append((3, 0, 0)) + gridCells.append((4, 0, 0)) + gridCells.append((5, 0, 0)) + gridCells.append((6, 0, 0)) + gridCells.append((7, 0, 0)) + gridCells.append((100, 100, 100)) + gridCells.append((30, 20, 3)) + + all_frontier_clusters = dict() # keeps track of clusters and the parent + remaining_cells = copy.deepcopy(gridCells) # cells remaining to be assigned to a group + + for a_cell in gridCells: + assigned = False + if all_frontier_clusters: + for key in all_frontier_clusters: + cells_in_cluster = all_frontier_clusters.get(key) + iterable_cells = copy.deepcopy(cells_in_cluster) + for other_cell in iterable_cells: + #if True in (abs(np.subtract(a_cell, other_cell)) <= cell_threshold): + if np.linalg.norm(np.asarray(a_cell)-np.asarray(other_cell)) <= cell_threshold: + if a_cell not in cells_in_cluster: + assigned = True + cells_in_cluster.append(a_cell) + remaining_cells.remove(a_cell) + all_frontier_clusters[key] = cells_in_cluster + + else: + assigned = True + + for potential_cell in remaining_cells: # in gridCells + # if True in (abs(np.subtract(a_cell, potential_cell)) <= cell_threshold): + if np.linalg.norm(np.asarray(a_cell) - np.asarray(potential_cell)) <= cell_threshold: + if potential_cell not in cells_in_cluster: + cells_in_cluster.append(potential_cell) + remaining_cells.remove(potential_cell) + all_frontier_clusters[key] = cells_in_cluster + + if not assigned: + first_list = list() + first_list.append(a_cell) + remaining_cells.remove(a_cell) + all_frontier_clusters[a_cell] = first_list + + + else: + first_list = list() + first_list.append(a_cell) + remaining_cells.remove(a_cell) + all_frontier_clusters[a_cell] = first_list + + print(str(all_frontier_clusters)) + + all_values = all_frontier_clusters.values() + + max_value = max(all_values) + print(max_value) + + max_key, max_value = max(all_frontier_clusters.items(), key=lambda x: len(set(x[1]))) + print(max_key) + print(max_value) \ No newline at end of file diff --git a/multi_map_ros/src/multi_map_ros/FrontierSelection/scripts/cluster_sorting_testing.py b/multi_map_ros/src/multi_map_ros/FrontierSelection/scripts/cluster_sorting_testing.py new file mode 100644 index 0000000..f6cec2a --- /dev/null +++ b/multi_map_ros/src/multi_map_ros/FrontierSelection/scripts/cluster_sorting_testing.py @@ -0,0 +1,30 @@ + +if __name__ == '__main__': + + gridCells1 = list() + gridCells2 = list() + gridCells3 = list() + gridCells4 = list() + + + gridCells1.append((0, 0, 0)) + gridCells1.append((1, 0, 0)) + gridCells2.append((2, 0, 0)) + gridCells2.append((3, 0, 0)) + gridCells3.append((4, 0, 0)) + gridCells3.append((5, 0, 0)) + gridCells3.append((6, 0, 0)) + gridCells3.append((7, 0, 0)) + gridCells4.append((100, 100, 100)) + gridCells4.append((30, 20, 3)) + + all_frontier_clusters = dict() + all_frontier_clusters["one"] = gridCells1 + all_frontier_clusters["two"] = gridCells2 + all_frontier_clusters["three"] = gridCells3 + all_frontier_clusters["four"] = gridCells4 + + + length_dict = {key: len(value) for key, value in all_frontier_clusters.items()} + new_list = sorted(length_dict, key=length_dict.get, reverse=True) + print(new_list) \ No newline at end of file diff --git a/multi_map_ros/src/multi_map_ros/FrontierSelection/src/frontier_clustering.py b/multi_map_ros/src/multi_map_ros/FrontierSelection/src/frontier_clustering.py new file mode 100755 index 0000000..d4cd523 --- /dev/null +++ b/multi_map_ros/src/multi_map_ros/FrontierSelection/src/frontier_clustering.py @@ -0,0 +1,359 @@ +#!/usr/bin/env python3.8 + +import rospy +import numpy as np +import copy +from scipy import signal as sig + +from std_msgs.msg import Header, String +from nav_msgs.msg import OccupancyGrid, GridCells +from geometry_msgs.msg import Point, PoseStamped, Pose, PoseArray +from tf import TransformListener +from rospy.exceptions import ROSTimeMovedBackwardsException, ROSInterruptException + +SEND_TO_FRONTIERS = False +SEND_STATE_MACHINE_FRONTIERS = True +SM_LIMIT = 10 + +class find_frontier: + def __init__(self): + rospy.init_node('find_fontier') + + self.cell_threshold = 0 + self.threshold_multiplier = 2 + + # self.tf_listener_ = TransformListener() + self.weight_output = .75 + self.clusters = list() + + self.max_msg_count_before_start = 10 + self.curr_msg_count = 0 + + self.robot_namespace = '/tb3_0' + + self.publish_next_frontier = False + self.publish_ns = "" + + # self.timer = rospy.Timer(rospy.Duration(5), self.frontier_callback) + + self.init_subscribers_and_publishers(self.robot_namespace) + + rospy.loginfo("FINISHED INIT") + + + def init_subscribers_and_publishers(self, ns): + + self.mapSub = rospy.Subscriber(ns + '/map', OccupancyGrid, self.map_callback) + self.fringe = rospy.Publisher('/map_fringe', GridCells, queue_size=10) + self.final_frontier_pub = rospy.Publisher('/selected_frontier', GridCells, queue_size=10) + self.center_fringe_pub = rospy.Publisher('/selected_center', GridCells, queue_size=10) + self.move_pub = rospy.Publisher(ns + '/move_base_simple/goal', PoseStamped, queue_size=10) + self.move_helper_sub = rospy.Subscriber('/move_base_simple/goal', PoseStamped, self.helper) + self.move_helper_pub = rospy.Publisher(ns + '/move_base_simple/goal', PoseStamped, + queue_size=10) + self.frontier_center_pub = rospy.Publisher(ns + '/frontier_list', PoseArray, queue_size=10) + self.frontier_center_request = rospy.Subscriber('/frontier_request', String, self.request_callback) + + + def request_callback(self, msg): + ns = str(msg.data).replace("\"","") + + if self.robot_namespace != ns: + self.robot_namespace = ns + self.init_subscribers_and_publishers(ns) + + self.publish_ns = ns + self.publish_next_frontier = True + + + def helper(self, msg): + self.move_helper_pub.publish(msg) + + def map_callback(self, msg): + self.map = msg + # if self.curr_msg_count > self.max_msg_count_before_start: + # self.frontier_callback(0) + # self.curr_msg_count = 0 + # self.curr_msg_count += 1 + # rospy.loginfo("curr message count is: " + str(self.curr_msg_count)) + + if SEND_STATE_MACHINE_FRONTIERS: + if self.publish_next_frontier and self.publish_ns in str(msg.header.frame_id): + rospy.loginfo("Publishing frontiers for: " + str(self.publish_ns)) + self.frontier_callback(0) + self.publish_next_frontier = False + else: + self.frontier_callback(0) + + def frontier_callback(self, timer_var): + msg = self.map + fringe_points = self.find_fringe(msg) + if SEND_TO_FRONTIERS: + all_frontier_clusters = self.cluster_cells(fringe_points) + best_frontier_cells = self.calculate_least_cost_frontier(all_frontier_clusters) + self.send_to_frontier(best_frontier_cells, msg) + elif SEND_STATE_MACHINE_FRONTIERS: + all_frontier_clusters = self.cluster_cells(fringe_points) + centers = self.get_centers(all_frontier_clusters) + self.send_centers_to_pub(centers) + + + def find_fringe(self, msg): + + res = msg.info.resolution + width = msg.info.width + height = msg.info.height + data = np.asarray(msg.data).reshape((width,height), order='F') + expl = (data > -1) + + # Takes in a list of (x,y) locations to paint as explored nodes + gridCells = GridCells() + gridCells.cell_width = res + gridCells.cell_height = res + + self.cell_threshold = self.threshold_multiplier*res + + header = Header() + header.frame_id = msg.header.frame_id + gridCells.header = header + + filter = np.asarray([-1,0,1,-2,0,2,-1,0,1]).reshape((3,3)) + edgeX = sig.convolve2d(expl, filter, mode='same', fillvalue=-1) + edgeY = sig.convolve2d(expl, filter.T, mode='same', fillvalue=-1) + edge = np.sqrt(np.add(np.power(edgeX, 2), np.power(edgeY, 2))) + open_cells = np.logical_and((data > -1), (data < 10)) + fringe = np.logical_and((edge >= 2), open_cells) + fringe_list = np.transpose(np.where(fringe)) + + pts = [] + raw_pts = [] + for node in fringe_list: + tup = self.map_to_world(node[0], node[1], msg) + pt = Point(tup[0], tup[1], 0) + raw_pt = (tup[0], tup[1], 0) + pts.append(pt) + raw_pts.append(raw_pt) + + gridCells.cells = pts + self.fringe.publish(gridCells) + return raw_pts + + def map_to_world(self, x, y, my_map): + """ + converts a point from the map to the world + :param x: float of x position + :param y: float of y position + :return: tuple of converted point + @Author tberg1234 (from rbe3002) + """ + resolution = my_map.info.resolution + + originX = my_map.info.origin.position.x + originY = my_map.info.origin.position.y + + # multiply by resolution, then move by origin offset + x = x * resolution + originX + resolution / 2 + y = y * resolution + originY + resolution / 2 + + return (x, y) + + + def cluster_cells(self, gridCells): + + all_frontier_clusters = dict() # keeps track of clusters and the parent + remaining_cells = copy.deepcopy(gridCells) # cells remaining to be assigned to a group + + for a_cell in gridCells: # in remaining_cells worked kinda but it was faster sooo + if remaining_cells: + assigned = False + if all_frontier_clusters: + for key in all_frontier_clusters: + cells_in_cluster = all_frontier_clusters.get(key) + iterable_cells = copy.deepcopy(cells_in_cluster) + for other_cell in iterable_cells: + if np.linalg.norm(np.asarray(a_cell) - np.asarray(other_cell)) <= self.cell_threshold: + if a_cell not in cells_in_cluster: + assigned = True + cells_in_cluster.append(a_cell) + if a_cell in remaining_cells: + remaining_cells.remove(a_cell) + # all_frontier_clusters[key] = cells_in_cluster + else: + assigned = True + + for potential_cell in remaining_cells: # in gridCells + if np.linalg.norm(np.asarray(a_cell) - np.asarray(potential_cell)) <= self.cell_threshold: + if potential_cell not in cells_in_cluster: + cells_in_cluster.append(potential_cell) + if potential_cell in remaining_cells: + remaining_cells.remove(potential_cell) + # all_frontier_clusters[key] = cells_in_cluster + + all_frontier_clusters[key] = cells_in_cluster + + if not assigned: + first_list = list() + first_list.append(a_cell) + remaining_cells.remove(a_cell) + all_frontier_clusters[a_cell] = first_list + + + else: + first_list = list() + first_list.append(a_cell) + remaining_cells.remove(a_cell) + all_frontier_clusters[a_cell] = first_list + + else: + pass + + return all_frontier_clusters + + + def calculate_least_cost_frontier(self, all_frontier_clusters): + # max_key, max_value = max(all_frontier_clusters.items(), key=lambda x: len(set(x[1]))) + # return max_key, max_value + + cost_dict = dict() + length_dict = {key: len(value) for key, value in all_frontier_clusters.items()} + robot_position = self.get_current_position_in_map() + + for key in all_frontier_clusters.keys(): + cells = all_frontier_clusters.get(key) + size = length_dict.get(key) + center = np.average(cells, axis=0) + distance = np.linalg.norm(np.asarray(center) - np.asarray(robot_position)) + + cost = self.weight_output*distance + (1-self.weight_output)*(1-size) + cost_dict[key] = cost + + sorted_costs = sorted(cost_dict, key=cost_dict.get, reverse=False) + return all_frontier_clusters.get(sorted_costs[0]) + + + def get_centers(self, all_frontier_clusters): + + cost_dict = dict() + length_dict = {key: len(value) for key, value in all_frontier_clusters.items()} + robot_position = self.get_current_position_in_map() + + for key in all_frontier_clusters.keys(): + cells = all_frontier_clusters.get(key) + size = length_dict.get(key) + center = np.average(cells, axis=0) + distance = np.linalg.norm(np.asarray(center) - np.asarray(robot_position)) + + cost = self.weight_output * distance + (1 - self.weight_output) * (1 - size) + cost_dict[key] = cost + + sorted_costs = sorted(cost_dict, key=cost_dict.get, reverse=False) + + centers = list() + for cost in sorted_costs: + fr = all_frontier_clusters.get(cost) + c = np.average(fr, axis=0) + centers.append(c) + + return centers + + + def send_centers_to_pub(self, centers): + + centers_for_pub = [] + counter = 0 + + for c in centers: + + if counter > SM_LIMIT: + break + + move_msg = Pose() + move_msg.position.x = c[0] + move_msg.position.y = c[1] + move_msg.position.z = c[2] + + move_msg.orientation.x = 0 + move_msg.orientation.y = 0 + move_msg.orientation.z = 0 + move_msg.orientation.w = 1 + + centers_for_pub.append(move_msg) + counter += 1 + + full_msg = PoseArray() + full_msg.poses = centers_for_pub + full_msg.header.frame_id = self.robot_namespace + '/map' + + self.frontier_center_pub.publish(full_msg) + + rospy.loginfo("sent frontier list") + + + def send_to_frontier(self, best_frontier_cells, msg): + + final_frontier = list() + for cell in best_frontier_cells: + final_frontier.append(Point(cell[0], cell[1], cell[2])) + + res = msg.info.resolution + + gridCells = GridCells() + gridCells.cell_width = res + gridCells.cell_height = res + + header = Header() + header.frame_id = msg.header.frame_id + gridCells.header = header + + gridCells.cells = final_frontier + + self.final_frontier_pub.publish(gridCells) + + center = np.average(best_frontier_cells, axis=0) + center_lst = list() + center_lst.append(Point(center[0], center[1], center[2])) + + gridCells.cells = center_lst + + self.center_fringe_pub.publish(gridCells) + + move_msg = PoseStamped() + move_msg.header.frame_id = 'map' + + move_msg.pose.position.x = center[0] + move_msg.pose.position.y = center[1] + move_msg.pose.position.z = center[2] + + rospy.loginfo("SENDING TO FRONTIER: " + str(center)) + + move_msg.pose.orientation.x = 0 + move_msg.pose.orientation.y = 0 + move_msg.pose.orientation.z = 0 + move_msg.pose.orientation.w = 1 + + self.move_pub.publish(move_msg) + + + def get_current_position_in_map(self): + # Position of the robot base in the map + + p1 = PoseStamped() + p1.header.frame_id = self.robot_namespace+"/base_link" + p1.pose.orientation.w = 1.0 # Neutral orientation + # try: + # tf_listener_ = TransformListener() + # p_in_base = tf_listener_.transformPose(self.robot_namespace + "/map", p1) + # except ROSTimeMovedBackwardsException: + # p_in_base = PoseStamped() + # print("ROS Interrupt Exception! Just ignore the exception!") + p_in_base = PoseStamped() + # p_in_base = self.tf_listener_.transformPose(self.robot_namespace+"/map", p1) + return (p_in_base.pose.position.x, p_in_base.pose.position.y, p_in_base.pose.position.z) + + + +if __name__ == '__main__': + node = find_frontier() + + while not rospy.is_shutdown(): + pass \ No newline at end of file diff --git a/multi_map_ros/src/multi_map_ros/FrontierSelection/src/move_base_pub.py b/multi_map_ros/src/multi_map_ros/FrontierSelection/src/move_base_pub.py new file mode 100755 index 0000000..cd86908 --- /dev/null +++ b/multi_map_ros/src/multi_map_ros/FrontierSelection/src/move_base_pub.py @@ -0,0 +1,28 @@ +#!/usr/bin/env python3.8 + +import rospy +import numpy as np +import copy +from scipy import signal as sig + +from std_msgs.msg import Header +from nav_msgs.msg import OccupancyGrid, GridCells +from geometry_msgs.msg import Point, PoseStamped +from tf import TransformListener + +class move_base_msg_forwarder: + def __init__(self): + rospy.init_node('move_base_msg_forwarder') + self.robot_namespace = '/tb3_0' + self.move_helper_sub = rospy.Subscriber('/move_base_simple/goal', PoseStamped, self.helper) + self.move_helper_pub = rospy.Publisher(self.robot_namespace+'/move_base_simple/goal', PoseStamped, queue_size=10) + + def helper(self, msg): + self.move_helper_pub.publish(msg) + + +if __name__ == '__main__': + node = move_base_msg_forwarder() + + while not rospy.is_shutdown(): + pass \ No newline at end of file diff --git a/multi_map_ros/src/multi_map_ros/FrontierSelection/src/move_base_reset.py b/multi_map_ros/src/multi_map_ros/FrontierSelection/src/move_base_reset.py new file mode 100755 index 0000000..acc4fee --- /dev/null +++ b/multi_map_ros/src/multi_map_ros/FrontierSelection/src/move_base_reset.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python3.8 + +import rospy +import roslaunch +import numpy as np +import copy +from scipy import signal as sig +import subprocess +import signal + +from std_msgs.msg import Header, String +from nav_msgs.msg import OccupancyGrid, GridCells +from geometry_msgs.msg import Point, PoseStamped +from tf import TransformListener + +class move_base_reset: + + def __init__(self): + rospy.init_node('mapping_reset_node') + self.robot_namespace = '/tb3_0' + self.stop_signal = rospy.Subscriber('/stop_move_base', String, self.stop) + self.start_signal = rospy.Subscriber('/start_move_base', String, self.start) + + + def stop(self, msg): + # self.launch.shutdown() + print("STOPPING") + self.child.send_signal(signal.SIGINT) + + + def start(self, msg): + # uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) + # roslaunch.configure_logging(uuid) + # self.launch = roslaunch.parent.ROSLaunchParent(uuid, [ + # "/home/taylor/multidrone_slam/src/2122_MultiDroneIndoorSLAM/turtlebot3_simulations/turtlebot3_gazebo/launch/multi_move_base.launch"]) + # self.launch.start() + # rospy.loginfo("move base started") + + print("STARTING") + self.child = subprocess.Popen(["roslaunch", "turtlebot3_gazebo", "multi_move_base.launch"]) + # child.wait() #You can use this line to block the parent process until the child process finished. + # print("parent process") + # print(self.child.poll()) + + rospy.loginfo('The PID of child: %d', self.child.pid) + # print("The PID of child:", self.child.pid) + + +if __name__ == '__main__': + node = move_base_reset() + + while not rospy.is_shutdown(): + pass \ No newline at end of file diff --git a/multi_map_ros/src/multi_map_ros/FrontierSelection/src/publish_inputs_for_learning.py b/multi_map_ros/src/multi_map_ros/FrontierSelection/src/publish_inputs_for_learning.py new file mode 100755 index 0000000..4690226 --- /dev/null +++ b/multi_map_ros/src/multi_map_ros/FrontierSelection/src/publish_inputs_for_learning.py @@ -0,0 +1,200 @@ +#!/usr/bin/env python3.8 + +import rospy +import numpy as np +import copy + +from matplotlib import pyplot, colors +from scipy import signal as sig + +from std_msgs.msg import Header +from nav_msgs.msg import OccupancyGrid, GridCells, MapMetaData, Odometry +from geometry_msgs.msg import Point, PoseStamped +from tf import TransformListener + +class publish_inputs_for_learning: + + def __init__(self): + rospy.init_node('publish_inputs_rl') + self.robot_namespace = '/tb3_0' + # self.desired_dimension_x = 640 + # self.desired_dimension_y = 640 + # self.desired_dimension_x = 288 + # self.desired_dimension_y = 160 + self.desired_dimension_x = 352 + self.desired_dimension_y = 224 + self.map = OccupancyGrid() + self.map_cells = np.array([]) + self.fringe = GridCells() + self.pub_msg = OccupancyGrid() + self.odom = Odometry() + self.resized_map_set = False + self.pose_map_sent = False + self.fringe_map_sent = False + self.full_map_sent = False + + #below commented because size of origional map + self.mapSub = rospy.Subscriber(self.robot_namespace+'/map', OccupancyGrid, self.map_callback) + self.mapPub = rospy.Publisher(self.robot_namespace+'/rl_map', OccupancyGrid, queue_size=10) + self.posePub = rospy.Publisher(self.robot_namespace+'/rl_pose', OccupancyGrid, queue_size=10) + self.frontierPub = rospy.Publisher(self.robot_namespace+'/rl_frontiers', OccupancyGrid, queue_size=10) + self.fringeSub = rospy.Subscriber('/map_fringe', GridCells, self.fringe_callback) + self.poseSub = rospy.Subscriber(self.robot_namespace+'/odom', Odometry, self.odom_callback) + + self.last_map = OccupancyGrid() + self.last_fringe = GridCells() + self.last_odom = Odometry() + + + def map_callback(self, msg): + self.map = msg + map_height = self.map.info.height + map_width = self.map.info.width + self.map_cells = np.array(self.map.data) + resized_map = np.reshape(self.map_cells, (map_height, map_width)) + resized_map = np.pad(resized_map, [(0, self.desired_dimension_y-map_height), (0, self.desired_dimension_x-map_width)], mode='constant', constant_values=-1) + final_resized_map = resized_map.reshape(-1) + + self.pub_msg = OccupancyGrid() + pub_msg_info = MapMetaData() + pub_msg_header = Header() + pub_msg_info.height = self.desired_dimension_y + pub_msg_info.width = self.desired_dimension_x + pub_msg_info.origin = self.map.info.origin + pub_msg_info.resolution = self.map.info.resolution + self.pub_msg.info = pub_msg_info + pub_msg_header.frame_id = self.map.header.frame_id + self.pub_msg.header = pub_msg_header + self.pub_msg.data = final_resized_map + + self.resized_map_set = True + + self.mapPub.publish(self.pub_msg) + + # if not self.full_map_sent: + # rospy.loginfo("FULL MAP SENT") + # self.full_map_sent = True + + self.last_map = msg + + + + def fringe_callback(self, msg): + self.fringe = msg + + if self.resized_map_set: + cells = msg.cells + + fringe_map = OccupancyGrid() + fringe_arr = np.full((self.desired_dimension_y, self.desired_dimension_x), -1) + fringe_resized_map = fringe_arr.reshape(-1) + index_arr = [] + for c in cells: + index_arr.append(int(self.point_to_index((c.x, c.y), self.pub_msg))) + + for i in index_arr: + fringe_resized_map[i] = 0 + + int_fringe_resized_map = fringe_resized_map.astype(np.int8) + + fringe_map_info = MapMetaData() + fringe_map_header = Header() + fringe_map_info.height = self.desired_dimension_y + fringe_map_info.width = self.desired_dimension_x + fringe_map_info.origin = self.map.info.origin + fringe_map_info.resolution = self.map.info.resolution + fringe_map.info = fringe_map_info + fringe_map_header.frame_id = self.map.header.frame_id + fringe_map.header = fringe_map_header + fringe_map.data = int_fringe_resized_map + self.frontierPub.publish(fringe_map) + + # if not self.fringe_map_sent: + # rospy.loginfo("FRINGE MAP SENT") + # self.fringe_map_sent = True + + self.last_fringe = msg + + + def odom_callback(self, msg): + self.odom = msg + + if self.resized_map_set: + odom_map = OccupancyGrid() + odom_arr = np.full((self.desired_dimension_y, self.desired_dimension_x), -1) + odom_resized_map = odom_arr.reshape(-1) + + index_of_robot = int(self.point_to_index((msg.pose.pose.position.x, msg.pose.pose.position.y), self.pub_msg)) + + odom_resized_map[index_of_robot] = 0 + + int_odom_resized_map = odom_resized_map.astype(np.int8) + + odom_map_info = MapMetaData() + odom_map_header = Header() + odom_map_info.height = self.desired_dimension_y + odom_map_info.width = self.desired_dimension_x + odom_map_info.origin = self.map.info.origin + odom_map_info.resolution = self.map.info.resolution + odom_map.info = odom_map_info + odom_map_header.frame_id = self.map.header.frame_id + odom_map.header = odom_map_header + odom_map.data = int_odom_resized_map + self.posePub.publish(odom_map) + + # if not self.pose_map_sent: + # rospy.loginfo("POSE MAP SENT") + # self.pose_map_sent = True + + self.last_odom = msg + + + def world_to_map(self, x, y, my_map): + """ + converts a point from the world to the map + :param x: float of x position + :param y: float of y position + :return: tuple of converted point + """ + + return self.convert_location((x, y), my_map) + + + def convert_location(self, loc, my_map): + """converts points to the grid""" + res = my_map.info.resolution + Xorigin = my_map.info.origin.position.x + Yorigin = my_map.info.origin.position.y + + # print("loc: " + str(loc)) + # print("Xorigin: " + str(Xorigin)) + # print("res: "+ str(res)) + + # offset from origin and then divide by resolution + Xcell = int((loc[0] - Xorigin - (res / 2)) / res) + Ycell = int((loc[1] - Yorigin - (res / 2)) / res) + return (Xcell, Ycell) + + + def point_to_index(self, point, my_map): + """convert a index to a point""" + pt = self.world_to_map(point[0], point[1], my_map) + return pt[1] * my_map.info.width + pt[0] + + + def world_to_map(self, x, y, my_map): + """ + converts a point from the world to the map + :param x: float of x position + :param y: float of y position + :return: tuple of converted point + """ + + return self.convert_location((x,y), my_map) + + +if __name__ == '__main__': + node = publish_inputs_for_learning() + + while not rospy.is_shutdown(): + pass \ No newline at end of file diff --git a/multi_map_ros/src/multi_map_ros/FrontierSelection/src/resize_map.py b/multi_map_ros/src/multi_map_ros/FrontierSelection/src/resize_map.py new file mode 100755 index 0000000..6b124c1 --- /dev/null +++ b/multi_map_ros/src/multi_map_ros/FrontierSelection/src/resize_map.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python3.8 + +import rospy +import numpy as np +import copy + +from matplotlib import pyplot, colors +from scipy import signal as sig + +from std_msgs.msg import Header +from nav_msgs.msg import OccupancyGrid, GridCells, MapMetaData +from geometry_msgs.msg import Point, PoseStamped +from tf import TransformListener + +class resize_map: + + def __init__(self): + rospy.init_node('resize_map') + self.robot_namespace = '/tb3_0' + self.mapSub = rospy.Subscriber(self.robot_namespace+'/map', OccupancyGrid, self.map_callback) + self.mapPub = rospy.Publisher(self.robot_namespace+'/resized_map', OccupancyGrid, queue_size=10) + self.map = OccupancyGrid() + self.map_cells = np.array([]) + self.desired_dimension = 640 + + def map_callback(self, msg): + self.map = msg + map_height = self.map.info.height + map_width = self.map.info.width + self.map_cells = np.array(self.map.data) + resized_map = np.reshape(self.map_cells, (map_height, map_width)) + resized_map = np.pad(resized_map, [(0, self.desired_dimension-map_height), (0, self.desired_dimension-map_width)], mode='constant') + final_resized_map = resized_map.reshape(-1) + + pub_msg = OccupancyGrid() + pub_msg_info = MapMetaData() + pub_msg_header = Header() + pub_msg_info.height = self.desired_dimension + pub_msg_info.width = self.desired_dimension + pub_msg_info.origin = self.map.info.origin + pub_msg_info.resolution = self.map.info.resolution + pub_msg.info = pub_msg_info + pub_msg_header.frame_id = self.map.header.frame_id + pub_msg.header = pub_msg_header + pub_msg.data = final_resized_map + self.mapPub.publish(pub_msg) + + +if __name__ == '__main__': + node = resize_map() + + while not rospy.is_shutdown(): + pass \ No newline at end of file diff --git a/slam_gmapping b/slam_gmapping new file mode 160000 index 0000000..e6aa316 --- /dev/null +++ b/slam_gmapping @@ -0,0 +1 @@ +Subproject commit e6aa31616ec1e703ffe29dd200f6171a233301c7 diff --git a/turtlebot3_simulations b/turtlebot3_simulations index 90bccd0..7f2b3a0 160000 --- a/turtlebot3_simulations +++ b/turtlebot3_simulations @@ -1 +1 @@ -Subproject commit 90bccd06786507832a41cd74326a96afce6e90c8 +Subproject commit 7f2b3a0e48a84a94bf2ab4fa7c6bf79c778835eb