├── .gitignore ├── LICENSE ├── README.md ├── kf_hungarian_tracker ├── README.md ├── config │ └── kf_hungarian.yaml ├── kf_hungarian_tracker │ ├── __init__.py │ ├── kf_hungarian_node.py │ └── obstacle_class.py ├── launch │ └── kf_hungarian.launch.py ├── package.xml ├── resource │ └── kf_hungarian_tracker ├── setup.cfg ├── setup.py └── test │ ├── test_copyright.py │ ├── test_flake8.py │ └── test_pep257.py └── nav2_dynamic_msgs ├── CMakeLists.txt ├── msg ├── Obstacle.msg └── ObstacleArray.msg └── package.xml /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | *.pyc 3 | \#* 4 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # navigation2_dynamic 2 | Nav2's dynamic obstacle detection, tracking, and processing pipelines. 3 | -------------------------------------------------------------------------------- /kf_hungarian_tracker/README.md: -------------------------------------------------------------------------------- 1 | # kf_hungarian_tracker 2 | 3 | This package implememts a multi-object tracker with Kalman filter and Hungarian algorithm. 4 | Hungarian algorithm is used to associate detection and previously known objects. 5 | For each object, a Kalman filter is maintained to track the position and velocity. 6 | 7 | ### Hungarian algorithm 8 | 9 | [Hungarian algorithm](https://en.wikipedia.org/wiki/Hungarian_algorithm) is a combinatorial optimization algorithm that solves the assignment problem in polynomial time. 10 | Here we use an implementation from `scipy.optimize.linear_sum_assignment`. The default cost function is Euclidean distance between two object's center. 11 | More cost functions like IoU of bounding boxes could be used. 12 | 13 | ### Parameters 14 | 15 | Parameters can be set in `config/kf_hungarian.yaml`. For more information on parameters for Kalman filter, check out [KalmanFilter](https://docs.opencv.org/master/dd/d6a/classcv_1_1KalmanFilter.html) from OpenCV. 16 | 17 | | parameters | Meaning | Default | 18 | | ---------------- | ------------- | ------- | 19 | | global_frame | transform from pointcloud frame to global frame,
None means message frame is global | camera_link | 20 | | death_threshold | maximum missing frames before deleting an obstacle | 3 | 21 | | top_down | whether project 3D points on ground plane, set to do top-down tracking | False | 22 | | measurement_noise_cov | measurement noise for Kalman filter [x,y,z] | [1., 1., 1.] | 23 | | error_cov_post | initial posteriori error estimate covariance matrix [x,y,z,vx,vy,vz] | [1., 1., 1., 10., 10., 10.] | 24 | | process_noise_cov | model process noise covariance with estimated [acceleration](https://github.com/tony23545/navigation2_dynamic/blob/master/kf_hungarian_tracker/kf_hungarian_tracker/obstacle_class.py#L59) [ax,ay,az] | [2., 2., 0.5] | 25 | | vel_filter | minimum and maximum velocity to filter obstacles [min,max] (m/s) | [0.1, 2.0] | 26 | | height_filter | minimum and maximum height (z) to filter obstacles [min,max] (m) | [-2.0, 2.0] | 27 | | cost_filter | filter Hungarian assignments with cost greater than threshold (unit is `m` for Euclidean cost function) | 1.0 | 28 | * The default units are `m` and `m/s`, could be changed according to the detection. 29 | -------------------------------------------------------------------------------- /kf_hungarian_tracker/config/kf_hungarian.yaml: -------------------------------------------------------------------------------- 1 | kf_hungarian_node: 2 | ros__parameters: 3 | death_threshold: 3 # maximum mising frames before deleting an obstacle 4 | global_frame: "camera_link" # none means camera frame is the global frame 5 | # Kamlan filter related 6 | top_down: False # whether use top-down (x, y) view or 3D (x, y, z) 7 | measurement_noise_cov: [1., 1., 1.] # if it's use top-down view, set the 3rd number to 0 8 | error_cov_post: [1., 1., 1., 10., 10., 10.] # if use top-down view, set the 3rd and 6th to 0 9 | process_noise_cov: [2., 2., 0.5] # processNoiseCov, model as acceration noise, if use top-down view, the 3rd number should be 0 10 | # obstacle filter 11 | vel_filter: [0.1, 2.0] # minimum and maximum velocity to filter obstacles 12 | height_filter: [-2.0, 2.0] # minimum and maximum height (z) to filter obstacles 13 | cost_filter: 1.0 # filter Hungarian assignment with cost greater than threshold 14 | -------------------------------------------------------------------------------- /kf_hungarian_tracker/kf_hungarian_tracker/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-navigation/navigation2_dynamic/e1b0d920fa309c7eb072524b00de7ee12e21076e/kf_hungarian_tracker/kf_hungarian_tracker/__init__.py -------------------------------------------------------------------------------- /kf_hungarian_tracker/kf_hungarian_tracker/kf_hungarian_node.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import uuid 3 | import math 4 | from scipy.optimize import linear_sum_assignment 5 | 6 | from nav2_dynamic_msgs.msg import Obstacle, ObstacleArray 7 | from visualization_msgs.msg import Marker, MarkerArray 8 | 9 | import rclpy 10 | import copy 11 | from rclpy.node import Node 12 | import colorsys 13 | from kf_hungarian_tracker.obstacle_class import ObstacleClass 14 | 15 | from tf2_ros import TransformException 16 | from tf2_ros.buffer import Buffer 17 | from tf2_ros.transform_listener import TransformListener 18 | from tf2_geometry_msgs import do_transform_point, do_transform_vector3 19 | from geometry_msgs.msg import PointStamped, Vector3Stamped 20 | 21 | 22 | class KFHungarianTracker(Node): 23 | """Use Kalman Fiter and Hungarian algorithm to track multiple dynamic obstacles 24 | 25 | Use Hungarian algorithm to match presenting obstacles with new detection and maintain a kalman filter for each obstacle. 26 | spawn ObstacleClass when new obstacles come and delete when they disappear for certain number of frames 27 | 28 | Attributes: 29 | obstacle_list: a list of ObstacleClass that currently present in the scene 30 | sec, nanosec: timing from sensor msg 31 | detection_sub: subscrib detection result from detection node 32 | tracker_obstacle_pub: publish tracking obstacles with ObstacleArray 33 | tracker_pose_pub: publish tracking obstacles with PoseArray, for rviz visualization 34 | """ 35 | 36 | def __init__(self): 37 | """initialize attributes and setup subscriber and publisher""" 38 | 39 | super().__init__("kf_hungarian_node") 40 | self.declare_parameters( 41 | namespace="", 42 | parameters=[ 43 | ("global_frame", "camera_link"), 44 | ("process_noise_cov", [2.0, 2.0, 0.5]), 45 | ("top_down", False), 46 | ("death_threshold", 3), 47 | ("measurement_noise_cov", [1.0, 1.0, 1.0]), 48 | ("error_cov_post", [1.0, 1.0, 1.0, 10.0, 10.0, 10.0]), 49 | ("vel_filter", [0.1, 2.0]), 50 | ("height_filter", [-2.0, 2.0]), 51 | ("cost_filter", 1.0), 52 | ("transform_to_global_frame", True), 53 | ("infer_orientation_from_velocity", True), 54 | ], 55 | ) 56 | self.global_frame = self.get_parameter("global_frame")._value 57 | self.death_threshold = self.get_parameter("death_threshold")._value 58 | self.measurement_noise_cov = self.get_parameter("measurement_noise_cov")._value 59 | self.error_cov_post = self.get_parameter("error_cov_post")._value 60 | self.process_noise_cov = self.get_parameter("process_noise_cov")._value 61 | self.vel_filter = self.get_parameter("vel_filter")._value 62 | self.height_filter = self.get_parameter("height_filter")._value 63 | self.top_down = self.get_parameter("top_down")._value 64 | self.cost_filter = self.get_parameter("cost_filter")._value 65 | self.transform_to_global_frame = self.get_parameter( 66 | "transform_to_global_frame" 67 | )._value 68 | self.infer_orientation_from_velocity = self.get_parameter( 69 | "infer_orientation_from_velocity" 70 | )._value 71 | 72 | self.obstacle_list = [] 73 | self.sec = 0 74 | self.nanosec = 0 75 | 76 | # subscribe to detector 77 | self.detection_sub = self.create_subscription( 78 | ObstacleArray, "detection", self.callback, 10 79 | ) 80 | 81 | # publisher for tracking result 82 | self.tracker_obstacle_pub = self.create_publisher(ObstacleArray, "tracking", 10) 83 | self.tracker_marker_pub = self.create_publisher( 84 | MarkerArray, "tracking_marker", 10 85 | ) 86 | 87 | # setup tf related 88 | self.tf_buffer = Buffer() 89 | self.tf_listener = TransformListener(self.tf_buffer, self) 90 | 91 | def callback(self, msg): 92 | """callback function for detection result""" 93 | 94 | # update delta time 95 | dt = (msg.header.stamp.sec - self.sec) + ( 96 | msg.header.stamp.nanosec - self.nanosec 97 | ) / 1e9 98 | self.sec = msg.header.stamp.sec 99 | self.nanosec = msg.header.stamp.nanosec 100 | 101 | # get detection 102 | detections = msg.obstacles 103 | num_of_detect = len(detections) 104 | num_of_obstacle = len(self.obstacle_list) 105 | 106 | # kalman predict 107 | for obj in self.obstacle_list: 108 | obj.predict(dt) 109 | 110 | if (self.transform_to_global_frame) and (self.global_frame is not None): 111 | try: 112 | trans = self.tf_buffer.lookup_transform( 113 | self.global_frame, msg.header.frame_id, rclpy.time.Time() 114 | ) 115 | msg.header.frame_id = self.global_frame 116 | # do_transform_vector3(vector, trans) resets trans.transform.translation 117 | # values to 0.0, so we need to preserve them for future usage in the loop below 118 | translation_backup_x = trans.transform.translation.x 119 | translation_backup_y = trans.transform.translation.y 120 | translation_backup_z = trans.transform.translation.z 121 | for i in range(len(detections)): 122 | trans.transform.translation.x = translation_backup_x 123 | trans.transform.translation.y = translation_backup_y 124 | trans.transform.translation.z = translation_backup_z 125 | # transform position (point) 126 | p = PointStamped() 127 | p.point = detections[i].position 128 | detections[i].position = do_transform_point(p, trans).point 129 | # transform velocity (vector3) 130 | v = Vector3Stamped() 131 | v.vector = detections[i].velocity 132 | detections[i].velocity = do_transform_vector3(v, trans).vector 133 | # transform size (vector3) 134 | s = Vector3Stamped() 135 | s.vector = detections[i].size 136 | detections[i].size = do_transform_vector3(s, trans).vector 137 | 138 | except TransformException as ex: 139 | self.get_logger().error( 140 | "fail to get tf from {} to {}: {}".format( 141 | msg.header.frame_id, self.global_frame, ex 142 | ) 143 | ) 144 | return 145 | 146 | # hungarian matching 147 | cost = np.zeros((num_of_obstacle, num_of_detect)) 148 | for i in range(num_of_obstacle): 149 | for j in range(num_of_detect): 150 | cost[i, j] = self.obstacle_list[i].distance(detections[j]) 151 | obs_ind, det_ind = linear_sum_assignment(cost) 152 | 153 | # filter assignment according to cost 154 | new_obs_ind = [] 155 | new_det_ind = [] 156 | for o, d in zip(obs_ind, det_ind): 157 | if cost[o, d] < self.cost_filter: 158 | new_obs_ind.append(o) 159 | new_det_ind.append(d) 160 | obs_ind = new_obs_ind 161 | det_ind = new_det_ind 162 | 163 | # kalman update 164 | for o, d in zip(obs_ind, det_ind): 165 | self.obstacle_list[o].correct(detections[d]) 166 | 167 | # birth of new detection obstacles and death of disappear obstacle 168 | self.birth(det_ind, num_of_detect, detections) 169 | dead_object_list = self.death(obs_ind, num_of_obstacle) 170 | 171 | # apply velocity and height filter 172 | filtered_obstacle_list = [] 173 | for obs in self.obstacle_list: 174 | obs_vel = np.linalg.norm( 175 | np.array([obs.msg.velocity.x, obs.msg.velocity.y, obs.msg.velocity.z]) 176 | ) 177 | obs_height = obs.msg.position.z 178 | if ( 179 | obs_vel > self.vel_filter[0] 180 | and obs_vel < self.vel_filter[1] 181 | and obs_height > self.height_filter[0] 182 | and obs_height < self.height_filter[1] 183 | ): 184 | filtered_obstacle_list.append(obs) 185 | 186 | # construct ObstacleArray 187 | if self.tracker_obstacle_pub.get_subscription_count() > 0: 188 | obstacle_array = ObstacleArray() 189 | obstacle_array.header = msg.header 190 | track_list = [] 191 | 192 | for obs in filtered_obstacle_list: 193 | track_list.append(obs.msg) 194 | 195 | obstacle_array.obstacles = track_list 196 | self.tracker_obstacle_pub.publish(obstacle_array) 197 | 198 | # rviz visualization 199 | if self.tracker_marker_pub.get_subscription_count() > 0: 200 | marker_array = MarkerArray() 201 | marker_list = [] 202 | # add current active obstacles 203 | for obs in filtered_obstacle_list: 204 | obstacle_uuid = uuid.UUID(bytes=bytes(obs.msg.uuid.uuid)) 205 | (r, g, b) = colorsys.hsv_to_rgb( 206 | obstacle_uuid.int % 360 / 360.0, 1.0, 1.0 207 | ) # encode id with rgb color 208 | 209 | # make a cube 210 | marker = Marker() 211 | marker.header = msg.header 212 | marker.ns = str(obstacle_uuid) 213 | marker.id = 0 214 | marker.type = 1 # CUBE 215 | marker.action = 0 216 | marker.color.a = 0.5 217 | marker.color.r = r 218 | marker.color.g = g 219 | marker.color.b = b 220 | marker.pose.position = obs.msg.position 221 | angle = np.arctan2(obs.msg.velocity.y, obs.msg.velocity.x) 222 | if self.infer_orientation_from_velocity: 223 | marker.pose.orientation.z = float(np.sin(angle / 2)) 224 | marker.pose.orientation.w = float(np.cos(angle / 2)) 225 | else: 226 | marker.pose.orientation.z = 0.0 227 | marker.pose.orientation.w = 1.0 228 | 229 | marker.scale = obs.msg.size 230 | marker_list.append(marker) 231 | # make an arrow 232 | arrow = Marker() 233 | arrow.header = msg.header 234 | arrow.ns = str(obstacle_uuid) 235 | arrow.id = 1 236 | arrow.type = 0 237 | arrow.action = 0 238 | arrow.color.a = 1.0 239 | arrow.color.r = r 240 | arrow.color.g = g 241 | arrow.color.b = b 242 | arrow.pose.position = obs.msg.position 243 | arrow.pose.orientation.z = float(np.sin(angle / 2)) 244 | arrow.pose.orientation.w = float(np.cos(angle / 2)) 245 | arrow.scale.x = np.linalg.norm( 246 | [obs.msg.velocity.x, obs.msg.velocity.y, obs.msg.velocity.z] 247 | ) 248 | arrow.scale.y = 0.05 249 | arrow.scale.z = 0.05 250 | marker_list.append(arrow) 251 | 252 | # add dead obstacles to delete in rviz 253 | for dead_uuid in dead_object_list: 254 | marker = Marker() 255 | marker.header = msg.header 256 | marker.ns = str(dead_uuid) 257 | marker.id = 0 258 | marker.action = 2 # delete 259 | arrow = Marker() 260 | arrow.header = msg.header 261 | arrow.ns = str(dead_uuid) 262 | arrow.id = 1 263 | arrow.action = 2 264 | marker_list.append(marker) 265 | marker_list.append(arrow) 266 | marker_array.markers = marker_list 267 | self.tracker_marker_pub.publish(marker_array) 268 | 269 | def birth(self, det_ind, num_of_detect, detections): 270 | """generate new ObstacleClass for detections that do not match any in current obstacle list""" 271 | for det in range(num_of_detect): 272 | if det not in det_ind: 273 | obstacle = ObstacleClass( 274 | detections[det], 275 | self.top_down, 276 | self.measurement_noise_cov, 277 | self.error_cov_post, 278 | self.process_noise_cov, 279 | ) 280 | self.obstacle_list.append(obstacle) 281 | 282 | def death(self, obj_ind, num_of_obstacle): 283 | """count obstacles' missing frames and delete when reach threshold""" 284 | new_object_list = [] 285 | dead_object_list = [] 286 | # for previous obstacles 287 | for obs in range(num_of_obstacle): 288 | if obs not in obj_ind: 289 | self.obstacle_list[obs].dying += 1 290 | else: 291 | self.obstacle_list[obs].dying = 0 292 | 293 | if self.obstacle_list[obs].dying < self.death_threshold: 294 | new_object_list.append(self.obstacle_list[obs]) 295 | else: 296 | obstacle_uuid = uuid.UUID( 297 | bytes=bytes(self.obstacle_list[obs].msg.uuid.uuid) 298 | ) 299 | dead_object_list.append(obstacle_uuid) 300 | 301 | # add newly born obstacles 302 | for obs in range(num_of_obstacle, len(self.obstacle_list)): 303 | new_object_list.append(self.obstacle_list[obs]) 304 | 305 | self.obstacle_list = new_object_list 306 | return dead_object_list 307 | 308 | 309 | def main(args=None): 310 | rclpy.init(args=args) 311 | 312 | node = KFHungarianTracker() 313 | node.get_logger().info("start spining tracker node...") 314 | 315 | rclpy.spin(node) 316 | 317 | rclpy.shutdown() 318 | 319 | 320 | if __name__ == "__main__": 321 | main() 322 | -------------------------------------------------------------------------------- /kf_hungarian_tracker/kf_hungarian_tracker/obstacle_class.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 3 | import uuid 4 | from unique_identifier_msgs.msg import UUID 5 | 6 | 7 | class ObstacleClass: 8 | """wrap a kalman filter and extra information for one single obstacle 9 | 10 | State space is 3D (x, y, z) by default, if you want to work on 2D (for example top-down view), you can simply make z a constant value and independent of x, y. 11 | 12 | Arrtibutes: 13 | position: 3d position of center point, numpy array with shape (3, 1) 14 | velocity: 3d velocity of center point, numpy array with shape (3, 1) 15 | kalman: cv2.KalmanFilter 16 | dying: count missing frames for this obstacle, if reach threshold, delete this obstacle 17 | """ 18 | 19 | def __init__( 20 | self, 21 | obstacle_msg, 22 | top_down, 23 | measurement_noise_cov, 24 | error_cov_post, 25 | process_noise_cov, 26 | ): 27 | """Initialize with an Obstacle msg and an assigned id""" 28 | self.msg = obstacle_msg 29 | 30 | uuid_msg = UUID() 31 | uuid_msg.uuid = list(uuid.uuid4().bytes) 32 | self.msg.uuid = uuid_msg 33 | 34 | position = np.array( 35 | [ 36 | [ 37 | obstacle_msg.position.x, 38 | obstacle_msg.position.y, 39 | obstacle_msg.position.z, 40 | ] 41 | ] 42 | ).T # shape 3*1 43 | velocity = np.array( 44 | [ 45 | [ 46 | obstacle_msg.velocity.x, 47 | obstacle_msg.velocity.y, 48 | obstacle_msg.velocity.z, 49 | ] 50 | ] 51 | ).T 52 | 53 | # check aganist state space dimension, top_down or not 54 | if top_down: 55 | measurement_noise_cov[2] = 0.0 56 | error_cov_post[2] = 0.0 57 | error_cov_post[5] = 0.0 58 | process_noise_cov[2] = 0.0 59 | 60 | # setup kalman filter 61 | self.kalman = cv2.KalmanFilter( 62 | 6, 3 63 | ) # 3d by default, 6d state space and 3d observation space 64 | self.kalman.measurementMatrix = np.array( 65 | [[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0]], np.float32 66 | ) 67 | self.kalman.measurementNoiseCov = np.diag(measurement_noise_cov).astype( 68 | np.float32 69 | ) 70 | self.kalman.statePost = np.concatenate([position, velocity]).astype(np.float32) 71 | self.kalman.errorCovPost = np.diag(error_cov_post).astype(np.float32) 72 | 73 | self.dying = 0 74 | self.top_down = top_down 75 | self.process_noise_cov = process_noise_cov 76 | 77 | def predict(self, dt): 78 | """update F and Q matrices, call KalmanFilter.predict and store position and velocity""" 79 | 80 | # construct new transition matrix 81 | """ 82 | F = 1, 0, 0, dt, 0, 0 83 | 0, 1, 0, 0, dt, 0 84 | 0, 0, 1, 0, 0, dt 85 | 0, 0, 0, 1, 0, 0 86 | 0, 0, 0, 0, 1, 0 87 | 0, 0, 0, 0, 0, 1 88 | """ 89 | F = np.eye(6).astype(np.float32) 90 | F[0, 3] = dt 91 | F[1, 4] = dt 92 | F[2, 5] = dt 93 | 94 | # construct new process conv matrix 95 | """assume constant velocity, and obstacle's acceleration has noise in x, y, z direction ax, ay, az 96 | Q = dt4*ax/4, 0, 0, dt3*ax/2, 0, 0 97 | 0, dt4*ay/4, 0, 0, dt3*ay/2, 0 98 | 0, 0, dt4*az/4, 0, 0, dt3*az/2 99 | dt3*ax/2, 0, 0, dt2*ax, 0, 0 100 | 0, dt3*ay/2, 0, 0, dt2*ay, 0 101 | 0, 0, dt3*az/2, 0, 0, dt2*az 102 | """ 103 | dt2 = dt**2 104 | dt3 = dt * dt2 105 | dt4 = dt2**2 106 | 107 | Q = np.array([[dt4*self.process_noise_cov[0]/4, 0, 0, dt3*self.process_noise_cov[0]/2, 0, 0], 108 | [0, dt4*self.process_noise_cov[1]/4, 0, 0, dt3*self.process_noise_cov[1]/2, 0], 109 | [0, 0, dt4*self.process_noise_cov[2]/4, 0, 0, dt3*self.process_noise_cov[2]/2], 110 | [dt3*self.process_noise_cov[0]/2, 0, 0, dt2*self.process_noise_cov[0], 0, 0], 111 | [0, dt3*self.process_noise_cov[1]/2, 0, 0, dt2*self.process_noise_cov[1], 0], 112 | [0, 0, dt3*self.process_noise_cov[2]/2, 0, 0, dt2*self.process_noise_cov[2]]]).astype(np.float32) 113 | 114 | self.kalman.transitionMatrix = F 115 | self.kalman.processNoiseCov = Q 116 | self.kalman.predict() 117 | self.msg.position.x = float(self.kalman.statePre[0][0]) 118 | self.msg.position.y = float(self.kalman.statePre[1][0]) 119 | self.msg.position.z = float(self.kalman.statePre[2][0]) 120 | self.msg.velocity.x = float(self.kalman.statePre[3][0]) 121 | self.msg.velocity.y = float(self.kalman.statePre[4][0]) 122 | self.msg.velocity.z = float(self.kalman.statePre[5][0]) 123 | 124 | def correct(self, detect_msg): 125 | """extract position as measurement and update KalmanFilter""" 126 | if self.top_down: 127 | detect_msg.position.z = 0.0 128 | measurement = np.array( 129 | [[detect_msg.position.x, detect_msg.position.y, detect_msg.position.z]] 130 | ).T.astype(np.float32) 131 | self.kalman.correct(measurement) 132 | self.msg.position.x = float(self.kalman.statePost[0][0]) 133 | self.msg.position.y = float(self.kalman.statePost[1][0]) 134 | self.msg.position.z = float(self.kalman.statePost[2][0]) 135 | self.msg.velocity.x = float(self.kalman.statePost[3][0]) 136 | self.msg.velocity.y = float(self.kalman.statePost[4][0]) 137 | self.msg.velocity.z = float(self.kalman.statePost[5][0]) 138 | self.msg.size = detect_msg.size 139 | self.msg.position_covariance = detect_msg.position_covariance 140 | self.msg.velocity_covariance = detect_msg.velocity_covariance 141 | 142 | def distance(self, other_msg): 143 | """measurement distance between two obstacles, dy default it's Euler distance between centers 144 | you can extent the Obstacle msg to include more features like class or uncertainty and include in the distance function""" 145 | position = np.array( 146 | [[self.msg.position.x, self.msg.position.y, self.msg.position.z]] 147 | ).T 148 | other_position = np.array( 149 | [[other_msg.position.x, other_msg.position.y, other_msg.position.z]] 150 | ).T 151 | return np.linalg.norm(position - other_position) 152 | -------------------------------------------------------------------------------- /kf_hungarian_tracker/launch/kf_hungarian.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | import launch 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch_ros.actions import Node 6 | 7 | 8 | def generate_launch_description(): 9 | config = os.path.join( 10 | get_package_share_directory("kf_hungarian_tracker"), 11 | "config", 12 | "kf_hungarian.yaml", 13 | ) 14 | 15 | kf_hungarian_node = Node( 16 | package="kf_hungarian_tracker", 17 | name="kf_hungarian_node", 18 | executable="kf_hungarian_node", 19 | parameters=[config], 20 | ) 21 | 22 | return launch.LaunchDescription([kf_hungarian_node]) 23 | -------------------------------------------------------------------------------- /kf_hungarian_tracker/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | kf_hungarian_tracker 5 | 0.0.1 6 | Use Kalman Filter and Hungarian algorithm to track multiple objects 7 | Steven Macenski 8 | Shengjian Chen 9 | Apache-2.0 10 | 11 | ament_copyright 12 | ament_flake8 13 | ament_pep257 14 | python3-pytest 15 | 16 | launch_ros 17 | 18 | python3-numpy 19 | python3-scipy 20 | nav2_dynamic_msgs 21 | visualization_msgs 22 | rclpy 23 | python3-opencv 24 | tf2_ros 25 | geometry_msgs 26 | tf2_geometry_msgs 27 | 28 | 29 | ament_python 30 | 31 | 32 | -------------------------------------------------------------------------------- /kf_hungarian_tracker/resource/kf_hungarian_tracker: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-navigation/navigation2_dynamic/e1b0d920fa309c7eb072524b00de7ee12e21076e/kf_hungarian_tracker/resource/kf_hungarian_tracker -------------------------------------------------------------------------------- /kf_hungarian_tracker/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script-dir=$base/lib/kf_hungarian_tracker 3 | [install] 4 | install-scripts=$base/lib/kf_hungarian_tracker 5 | [tool:pytest] 6 | testpaths = test 7 | -------------------------------------------------------------------------------- /kf_hungarian_tracker/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | import os, glob 3 | 4 | package_name = 'kf_hungarian_tracker' 5 | 6 | setup( 7 | name=package_name, 8 | version='0.0.1', 9 | packages=[package_name], 10 | data_files=[ 11 | ('share/ament_index/resource_index/packages', 12 | ['resource/' + package_name]), 13 | ('share/' + package_name, ['package.xml']), 14 | ('share/' + package_name + '/config', glob.glob('config/*.yaml')), 15 | ('share/' + package_name + '/launch', glob.glob('launch/*.launch.py')) 16 | ], 17 | install_requires=['setuptools'], 18 | zip_safe=True, 19 | maintainer='Shengjian Chen, Steven Macenski, Alexander Yuen', 20 | maintainer_email='csj15thu@gmail.com, stevenmacenski@gmail.com, alex@polymathrobotics.com', 21 | description='Use Kalman Filter and Hungarian algorithm to track multiple objects', 22 | license='Apache-2.0', 23 | tests_require=['pytest'], 24 | test_suite='test', # Name of the test suite 25 | entry_points={ 26 | 'console_scripts': [ 27 | 'kf_hungarian_node = kf_hungarian_tracker.kf_hungarian_node:main' 28 | ], 29 | }, 30 | ) 31 | -------------------------------------------------------------------------------- /kf_hungarian_tracker/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.copyright 20 | @pytest.mark.linter 21 | def test_copyright(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /kf_hungarian_tracker/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /kf_hungarian_tracker/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /nav2_dynamic_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(nav2_dynamic_msgs) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | # uncomment the following section in order to fill in 21 | # further dependencies manually. 22 | # find_package( REQUIRED) 23 | find_package(rclcpp REQUIRED) 24 | find_package(rosidl_default_generators REQUIRED) 25 | find_package(builtin_interfaces REQUIRED) 26 | find_package(std_msgs REQUIRED) 27 | find_package(sensor_msgs REQUIRED) 28 | find_package(unique_identifier_msgs REQUIRED) 29 | 30 | rosidl_generate_interfaces(${PROJECT_NAME} 31 | "msg/Obstacle.msg" 32 | "msg/ObstacleArray.msg" 33 | DEPENDENCIES std_msgs sensor_msgs unique_identifier_msgs 34 | ) 35 | 36 | if(BUILD_TESTING) 37 | find_package(ament_lint_auto REQUIRED) 38 | # the following line skips the linter which checks for copyrights 39 | # uncomment the line when a copyright and license is not present in all source files 40 | #set(ament_cmake_copyright_FOUND TRUE) 41 | # the following line skips cpplint (only works in a git repo) 42 | # uncomment the line when this package is not in a git repo 43 | #set(ament_cmake_cpplint_FOUND TRUE) 44 | ament_lint_auto_find_test_dependencies() 45 | endif() 46 | 47 | ament_package() 48 | -------------------------------------------------------------------------------- /nav2_dynamic_msgs/msg/Obstacle.msg: -------------------------------------------------------------------------------- 1 | unique_identifier_msgs/UUID uuid 2 | float32 score # detection confidence 3 | geometry_msgs/Point position # center 4 | geometry_msgs/Vector3 velocity 5 | geometry_msgs/Vector3 size 6 | float64[9] position_covariance 7 | float64[9] velocity_covariance 8 | -------------------------------------------------------------------------------- /nav2_dynamic_msgs/msg/ObstacleArray.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | Obstacle[] obstacles 3 | -------------------------------------------------------------------------------- /nav2_dynamic_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav2_dynamic_msgs 5 | 0.0.1 6 | Msg and srv for nav2_dynamic_obstacle 7 | Steven Macenski 8 | Shengjian Chen 9 | Apache-2.0 10 | 11 | ament_cmake 12 | 13 | rosidl_default_generators 14 | 15 | rosidl_interface_packages 16 | 17 | sensor_msgs 18 | std_msgs 19 | unique_identifier_msgs 20 | 21 | ament_lint_auto 22 | ament_lint_common 23 | 24 | 25 | ament_cmake 26 | 27 | 28 | --------------------------------------------------------------------------------