feat(slam): add rtabmap_ros

rtabmap_ros/rtabmap_util/scripts/assemble_local_grids.py

@@ -0,0 +1,119 @@
+#!/usr/bin/env python
+
+# Similar to map_assembler node, this minimal python example shows how
+# to reconstruct the obstacle map by subscribing only to
+# graph and latest data added to map (for constant network bandwidth usage).
+
+import rospy
+from sets import Set
+
+import message_filters
+from rtabmap_msgs.msg import MapGraph
+from sensor_msgs.msg import PointCloud2
+from geometry_msgs.msg import Pose
+from geometry_msgs.msg import TransformStamped
+from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud
+
+
+posesDict = {}
+cloudsDict = {}
+assembledCloud = PointCloud2()
+pub = rospy.Publisher('assembled_local_grids', PointCloud2, queue_size=10)
+
+def callback(graph, cloud):
+    global assembledCloud
+    global posesDict
+    global cloudsDict
+    global pub
+
+    begin = rospy.get_time()
+
+    nodeId = graph.posesId[-1]
+    pose = graph.poses[-1]
+    size = cloud.width
+
+    posesDict[nodeId] = pose
+    cloudsDict[nodeId] = cloud
+
+    # Update pose of our buffered clouds.
+    # Check also if the clouds have moved because of a loop closure. If so, we have to update the rendering.
+    maxDiff = 0
+    for i in range(0,len(graph.posesId)):
+        if graph.posesId[i] in posesDict:
+            currentPose = posesDict[graph.posesId[i]].position
+            newPose = graph.poses[i].position
+            diff = max([abs(currentPose.x-newPose.x), abs(currentPose.y-newPose.y), abs(currentPose.z-newPose.z)])
+            if maxDiff < diff:
+                maxDiff = diff
+        else:
+            rospy.loginfo("Old node %d not found in cache, creating an empty cloud.", graph.posesId[i])
+            posesDict[graph.posesId[i]] = graph.poses[i]
+            cloudsDict[graph.posesId[i]] = PointCloud2()
+
+    # If we don't move, some nodes would be removed from the graph, so remove them from our buffered clouds.
+    newGraph = Set(graph.posesId)
+    totalPoints = 0
+    for p in posesDict.keys():
+        if p not in newGraph:
+            posesDict.pop(p)
+            cloudsDict.pop(p)
+        else:
+            totalPoints = totalPoints + cloudsDict[p].width
+
+    if maxDiff > 0.1:
+        # if any node moved more than 10 cm, request an update of the assembled map so far
+        newAssembledCloud = PointCloud2()
+        rospy.loginfo("Map has been optimized! maxDiff=%.3fm, re-updating the whole map...", maxDiff)
+        for i in range(0,len(graph.posesId)):
+            posesDict[graph.posesId[i]] = graph.poses[i]
+            t = TransformStamped()
+            p = posesDict[graph.posesId[i]]
+            t.transform.translation = p.position
+            t.transform.rotation = p.orientation
+            transformedCloud = do_transform_cloud(cloudsDict[graph.posesId[i]], t)
+            if i==0:
+                newAssembledCloud = transformedCloud
+            else:
+                newAssembledCloud.data = newAssembledCloud.data + transformedCloud.data
+                newAssembledCloud.width = newAssembledCloud.width + transformedCloud.width
+                newAssembledCloud.row_step = newAssembledCloud.row_step + transformedCloud.row_step
+        assembledCloud = newAssembledCloud
+    else:
+        t = TransformStamped()
+        t.transform.translation = pose.position
+        t.transform.rotation = pose.orientation
+        transformedCloud = do_transform_cloud(cloud, t)
+        # just concatenate new cloud to current assembled map
+        if assembledCloud.width == 0:
+            assembledCloud = transformedCloud
+        else:
+            # Adding only the difference would be more efficient
+            assembledCloud.data = assembledCloud.data + transformedCloud.data
+            assembledCloud.width = assembledCloud.width + transformedCloud.width
+            assembledCloud.row_step = assembledCloud.row_step + transformedCloud.row_step
+
+    updateTime = rospy.get_time() - begin
+
+    rospy.loginfo("Received node %d (%d pts) at xyz=%.2f %.2f %.2f, q_xyzw=%.2f %.2f %.2f %.2f (Map: Nodes=%d Points=%d Assembled=%d Update=%.0fms)", 
+        nodeId, size,
+        pose.position.x, pose.position.y, pose.position.z,
+        pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w,
+        len(cloudsDict), totalPoints, assembledCloud.width, updateTime*1000)
+
+    assembledCloud.header = graph.header
+    pub.publish(assembledCloud)
+
+def main():
+    rospy.init_node('assemble_local_grids', anonymous=True)
+    graph_sub = message_filters.Subscriber('rtabmap/mapGraph', MapGraph)
+    cloud_sub = message_filters.Subscriber('rtabmap/local_grid_obstacle', PointCloud2)
+
+    ts = message_filters.TimeSynchronizer([graph_sub, cloud_sub], 2)
+    ts.registerCallback(callback)
+    rospy.spin()
+
+if __name__ == '__main__':
+    try:
+        main()
+    except rospy.ROSInterruptException:
+        pass

rtabmap_ros/rtabmap_util/scripts/gazebo_ground_truth.py

@@ -0,0 +1,54 @@
+#!/usr/bin/env python
+import rospy
+import tf
+
+from tf2_msgs.msg import TFMessage
+from gazebo_msgs.msg import LinkStates
+from geometry_msgs.msg import TransformStamped
+
+target_frame_id = ""
+
+def callBack(linkStates):
+	global delta, first
+
+	found = False
+	for i in range(len(linkStates.name)):
+		if linkStates.name[i] == gazebo_frame_id:
+			p = linkStates.pose[i]
+			found = True
+			break
+	
+	if not found:
+ 	    roslog.warn("Gazebo link state \"" + gazebo_frame_id +"\" not found, cannot generate ground truth.")
+	    return
+   
+	t = TransformStamped()
+	t.header.frame_id = frame_id
+        t.header.stamp = rospy.Time.now()
+
+	t.child_frame_id = child_frame_id
+
+	t.transform.translation.x = p.position.x
+	t.transform.translation.y = p.position.y
+	t.transform.translation.z = p.position.z 
+
+	t.transform.rotation.x = p.orientation.x
+	t.transform.rotation.y = p.orientation.y
+	t.transform.rotation.z = p.orientation.z
+	t.transform.rotation.w = p.orientation.w
+	
+	tf_pub.publish(TFMessage([t]))
+
+if __name__ == '__main__':
+    rospy.init_node('generate_gazebo_ground_truth', disable_signals=True)
+
+    frame_id = rospy.get_param('~frame_id', 'world')
+    child_frame_id = rospy.get_param('~child_frame_id', 'base_link_gt')
+    gazebo_frame_id = rospy.get_param('~gazebo_frame_id', 'base_link')
+
+    gazebo_sub = rospy.Subscriber('/gazebo/link_states', LinkStates, callBack)
+   
+    tf_pub = rospy.Publisher('/tf', TFMessage, queue_size=10)
+    tf.TransformBroadcaster()
+
+    rospy.spin()

rtabmap_ros/rtabmap_util/scripts/netvlad_tf_ros.py

@@ -0,0 +1,89 @@
+#!/usr/bin/env python
+
+# Using netvlad tensorflow-v1 implementation from https://github.com/uzh-rpg/netvlad_tf_open/
+# For ROS melodic, follow the following instructions to rebuild cv_bridge with Python3
+# https://medium.com/@beta_b0t/how-to-setup-ros-with-python-3-44a69ca36674
+# On Jetpack 4.4 (18.04 and OpenCV4), use vision_opencv's noetic branch. In cv_bridge/CMakeLists.txt,
+# apply this patch:
+#   -find_package(Boost REQUIRED python37)
+#   +find_package(Boost REQUIRED python3)
+
+from __future__ import print_function
+
+import roslib
+import sys
+import rospy
+import cv2
+import numpy as np
+import tensorflow as tf
+import time
+
+import netvlad_tf.net_from_mat as nfm
+import netvlad_tf.nets as nets
+
+from std_msgs.msg import String
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge, CvBridgeError
+from rtabmap_python import compression as cp
+from rtabmap_msgs.msg import GlobalDescriptor
+
+class netvlad_ros:
+
+  def __init__(self):
+
+    self.dim = rospy.get_param('~dim', 4096)
+    self.scale = rospy.get_param('~scale', 1.0)
+    rospy.loginfo("Parameter dim=%d", self.dim)
+    rospy.loginfo("Parameter scale=%d", self.scale)
+
+    tf.reset_default_graph()
+
+    self.image_batch = tf.placeholder(
+        dtype=tf.float32, shape=[None, None, None, 3])
+
+    self.net_out = nets.vgg16NetvladPca(self.image_batch)
+    self.saver = tf.train.Saver()
+
+    self.sess = tf.Session()
+    self.saver.restore(self.sess, nets.defaultCheckpoint())
+
+    self.pub = rospy.Publisher('netvlad_descriptor', GlobalDescriptor, queue_size=1)  
+
+    self.bridge = CvBridge()
+    self.image_sub = rospy.Subscriber("image",Image,self.callback, queue_size=1)
+
+  def callback(self,data):
+    start = time.time()
+    try:
+      cv_image = self.bridge.imgmsg_to_cv2(data, "rgb8")
+    except CvBridgeError as e:
+      print(e)
+
+    if self.scale != 1.0:
+      width = int(cv_image.shape[1] * self.scale)
+      height = int(cv_image.shape[0] * self.scale)
+      cv_image = cv2.resize(cv_image, (width, height), interpolation = cv2.INTER_AREA)
+
+    batch = np.expand_dims(cv_image, axis=0)
+    result = self.sess.run(self.net_out, feed_dict={self.image_batch: batch})
+    result = result[:,:self.dim]
+
+    descriptor = GlobalDescriptor()
+    descriptor.type = 0
+    descriptor.header = data.header
+    descriptor.data = cp.compress(result)
+    self.pub.publish(descriptor)
+    end = time.time()
+    rospy.loginfo("Extracting descriptor (img=%dx%d, dim=%d): %fs", cv_image.shape[1], cv_image.shape[0], self.dim, end-start)
+
+def main(args):
+  rospy.init_node('netvlad', anonymous=True)
+  n = netvlad_ros()
+  try:
+    rospy.spin()
+  except KeyboardInterrupt:
+    print("Shutting down")
+
+if __name__ == '__main__':
+    main(sys.argv)
+

rtabmap_ros/rtabmap_util/scripts/objects_to_tags.py

@@ -0,0 +1,46 @@
+#!/usr/bin/env python
+import rospy
+from apriltag_ros.msg import AprilTagDetectionArray
+from apriltag_ros.msg import AprilTagDetection
+from find_object_2d.msg import ObjectsStamped
+import tf
+import geometry_msgs.msg
+
+objFramePrefix_ = "object"
+distanceMax_ = 0.0
+
+def callback(data):
+    global objFramePrefix_
+    global distanceMax_
+    if len(data.objects.data) > 0:
+        output = AprilTagDetectionArray()
+        output.header = data.header
+        for i in range(0,len(data.objects.data),12):
+            try:
+                objId = data.objects.data[i]
+                (trans,quat) = listener.lookupTransform(data.header.frame_id, objFramePrefix_+'_'+str(int(objId)), data.header.stamp)
+                tag = AprilTagDetection()
+                tag.id.append(objId)
+                tag.pose.pose.pose.position.x = trans[0]
+                tag.pose.pose.pose.position.y = trans[1]
+                tag.pose.pose.pose.position.z = trans[2]
+                tag.pose.pose.pose.orientation.x = quat[0]
+                tag.pose.pose.pose.orientation.y = quat[1]
+                tag.pose.pose.pose.orientation.z = quat[2]
+                tag.pose.pose.pose.orientation.w = quat[3]
+                tag.pose.header = output.header
+                if distanceMax_ <= 0.0 or trans[2] < distanceMax_:
+                    output.detections.append(tag)
+            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
+                continue
+        if len(output.detections) > 0:
+            pub.publish(output)
+
+if __name__ == '__main__':
+    pub = rospy.Publisher('tag_detections', AprilTagDetectionArray, queue_size=10)
+    rospy.init_node('objects_to_tags', anonymous=True)
+    rospy.Subscriber("objectsStamped", ObjectsStamped, callback)
+    objFramePrefix_ = rospy.get_param('~object_prefix', objFramePrefix_)
+    distanceMax_ = rospy.get_param('~distance_max', distanceMax_)
+    listener = tf.TransformListener()
+    rospy.spin()

rtabmap_ros/rtabmap_util/scripts/patrol.py

@@ -0,0 +1,87 @@
+#!/usr/bin/env python
+import rospy
+import sys
+from std_msgs.msg import Bool
+from rtabmap_ros.msg import Goal
+
+pub = rospy.Publisher('rtabmap/goal_node', Goal, queue_size=1)
+waypoints = []
+currentIndex = 0
+waitingTime = 1.0
+frameId = ""
+
+def callback(data):
+    global currentIndex
+    global waitingTime
+    global frameId
+    if data.data:
+        rospy.loginfo(rospy.get_caller_id() + ": Goal '%s' reached! Publishing next goal in %.1f sec...", waypoints[currentIndex], waitingTime)
+    else:
+        rospy.loginfo(rospy.get_caller_id() + ": Goal '%s' failed! Publishing next goal in %.1f sec...", waypoints[currentIndex], waitingTime)
+
+    currentIndex = (currentIndex+1) % len(waypoints)
+
+    # Waiting time before sending next goal
+    rospy.sleep(waitingTime)
+
+    msg = Goal()
+    msg.frame_id = frameId
+    try:
+        int(waypoints[currentIndex])
+        is_dig = True
+    except ValueError:
+        is_dig = False
+    if is_dig:
+        msg.node_id = int(waypoints[currentIndex])
+        msg.node_label = ""
+    else:
+        msg.node_id = 0
+        msg.node_label = waypoints[currentIndex]
+    
+    rospy.loginfo(rospy.get_caller_id() + ": Publishing goal '%s'! (%d/%d)", waypoints[currentIndex], currentIndex+1, len(waypoints))
+    msg.header.stamp = rospy.get_rostime()
+    pub.publish(msg)
+
+def main():
+    rospy.init_node('patrol', anonymous=False)
+    sub = rospy.Subscriber("rtabmap/goal_reached", Bool, callback)
+    global waitingTime
+    global frameId
+    waitingTime = rospy.get_param('~time', waitingTime)
+    frameId = rospy.get_param('~frame_id', frameId)
+    rospy.sleep(1.) # make sure that subscribers have seen this node before sending a goal
+
+    rospy.loginfo(rospy.get_caller_id() + ": Waypoints: [%s]", str(waypoints).strip('[]'))
+    rospy.loginfo(rospy.get_caller_id() + ": time: %f", waitingTime)
+    rospy.loginfo(rospy.get_caller_id() + ": publish goal on %s", pub.resolved_name)
+    rospy.loginfo(rospy.get_caller_id() + ": receive goal status on %s", sub.resolved_name)
+
+    # send the first goal
+    msg = Goal()
+    msg.frame_id = frameId
+    try:
+        int(waypoints[currentIndex])
+        is_dig = True
+    except ValueError:
+        is_dig = False
+    if is_dig:
+        msg.node_id = int(waypoints[currentIndex])
+        msg.node_label = ""
+    else:
+        msg.node_id = 0
+        msg.node_label = waypoints[currentIndex]
+    while rospy.Time.now().secs == 0:
+         rospy.loginfo(rospy.get_caller_id() + ": Waiting clock...")
+         rospy.sleep(.1)
+    msg.header.stamp = rospy.Time.now()
+    rospy.loginfo(rospy.get_caller_id() + ": Publishing goal '%s'! (%d/%d)", waypoints[currentIndex], currentIndex+1, len(waypoints))
+    pub.publish(msg)
+    rospy.spin()    
+
+if __name__ == '__main__':
+    if len(sys.argv) < 3:
+        print("usage: patrol.py waypointA waypointB waypointC ... [_time:=1 frame_id:=base_footprint] [topic remaps] (at least 2 waypoints, can be node id, landmark or label)")
+    else:
+        waypoints = sys.argv[1:] 
+        waypoints = [x for x in waypoints if not x.startswith('/') and not x.startswith('_')]
+        main()

rtabmap_ros/rtabmap_util/scripts/point_to_tf.py

@@ -0,0 +1,28 @@
+#!/usr/bin/env python
+import rospy
+import tf
+from geometry_msgs.msg import PointStamped
+
+def callback(point):
+    global br
+    global frame_id
+    local_frame_id = point.header.frame_id
+    if not local_frame_id:
+        local_frame_id = frame_id
+    br.sendTransform(
+        (point.point.x, point.point.y, point.point.z),
+        tf.transformations.quaternion_from_euler(0,0,0),
+        point.header.stamp,
+        local_frame_id,
+        fixed_frame_id)
+
+if __name__ == "__main__":
+
+    rospy.init_node("point_to_tf", anonymous=True)
+
+    frame_id = rospy.get_param('~frame_id', 'point')
+    fixed_frame_id = rospy.get_param('~fixed_frame_id', 'world')
+
+    br = tf.TransformBroadcaster()
+    rospy.Subscriber("point", PointStamped, callback, queue_size=1)
+    rospy.spin()

rtabmap_ros/rtabmap_util/scripts/republish_tf_static.py

@@ -0,0 +1,43 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import DurabilityPolicy
+from rclpy.qos import HistoryPolicy
+from rclpy.qos import QoSProfile
+from tf2_msgs.msg import TFMessage
+
+class StaticTransformRepublisher(Node):
+
+    def __init__(self):
+        super().__init__('static_transform_republisher')
+        qos = QoSProfile(
+                depth=1,
+                durability=DurabilityPolicy.TRANSIENT_LOCAL,
+                history=HistoryPolicy.KEEP_LAST,
+                )
+        self.publisher_ = self.create_publisher(TFMessage, '/tf_static', qos)
+        self.data = TFMessage()
+        self.subscription = self.create_subscription(
+            TFMessage,
+            '/tf_static_old',
+            self.listener_callback,
+            10)
+        self.subscription  # prevent unused variable warning
+
+    def listener_callback(self, msg):
+        if len(self.data.transforms) == 0:
+            self.data = msg
+        else:
+            self.data.transforms = self.data.transforms + msg.transforms
+        self.get_logger().info('"Received /tf_static_old and republising latched /tf_static"')
+        self.publisher_.publish(self.data)
+
+def main(args=None):
+    rclpy.init(args=args)
+    static_transform_republisher = StaticTransformRepublisher()
+    rclpy.spin(static_transform_republisher)
+    minimal_publisher.destroy_node()
+    rclpy.shutdown()
+    
+if __name__ == '__main__':
+    main()

rtabmap_ros/rtabmap_util/scripts/transform_to_tf.py

@@ -0,0 +1,44 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import TransformStamped
+from tf2_ros import TransformBroadcaster
+
+class TransformToTf(Node):
+
+    def __init__(self):
+        super().__init__('transform_to_tf')
+        
+        self.declare_parameter('frame_id', 'world')
+        self.declare_parameter('child_frame_id', 'transform')
+        self.frame_id = self.get_parameter('frame_id').get_parameter_value().string_value
+        self.child_frame_id = self.get_parameter('child_frame_id').get_parameter_value().string_value
+
+        self.tf_broadcaster = TransformBroadcaster(self)
+
+        self.subscription = self.create_subscription(
+            TransformStamped,
+            'transform',
+            self.callback,
+            1)
+        self.subscription  # prevent unused variable warning
+
+    def callback(self, t):
+        if not t.header.frame_id:
+            t.header.frame_id = self.frame_id
+        if not t.child_frame_id:
+            t.child_frame_id = self.child_frame_id
+
+        self.tf_broadcaster.sendTransform(t)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    transform_to_tf = TransformToTf()
+    rclpy.spin(transform_to_tf)
+    transform_to_tf.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()
+   

rtabmap_ros/rtabmap_util/scripts/yaml_to_camera_info.py

@@ -0,0 +1,81 @@
+#!/usr/bin/env python3
+import rclpy
+import yaml
+import sys
+from rclpy.node import Node
+from sensor_msgs.msg import CameraInfo
+from sensor_msgs.msg import Image
+
+def yaml_to_CameraInfo(yaml_fname):
+    with open(yaml_fname, "r") as file_handle:
+        first_line = file_handle.readline()
+        if "%YAML:" not in first_line:
+            file_handle.seek(0)
+        calib_data = yaml.load(file_handle, Loader=yaml.FullLoader)
+ 
+    msg = CameraInfo()
+    msg.width = calib_data["image_width"]
+    msg.height = calib_data["image_height"]
+    msg.k = calib_data["camera_matrix"]["data"]
+    msg.d = calib_data["distortion_coefficients"]["data"]
+    msg.r = calib_data["rectification_matrix"]["data"]
+    msg.p = calib_data["projection_matrix"]["data"]
+    msg.distortion_model = calib_data["distortion_model"]
+    return msg
+
+class YamlToCameraInfo(Node):
+
+    def __init__(self):
+        super().__init__('yaml_to_camera_info')
+        
+        self.declare_parameter('yaml_path', '')
+        self.declare_parameter('scale', 1.0)
+        yaml_path = self.get_parameter('yaml_path').get_parameter_value().string_value
+        scale = self.get_parameter('scale').get_parameter_value().double_value
+
+        if not yaml_path:
+            print('yaml_path parameter should be set to path of the calibration file!')
+            sys.exit(1)
+
+        self.declare_parameter('frame_id', '')
+        self.frame_id = self.get_parameter('frame_id').get_parameter_value().string_value
+        
+        self.camera_info_msg = yaml_to_CameraInfo(yaml_path)
+        
+        if scale!=1.0:
+            self.camera_info_msg.k[0] = self.camera_info_msg.k[0]*scale
+            self.camera_info_msg.k[2] = self.camera_info_msg.k[2]*scale
+            self.camera_info_msg.k[4] = self.camera_info_msg.k[4]*scale
+            self.camera_info_msg.k[5] = self.camera_info_msg.k[5]*scale
+            self.camera_info_msg.p[0] = self.camera_info_msg.p[0]*scale
+            self.camera_info_msg.p[2] = self.camera_info_msg.p[2]*scale
+            self.camera_info_msg.p[3] = self.camera_info_msg.p[3]*scale
+            self.camera_info_msg.p[5] = self.camera_info_msg.p[5]*scale
+            self.camera_info_msg.p[6] = self.camera_info_msg.p[6]*scale
+            self.camera_info_msg.width = int(self.camera_info_msg.width*scale)
+            self.camera_info_msg.height = int(self.camera_info_msg.height*scale)
+
+        self.publisher_ = self.create_publisher(CameraInfo, 'camera_info', 1)
+        self.subscription = self.create_subscription(
+            Image,
+            'image',
+            self.callback,
+            1)
+        self.subscription  # prevent unused variable warning
+
+    def callback(self, image):
+        self.camera_info_msg.header = image.header
+        if self.frame_id:
+            self.camera_info_msg.header.frame_id = self.frame_id
+        self.publisher_.publish(self.camera_info_msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    yaml_to_camera_info = YamlToCameraInfo()
+    rclpy.spin(yaml_to_camera_info)
+    yaml_to_camera_info.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()