Ros nodes (#3)

Add support for merging ROS nodes

Part of #7

.gitignore

@@ -58,8 +58,11 @@ CATKIN_IGNORE
 # VSCode
 .vscode
 
+# Pycharm
+.idea/
+
 # macOS cache
 .DS_Store
 
 # Coveralls
-.coverage
+.coverage

.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:

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; \

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
 )
 

coms/launch/test_maphandler.launch

@@ -0,0 +1,23 @@
+<launch>
+    <arg name='robot_name' default='test_bot' />
+    <arg name='map_service' default='dynamic_map'/>
+
+    <node name='merge_handler' pkg='coms' type='merge_handler.py' output="screen">
+        <param name="robot_name" value="$(arg robot_name)"/>
+        <param name="map_service" value="$(arg map_service)"/>
+    </node>
+
+    <node name='gmapping_merger' pkg='coms' type='gmapping_merger.py' output="screen">
+        <param name="robot_name" value="$(arg robot_name)"/>
+        <param name="map_service" value="$(arg map_service)"/>
+    </node>
+
+    <node name='test_amcl' pkg='coms' type='test_amcl.py' output="screen">
+        <param name="robot_name" value="$(arg robot_name)"/>
+    </node>
+
+    <node name='test_gmapping' pkg='coms' type='test_gmapping.py' output="screen">
+        <param name="robot_name" value="$(arg robot_name)"/>
+        <param name="map_service" value="$(arg map_service)"/>
+    </node>
+</launch>

coms/launch/test_mapmerge.launch

@@ -0,0 +1,18 @@
+<launch>
+    <arg name='robot_name' default='test_bot' />
+    <arg name='map_service' default='dynamic_map'/>
+
+    <node name='mapmerger' pkg='coms' type='mapmerger.py' output="screen">
+        <param name="robot_name" value="$(arg robot_name)"/>
+        <param name="map_service" value="$(arg map_service)"/>
+    </node>
+
+    <node name='test_amcl' pkg='coms' type='test_amcl.py' output="screen">
+        <param name="robot_name" value="$(arg robot_name)"/>
+    </node>
+
+    <node name='test_gmapping' pkg='coms' type='test_gmapping.py' output="screen">
+        <param name="robot_name" value="$(arg robot_name)"/>
+        <param name="map_service" value="$(arg map_service)"/>
+    </node>
+</launch>

coms/package.xml

@@ -10,13 +10,17 @@
 
   <license>MIT</license>
 
+  <build_depend>message_generation</build_depend>
+  <exec_depend>message_runtime</exec_depend>
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
   <build_export_depend>rospy</build_export_depend>
   <exec_depend>rospy</exec_depend>
   <exec_depend>python3-numpy</exec_depend>
   <exec_depend>std_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
 
   <export>
   </export>

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

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 *

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)

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])
+    return apply_warp(map2, M[:2])

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):
     """

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
 

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()