gps_to_path: Added path coverage computation.

Author: peci1

gps_to_path/launch/visualization.launch

@@ -89,6 +89,13 @@
         <remap from="waypoints" to="original_waypoints" />
         <remap from="path" to="original_waypoints_path" />
     </node>
+    
+    <node name="path_coverage" pkg="gps_to_path" type="path_coverave" respawn="true">
+        <param name="out_frame_id" value="utm_local" />
+        <param name="num_dimensions" value="2" />
+        <remap from="~path" to="/original_waypoints_path" />
+        <remap from="~odom" to="/gps_odom" />
+    </node>
 
     <!-- Support for remaining battery visualization. -->
     <node name="$(anon tradr_battery_viz)" pkg="topic_tools" type="relay_field"

gps_to_path/nodes/path_coverage

@@ -0,0 +1,178 @@
+#!/usr/bin/env python
+
+# SPDX-License-Identifier: BSD-3-Clause
+# SPDX-FileCopyrightText: Czech Technical University in Prague
+
+"""
+Visualize coverage of a path by some odometry.
+
+Publications:
+- `~coverage` (:class:`sensor_msgs.msg.PointCloud2`): Coverage pointcloud.
+  - Channel 'coverage' contains the binary covered (1)/not covered (0) information.
+  - Channel 'min_distance' is the minimum distance of the point on path from an odometry point (will be infinity for
+    points on path that never were the closest to any odometry measurement).
+
+Subscriptions:
+- `~path` (:class:`nav_msgs.msg.Path`): The path whose coverage should be visualized.
+- `~odom` (:class:`nav_msgs.msg.Odometry`): The odometry that fulfills the path points.
+
+Parameters:
+- `~max_coverage_distance` (float, defaults to 1.0, meters): Maximum distance from path to be treated as covered.
+- `~path_points_distance` (float, defaults to 1.0, meters): Maximum distance between two points on the path (used for
+                                                            discretization of the path).
+- `~out_frame_id` (string, defaults to empty string): Frame ID of the output pointcloud. If empty, path frame ID is
+                                                      used.
+- `~num_dimensions` (int, 2 or 3, defaults to 3): Whether to compute the distance only in XY or in XYZ.
+"""
+
+from threading import Lock
+
+import rospy
+from geometry_msgs.msg import PoseStamped
+from ros_numpy import numpify, msgify
+from tf2_ros import TransformListener, Buffer
+import tf2_geometry_msgs
+
+from nav_msgs.msg import Path, Odometry
+from sensor_msgs.msg import PointCloud2
+import numpy as np
+
+from scipy.spatial import cKDTree as KDTree
+
+import cras
+
+
+class PathCoverage(cras.Node):
+    """Visualize path coverage."""
+
+    def __init__(self):
+        super(PathCoverage, self).__init__()
+
+        self.path_points = None
+        self.path_points_kdtree = None
+        self.min_distances = None
+
+        self.mutex = Lock()
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer)
+
+        self.max_coverage_distance = cras.get_param('~max_coverage_distance', 1.0, "m")
+        self.path_points_distance = cras.get_param('~path_points_distance', 1.0, "m")
+        self.out_frame_id = cras.get_param('~out_frame_id', "")
+        self.ndims = cras.get_param('~num_dimensions', 3)
+        if self.ndims not in (2, 3):
+            raise RuntimeError("num_dimensions has to be either 2 or 3")
+        self.pub_coverage = rospy.Publisher('~coverage', PointCloud2, queue_size=1, latch=True)
+        self.sub_path = rospy.Subscriber('~path', Path, self.path_cb, queue_size=1)
+        self.sub_odom = rospy.Subscriber('~odom', Odometry, self.odom_cb, queue_size=100)
+
+    def reset(self):
+        self.sub_path.unregister()
+        with self.mutex:
+            self.path_points = None
+            self.path_points_kdtree = None
+            self.min_distances = None
+
+        self.sub_path = rospy.Subscriber('~path', Path, self.path_cb, queue_size=1)
+        super(PathCoverage, self).reset()
+
+    def path_cb(self, msg):
+        """
+        :param Path msg: 
+        :return: 
+        """
+        self.check_time_jump()
+
+        if len(self.out_frame_id) == 0:
+            self.out_frame_id = msg.header.frame_id
+
+        with self.mutex:
+            new_points = self.path_msg_to_points(msg)
+            if self.path_points is None:
+                self.path_points = new_points
+                self.min_distances = np.ones(new_points.shape[0]) * np.inf
+            else:
+                distances, _ = self.path_points_kdtree.query(
+                    new_points[:, :self.ndims], k=1, distance_upper_bound=self.path_points_distance)
+                not_present_idxs = np.where(np.isinf(distances))
+                added_points = new_points[not_present_idxs]
+                if added_points.shape[0] > 0:
+                    self.path_points = np.vstack((self.path_points, added_points))
+                    self.min_distances = np.hstack((self.min_distances, np.ones(added_points.shape[0]) * np.inf))
+            self.path_points_kdtree = KDTree(self.path_points[:, :self.ndims])
+
+    def odom_cb(self, msg):
+        """
+        :param Odometry msg: 
+        :return: 
+        """
+        self.check_time_jump()
+        if self.path_points is None:
+            return
+
+        pos_stamped = PoseStamped()
+        pos_stamped.header = msg.header
+        pos_stamped.pose = msg.pose.pose
+        pos_transformed = self.tf_buffer.transform(pos_stamped, self.out_frame_id)
+        pos = numpify(pos_transformed.pose.position)[:self.ndims]
+        with self.mutex:
+            d, i = self.path_points_kdtree.query(pos, k=1, distance_upper_bound=self.max_coverage_distance)
+            if np.isfinite(d):
+                self.min_distances[i] = min(self.min_distances[i], d)
+            pcl = np.zeros((self.path_points.shape[0],), dtype=[
+                ('x', np.float32),
+                ('y', np.float32),
+                ('z', np.float32),
+                ('coverage', np.uint8),
+                ('min_distance', np.float32),
+            ])
+            pcl['x'] = self.path_points[:, 0]
+            pcl['y'] = self.path_points[:, 1]
+            pcl['z'] = self.path_points[:, 2]
+            pcl['coverage'] = self.min_distances <= self.max_coverage_distance
+            pcl['min_distance'] = self.min_distances
+            coverage_pct = np.sum(pcl['coverage']) / float(self.min_distances.shape[0]) * 100.0
+        pcl_msg = msgify(PointCloud2, pcl)
+        pcl_msg.header.frame_id = self.out_frame_id
+        pcl_msg.header.stamp = msg.header.stamp
+        self.pub_coverage.publish(pcl_msg)
+
+        rospy.loginfo_throttle(10.0, "Path coverage %.1f%%" % (coverage_pct,))
+
+    def path_msg_to_points(self, msg):
+        """
+        :param Path msg: 
+        :return: 
+        """
+        points = list()
+        for pose in msg.poses:
+            point = numpify(self.tf_buffer.transform(pose, self.out_frame_id).pose.position)
+            if len(points) == 0 or np.linalg.norm(point - points[-1]) <= self.path_points_distance:
+                points.append(point)
+            else:
+                dir = (point - points[-1])
+                length = np.linalg.norm(dir)
+                dir /= length
+                dir *= self.path_points_distance
+                # dir is now a vector of length path_points_distance
+                for _ in range(1, int(np.floor(length / self.path_points_distance))):
+                    p = points[-1] + dir
+                    points.append(p)
+                points.append(point)
+
+        return np.array(points)
+
+    @staticmethod
+    def run():
+        rospy.spin()
+
+
+def main():
+    rospy.init_node('path_coverage')
+
+    node = PathCoverage()
+    node.run()
+
+
+if __name__ == '__main__':
+    main()