diff --git a/.gitignore b/.gitignore index 6f47218..84832d6 100644 --- a/.gitignore +++ b/.gitignore @@ -58,8 +58,11 @@ CATKIN_IGNORE # VSCode .vscode +# Pycharm +.idea/ + # macOS cache .DS_Store # Coveralls -.coverage \ No newline at end of file +.coverage diff --git a/.travis.yml b/.travis.yml index 5187399..9456321 100644 --- a/.travis.yml +++ b/.travis.yml @@ -4,8 +4,8 @@ services: - docker before_install: - - docker-compose build - - docker-compose up -d + - docker-compose build --no-cache + - docker-compose up - docker exec -it "$(docker ps -aqf 'name=coms*')" make -f /root/catkin_ws/src/Makefile check-health script: diff --git a/Dockerfile b/Dockerfile index aaa5300..bf81ea1 100644 --- a/Dockerfile +++ b/Dockerfile @@ -64,7 +64,7 @@ RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" # make install; # Install extra dependencies -RUN pip3 install pyquaternion; \ +RUN pip3 install pyquaternion scikit-image; \ sudo apt-get install -y iproute2 ros-noetic-catkin python3-catkin-tools; \ rosdep init; \ rosdep update; \ diff --git a/coms/CMakeLists.txt b/coms/CMakeLists.txt index 730166a..bc38f1d 100644 --- a/coms/CMakeLists.txt +++ b/coms/CMakeLists.txt @@ -10,6 +10,8 @@ project(coms) find_package(catkin REQUIRED COMPONENTS rospy std_msgs + nav_msgs + message_generation ) ## System dependencies are found with CMake's conventions @@ -53,11 +55,11 @@ catkin_python_setup() # ) ## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) +add_service_files( + FILES + MergeMap.srv + TriggerMerge.srv +) ## Generate actions in the 'action' folder # add_action_files( @@ -67,10 +69,10 @@ catkin_python_setup() # ) ## Generate added messages and services with any dependencies listed here -# generate_messages(DEPENDENCIES - # std_msgs # Or other packages containing msgs -# std_msgs -#) +generate_messages(DEPENDENCIES + std_msgs + nav_msgs +) ################################################ ## Declare ROS dynamic reconfigure parameters ## @@ -106,6 +108,8 @@ catkin_package( # LIBRARIES my_robot_common CATKIN_DEPENDS rospy std_msgs + nav_msgs + message_runtime # DEPENDS system_lib ) diff --git a/coms/launch/test_maphandler.launch b/coms/launch/test_maphandler.launch new file mode 100644 index 0000000..b653306 --- /dev/null +++ b/coms/launch/test_maphandler.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/coms/launch/test_mapmerge.launch b/coms/launch/test_mapmerge.launch new file mode 100644 index 0000000..6995607 --- /dev/null +++ b/coms/launch/test_mapmerge.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/coms/package.xml b/coms/package.xml index 498759c..4155030 100644 --- a/coms/package.xml +++ b/coms/package.xml @@ -10,13 +10,17 @@ MIT + message_generation + message_runtime catkin rospy std_msgs + nav_msgs rospy rospy python3-numpy std_msgs + nav_msgs diff --git a/coms/src/coms/sim.py b/coms/src/coms/sim.py index e5a28cb..a24072d 100644 --- a/coms/src/coms/sim.py +++ b/coms/src/coms/sim.py @@ -3,11 +3,11 @@ import socket import roslaunch import rospy -from msg.ping import Ping from threading import Lock from typing import List, Tuple from subprocess import check_output, call from std_msgs.msg import String +from msg.ping import Ping from coms.constants import QUICK_WAIT_TIMER, RESPONSE_TIMEOUT, BROADCAST_INTERVAL, ENCODING, CATKIN_WS, NET_CONFIG, SUB_TOPIC, PUB_TOPIC # noqa: E501 from coms.utils import get_interface_from_ip, get_port_from, get_ip_list, get_device_numbers from coms.server import server, send_messsage diff --git a/coms/src/mapmerge/__init__.py b/coms/src/mapmerge/__init__.py new file mode 100644 index 0000000..13c095c --- /dev/null +++ b/coms/src/mapmerge/__init__.py @@ -0,0 +1,5 @@ +from mapmerge.constants import * +from mapmerge.keypoint_merge import * +from mapmerge.merge_utils import * +from mapmerge.ros_utils import * +from mapmerge.hough_merge import * diff --git a/coms/src/mapmerge/hough_merge.py b/coms/src/mapmerge/hough_merge.py index 1e6b823..6cd2c2f 100644 --- a/coms/src/mapmerge/hough_merge.py +++ b/coms/src/mapmerge/hough_merge.py @@ -3,9 +3,9 @@ import matplotlib.pyplot as plt import scipy import cv2 -from merge_utils import apply_warp, acceptance_index +from mapmerge.merge_utils import apply_warp, acceptance_index -from constants import * +from mapmerge.constants import * def hough_spectrum_calculation(image): h, theta, d = hough_map_transform(image) diff --git a/coms/src/mapmerge/keypoint_merge.py b/coms/src/mapmerge/keypoint_merge.py index 1b516c1..d44e54f 100644 --- a/coms/src/mapmerge/keypoint_merge.py +++ b/coms/src/mapmerge/keypoint_merge.py @@ -1,24 +1,26 @@ import numpy as np import cv2 -from constants import * +from mapmerge.constants import * + +from mapmerge.merge_utils import apply_warp -from merge_utils import apply_warp def blur_map(map): src = np.copy(map) - blur = cv2.GaussianBlur(src,(3,3), sigmaX=1, sigmaY=1) + blur = cv2.GaussianBlur(src, (3, 3), sigmaX=1, sigmaY=1) return blur + def sift_mapmerge(map1, map2): map1, map2 = blur_map(map1), blur_map(map2) sift = cv2.SIFT_create() kp1, desc1 = sift.detectAndCompute(map1, None) kp2, desc2 = sift.detectAndCompute(map2, None) - index_params = dict(algorithm = 0, trees = 5) + index_params = dict(algorithm=0, trees=5) search_params = dict(checks=50) try: - flann = cv2.FlannBasedMatcher(index_params,search_params) - matches = flann.knnMatch(desc1,desc2,k=2) + flann = cv2.FlannBasedMatcher(index_params, search_params) + matches = flann.knnMatch(desc1, desc2, k=2) good_matches = [] for m, n in matches: # lowes ratio @@ -27,21 +29,22 @@ def sift_mapmerge(map1, map2): except: # failed merge return np.ones_like(map2) * UNKNOWN - src_pts = np.float32([ kp1[m.queryIdx].pt for m in good_matches ]).reshape(-1,1,2) - dst_pts = np.float32([ kp2[m.trainIdx].pt for m in good_matches ]).reshape(-1,1,2) + src_pts = np.float32([kp1[m.queryIdx].pt for m in good_matches]).reshape(-1, 1, 2) + dst_pts = np.float32([kp2[m.trainIdx].pt for m in good_matches]).reshape(-1, 1, 2) M, mask = cv2.findHomography(dst_pts, src_pts, cv2.RANSAC, 5.0) return apply_warp(map2, M[:2]) + def orb_mapmerge(map1, map2): - map1, map2 = blur_map(map1), blur_map(map2) + # map1, map2 = blur_map(map1), blur_map(map2) orb = cv2.ORB_create(nfeatures=1000) kp1, desc1 = orb.detectAndCompute(map1, None) kp2, desc2 = orb.detectAndCompute(map2, None) - index_params = dict(algorithm = 6, trees = 6, key_size=12, multi_probe_level = 1) + index_params = dict(algorithm=6, trees=6, key_size=12, multi_probe_level=1) search_params = dict(checks=50) - flann = cv2.FlannBasedMatcher(index_params,search_params) - matches = flann.knnMatch(desc1,desc2,k=2) + flann = cv2.FlannBasedMatcher(index_params, search_params) + matches = flann.knnMatch(desc1, desc2, k=2) good_matches = [] try: for m, n in matches: @@ -51,8 +54,8 @@ def orb_mapmerge(map1, map2): except: # failed merge return np.ones_like(map2) * UNKNOWN - src_pts = np.float32([ kp1[m.queryIdx].pt for m in good_matches ]).reshape(-1,1,2) - dst_pts = np.float32([ kp2[m.trainIdx].pt for m in good_matches ]).reshape(-1,1,2) + src_pts = np.float32([kp1[m.queryIdx].pt for m in good_matches]).reshape(-1, 1, 2) + dst_pts = np.float32([kp2[m.trainIdx].pt for m in good_matches]).reshape(-1, 1, 2) M, mask = cv2.findHomography(dst_pts, src_pts, cv2.RANSAC, 5.0) - return apply_warp(map2, M[:2]) \ No newline at end of file + return apply_warp(map2, M[:2]) diff --git a/coms/src/mapmerge/merge_utils.py b/coms/src/mapmerge/merge_utils.py index b95a921..bb33962 100644 --- a/coms/src/mapmerge/merge_utils.py +++ b/coms/src/mapmerge/merge_utils.py @@ -1,7 +1,7 @@ import numpy as np import matplotlib.pyplot as plt import cv2 -from constants import * +from mapmerge.constants import * def load_mercer_map(txt_path, dtype=np.uint8): """ diff --git a/coms/src/mapmerge/ros_utils.py b/coms/src/mapmerge/ros_utils.py index 63298f6..82938ea 100644 --- a/coms/src/mapmerge/ros_utils.py +++ b/coms/src/mapmerge/ros_utils.py @@ -1,7 +1,24 @@ import numpy as np import cv2 -from constants import * +from mapmerge.constants import * + +def numpy_to_ros(array): + """ + returns a numpy array from a ros int8 array + """ + array[array == FREE] = 100 + array[array == UNKNOWN] = -1 + return np.asarray(array, dtype=np.int8) + +def ros_to_numpy(array): + """ + returns a int8 array from numpy + """ + array[array == -1] = UNKNOWN + array = np.asarray(array, dtype=np.uint8) + array[array == 100] = FREE + return array def pgm_to_numpy(filename): """ @@ -16,8 +33,8 @@ def pgm_to_numpy(filename): img = cv2.imread(filename, -1) if img is None: print("INVALID FILENAME", filename) - img = np.asarray(img) - img[img > 250] = FREE # temp + + img[img >= 250] = FREE # temp img[(img > 0) & (img < 250)] = UNKNOWN return img diff --git a/coms/src/nodes/gmapping_merger.py b/coms/src/nodes/gmapping_merger.py new file mode 100755 index 0000000..df20e98 --- /dev/null +++ b/coms/src/nodes/gmapping_merger.py @@ -0,0 +1,78 @@ +#!/usr/bin/env python3 +from merger import ROSMerger +from std_msgs.msg import Header +from nav_msgs.srv import GetMap, GetMapResponse +from nav_msgs.msg import OccupancyGrid, MapMetaData +from coms.srv import MergeMap +import numpy as np +import rospy + + +class GmappingMerger(ROSMerger): + + def __init__(self) -> None: + super().__init__() + self.robot_name = rospy.get_param('robot_name', 'tb0') + self.request_service = f'{self.robot_name}/merge' + self.map_service = rospy.get_param('map_service', 'dynamic_map') + + self.latest_map = np.array([]) + + rospy.init_node(f"{self.robot_name}_GmappingMerger") + self.rate = rospy.Rate(10) # 10hz + + def run(self) -> None: + rospy.loginfo(f'{self.robot_name} gmapping merger node starting') + + while not rospy.is_shutdown(): + resp = self.get_latest() + if self.check_new(resp): + self.request_merge(resp) + self.rate.sleep() + + rospy.loginfo(f'{self.robot_name} gmapping merger node shutting down') + + def request_merge(self, new_map: OccupancyGrid) -> bool: + """ + Requests a merge from the specified ros service {request_service}. + """ + rospy.wait_for_service(self.request_service) + attempts = 0 + max_attemps = 10 + try: + req = new_map.map + request_service = rospy.ServiceProxy(self.request_service, MergeMap) + return request_service(req) + except rospy.ServiceException as e: + attempts += 1 + rospy.loginfo("service call failed: %s" % e) + if attempts >= max_attemps: + return False + return False + + def get_latest(self): + """ + gets the latest map from map service + """ + rospy.wait_for_service(self.map_service) + while 1: + try: + map_service = rospy.ServiceProxy(self.map_service, GetMap) + return map_service() + except rospy.ServiceException as e: + rospy.loginfo("service call failed: %s" % e) + self.rate.sleep() + + def check_new(self, new_map: OccupancyGrid) -> bool: + if not self.latest_map.any(): + # if we don't have a latest map + return True + + temp = np.array(new_map.data) + # return True if maps don't match, continue to merge req + return not np.all(self.latest_map, temp) + + +if __name__ == '__main__': + GM = GmappingMerger() + GM.run() diff --git a/coms/src/nodes/merge_handler.py b/coms/src/nodes/merge_handler.py new file mode 100755 index 0000000..c7f6e3d --- /dev/null +++ b/coms/src/nodes/merge_handler.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python3 +import rospy +from coms.srv import MergeMap, MergeMapResponse, MergeMapRequest +from nav_msgs.srv import GetMap, GetMapResponse, GetMapRequest +from nav_msgs.msg import OccupancyGrid +from mapmerge.keypoint_merge import orb_mapmerge +from mapmerge.ros_utils import pgm_to_numpy, numpy_to_ros, ros_to_numpy +import numpy as np +import matplotlib.pyplot as plt + +class MergeHandler: + def __init__(self): + self.robot_name = rospy.get_param('~robot_name', 'tb0') + self.map_size = rospy.get_param('~map_size', 100) + + #self.latest_map = np.array([]) + self.seq = 0 + self.data_path = "/root/catkin_ws/src/coms/testData" # avert your eyes, static path! + self.latest_map = pgm_to_numpy(self.data_path + "/map_part0.pgm") + + rospy.init_node(f"{self.robot_name}_MergeHandler") + self.merge_service = rospy.Service(self.robot_name + "/merge", MergeMap, self.merge_new) + self.get_map_service = rospy.Service(self.robot_name + "/get_map", GetMap, self.serve_map) + self.map_publisher = rospy.Publisher(f'{self.robot_name}/map', OccupancyGrid, queue_size=10) + + self.rate = rospy.Rate(10) # 10hz + + def run(self) -> None: + rospy.loginfo(f'{self.robot_name} merge handler node starting') + while not rospy.is_shutdown(): + self.publish_latest() + self.rate.sleep() + rospy.loginfo(f'{self.robot_name} merge handler node shutting down') + + def publish_latest(self) -> None: + occ = self.create_occupancy_msg(self.seq) + self.seq += 1 + self.map_publisher.publish(occ) + + def merge_new(self, req: MergeMapRequest) -> MergeMapResponse: + new_map = ros_to_numpy(np.array(req.map.data)) + try: + merged = orb_mapmerge(new_map, self.latest_map) + # TODO checks and maybe a lock? depends on the rospy callback threading + self.latest_map = merged + return True + except Exception as e: + rospy.loginfo(f"Could not merge maps: {e}") + return False + + def serve_map(self, _) -> GetMapResponse: + """ + serves the latest map + """ + occ = self.create_occupancy_msg(self.seq) + return GetMapResponse(occ) + + def create_occupancy_msg(self, seq: int) -> OccupancyGrid: + occ = OccupancyGrid() + occ.header.seq = seq + occ.data = tuple(numpy_to_ros(self.latest_map.flatten())) + return occ + +if __name__ == "__main__": + MH = MergeHandler() + MH.run() diff --git a/coms/src/nodes/merger.py b/coms/src/nodes/merger.py new file mode 100644 index 0000000..ff9d2b4 --- /dev/null +++ b/coms/src/nodes/merger.py @@ -0,0 +1,17 @@ +from __future__ import annotations +from abc import ABC, abstractmethod +from nav_msgs.msg import OccupancyGrid + + +class ROSMerger(ABC): + + def __init__(self: ROSMerger) -> None: + pass + + @abstractmethod + def request_merge(self: ROSMerger, new_map: OccupancyGrid) -> bool: + pass + + @abstractmethod + def get_latest(self: ROSMerger) -> OccupancyGrid: + pass diff --git a/coms/src/nodes/netsim_merger.py b/coms/src/nodes/netsim_merger.py new file mode 100644 index 0000000..3cd8237 --- /dev/null +++ b/coms/src/nodes/netsim_merger.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python3 +from merger import ROSMerger +from std_msgs.msg import Header +from nav_msgs.srv import GetMap, GetMapResponse +from nav_msgs.msg import OccupancyGrid, MapMetaData +from coms.srv import MergeMap, TriggerMerge, TriggerMergeResponse, TriggerMergeRequest +import numpy as np +import rospy + + +class NetSimMerger(ROSMerger): + + def __init__(self) -> None: + super().__init__() + self.robot_name = rospy.get_param('robot_name', 'tb0') + self.merge_service = f'{self.robot_name}/merge' + self.map_service = f'{self.robot_name}/get_map' + + self.netsim_merge = rospy.Service(self.map_service, TriggerMerge, self.get_netsim_map) + + rospy.init_node(f"{self.robot_name}_NetSimMerger") + self.rate = rospy.Rate(10) # 10hz + rospy.spin() + + def request_merge(self, new_map: OccupancyGrid) -> bool: + """ + Requests a merge from the specified ros service {merge_service}. + """ + rospy.wait_for_service(self.merge_service) + attempts = 0 + max_attemps = 10 + try: + req = new_map.map + merge_service = rospy.ServiceProxy(self.merge_service, MergeMap) + return merge_service(req) + except rospy.ServiceException as e: + attempts += 1 + rospy.loginfo("service call failed: %s" % e) + if attempts >= max_attemps: + return False + return False + + def get_latest(self): + """ + gets the latest map from map service + """ + rospy.wait_for_service(self.map_service) + while 1: + try: + map_service = rospy.ServiceProxy(self.map_service, GetMap) + return map_service() + except rospy.ServiceException as e: + rospy.loginfo("service call failed: %s" % e) + self.rate.sleep() + + # def get_netsim_map(self, req: TriggerMergeRequest): + # """ + # use network simulation to trade maps with another robot + # """ + # return self.netsim_coms() + + # def netsim_coms(self, robot, map) -> OccupancyGrid: + # pass + + # def run(self): + # while 1: + # if merge: + # self.netsim_coms() + + +if __name__ == '__main__': + NSM = NetSimMerger() diff --git a/coms/src/nodes/test_amcl.py b/coms/src/nodes/test_amcl.py new file mode 100755 index 0000000..d405998 --- /dev/null +++ b/coms/src/nodes/test_amcl.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python3 +import rospy +import numpy as np +from nav_msgs.msg import OccupancyGrid + + +class TestAMCL: + def __init__(self): + # GET PARAMS + self.robot_name = rospy.get_param('~robot_name', 'tb0') + + # DATA + self.map = np.array([]) + + # INIT ROS + rospy.init_node(f'{self.robot_name}_test_AMCL') + self.map_subscriber = rospy.Subscriber(f'{self.robot_name}/map', OccupancyGrid, self.verify_testcase) + + # keep the process going + self.rate = rospy.Rate(10) + rospy.spin() + + def verify_testcase(self, msg): + # write some test case methods + assert msg is not None + self.map = np.array(msg.data) + rospy.loginfo(f'{self.map}') + + +if __name__ == "__main__": + TA = TestAMCL() diff --git a/coms/src/nodes/test_gmapping.py b/coms/src/nodes/test_gmapping.py new file mode 100755 index 0000000..9e118c6 --- /dev/null +++ b/coms/src/nodes/test_gmapping.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python3 +import rospy +import numpy as np +from nav_msgs.srv import GetMap, GetMapResponse +from nav_msgs.msg import OccupancyGrid +from mapmerge.ros_utils import pgm_to_numpy, numpy_to_ros + +class TestGMAPPING: + def __init__(self): + # GET PARAMS + self.robot_name = rospy.get_param('~robot_name', 'tb0') + self.map_service = rospy.get_param('~map_service', 'dynamic_map') + + # DATA + self.seq = 0 + self.map = np.eye(100, dtype=np.int16) + self.data_path = "/root/catkin_ws/src/coms/testData" # avert your eyes, static path! + self.map = pgm_to_numpy(self.data_path + "/full_map.pgm") + + # INIT ROS + rospy.init_node(f'{self.robot_name}_test_GMAPPING') + self.map_service = rospy.Service(self.map_service, GetMap, self.serve_map) + + # keep the process going + self.rate = rospy.Rate(10) + rospy.spin() + + def serve_map(self, _): + """ + serves a dummy map. This can be configured to pass testcases through the rosnodes + """ + self.seq += 1 + occ = OccupancyGrid() + occ.header.seq = self.seq + occ.data = list(numpy_to_ros(self.map.flatten())) + return GetMapResponse(occ) + + + +if __name__ == "__main__": + TA = TestGMAPPING() \ No newline at end of file diff --git a/coms/srv/MergeMap.srv b/coms/srv/MergeMap.srv new file mode 100644 index 0000000..bb69867 --- /dev/null +++ b/coms/srv/MergeMap.srv @@ -0,0 +1,4 @@ +# map to be merged in +nav_msgs/OccupancyGrid map +--- +bool success \ No newline at end of file diff --git a/coms/srv/TriggerMerge.srv b/coms/srv/TriggerMerge.srv new file mode 100644 index 0000000..7243eea --- /dev/null +++ b/coms/srv/TriggerMerge.srv @@ -0,0 +1,3 @@ +string robot_id +--- +bool success \ No newline at end of file