Frontier search for testing master merge v2

.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

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"]

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:

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

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

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)

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)