├── .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 |
--------------------------------------------------------------------------------