Skip to content

Commit b6e9a6b

Browse files
committed
chore: add comments, refactoring
Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
1 parent 27ac444 commit b6e9a6b

File tree

2 files changed

+42
-35
lines changed

2 files changed

+42
-35
lines changed

perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp

+8-6
Original file line numberDiff line numberDiff line change
@@ -117,17 +117,13 @@ class InputManager
117117
{
118118
public:
119119
explicit InputManager(rclcpp::Node & node);
120-
121120
void init(const std::vector<InputChannel> & input_channels);
122-
void setTriggerFunction(std::function<void()> func_trigger) { func_trigger_ = func_trigger; }
123121

122+
void setTriggerFunction(std::function<void()> func_trigger) { func_trigger_ = func_trigger; }
124123
void onTrigger(const uint & index) const;
125124

126-
void getObjectTimeInterval(
127-
const rclcpp::Time & now, rclcpp::Time & object_latest_time,
128-
rclcpp::Time & object_oldest_time) const;
129-
void optimizeTimings();
130125
bool getObjects(const rclcpp::Time & now, ObjectsList & objects_list);
126+
131127
bool isChannelSpawnEnabled(const uint & index) const
132128
{
133129
return input_streams_[index]->isSpawnEnabled();
@@ -151,6 +147,12 @@ class InputManager
151147
double target_stream_interval_std_{0.02}; // [s]
152148
double target_latency_{0.2}; // [s]
153149
double target_latency_band_{1.0}; // [s]
150+
151+
private:
152+
void getObjectTimeInterval(
153+
const rclcpp::Time & now, rclcpp::Time & object_latest_time,
154+
rclcpp::Time & object_oldest_time) const;
155+
void optimizeTimings();
154156
};
155157

156158
} // namespace multi_object_tracker

perception/multi_object_tracker/src/processor/input_manager.cpp

+34-29
Original file line numberDiff line numberDiff line change
@@ -127,15 +127,16 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim
127127
// Update time
128128
latest_message_time_ = now;
129129
constexpr double delay_threshold = 3.0; // [s]
130-
if (std::abs((latest_measurement_time_ - objects_time).seconds()) > delay_threshold) {
131-
// Reset the latest measurement time if the time difference is too large
130+
if (objects_time < latest_measurement_time_ - rclcpp::Duration::from_seconds(delay_threshold)) {
131+
// If the given object time is older than the latest measurement time by more than the
132+
// threshold, the system time may have been reset. Reset the latest measurement time
132133
latest_measurement_time_ = objects_time;
133134
RCLCPP_WARN(
134135
node_.get_logger(),
135136
"InputManager::updateTimingStatus %s: Resetting the latest measurement time to %f",
136137
long_name_.c_str(), objects_time.seconds());
137138
} else {
138-
// Aware reversed message arrival
139+
// Update only if the object time is newer than the latest measurement time
139140
latest_measurement_time_ =
140141
latest_measurement_time_ < objects_time ? objects_time : latest_measurement_time_;
141142
}
@@ -237,36 +238,36 @@ void InputManager::getObjectTimeInterval(
237238
const rclcpp::Time & now, rclcpp::Time & object_latest_time,
238239
rclcpp::Time & object_oldest_time) const
239240
{
241+
// Set the object time interval
242+
243+
// 1. object_latest_time
244+
// The object_latest_time is the current time minus the target stream latency
240245
object_latest_time =
241-
now - rclcpp::Duration::from_seconds(
242-
target_stream_latency_ -
243-
0.1 * target_stream_latency_std_); // object_latest_time with 0.1 sigma safety margin
246+
now - rclcpp::Duration::from_seconds(target_stream_latency_ - 0.1 * target_stream_latency_std_);
247+
244248
// check the target stream can be included in the object time interval
245249
if (input_streams_.at(target_stream_idx_)->isTimeInitialized()) {
246-
rclcpp::Time latest_measurement_time =
250+
const rclcpp::Time latest_measurement_time =
247251
input_streams_.at(target_stream_idx_)->getLatestMeasurementTime();
248-
249252
// if the object_latest_time is older than the latest measurement time, set it to the latest
250253
// object time
251254
object_latest_time =
252255
object_latest_time < latest_measurement_time ? latest_measurement_time : object_latest_time;
253256
}
254257

255-
object_oldest_time = object_latest_time - rclcpp::Duration::from_seconds(1.0);
256-
// if the object_oldest_time is older than the latest object time, set it to the latest object
257-
// time
258-
object_oldest_time = object_oldest_time < latest_exported_object_time_
259-
? latest_exported_object_time_
260-
: object_oldest_time;
261-
262-
// check the object time interval is valid
263-
if (object_oldest_time > object_latest_time) {
264-
RCLCPP_WARN(
265-
node_.get_logger(),
266-
"InputManager::getObjectTimeInterval Invalid object time interval, object_latest_time: %f, "
267-
"object_oldest_time: %f",
268-
(now - object_latest_time).seconds(), (now - object_oldest_time).seconds());
269-
object_oldest_time = object_latest_time - rclcpp::Duration::from_seconds(1.0);
258+
// 2. object_oldest_time
259+
// The default object_oldest_time is to have a 1-second time interval
260+
const rclcpp::Time object_oldest_time_default =
261+
object_latest_time - rclcpp::Duration::from_seconds(1.0);
262+
if (latest_exported_object_time_ < object_oldest_time_default) {
263+
// if the latest exported object time is too old, set to the default
264+
object_oldest_time = object_oldest_time_default;
265+
} else if (latest_exported_object_time_ > object_latest_time) {
266+
// if the latest exported object time is newer than the object_latest_time, set to the default
267+
object_oldest_time = object_oldest_time_default;
268+
} else {
269+
// The object_oldest_time is the latest exported object time
270+
object_oldest_time = latest_exported_object_time_;
270271
}
271272
}
272273

@@ -316,7 +317,8 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li
316317
objects_list.clear();
317318

318319
// Get the time interval for the objects
319-
rclcpp::Time object_latest_time, object_oldest_time;
320+
rclcpp::Time object_latest_time;
321+
rclcpp::Time object_oldest_time;
320322
getObjectTimeInterval(now, object_latest_time, object_oldest_time);
321323

322324
// Optimize the target stream, latency, and its band
@@ -343,16 +345,19 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li
343345
if (is_any_object) {
344346
latest_exported_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp);
345347
} else {
346-
// check time jump
348+
// check time jump back
347349
if (now < latest_exported_object_time_) {
348350
RCLCPP_WARN(
349351
node_.get_logger(),
350-
"InputManager::getObjects Time jump detected, now: %f, latest_exported_object_time_: %f",
352+
"InputManager::getObjects Detected jump back in time, now: %f, "
353+
"latest_exported_object_time_: %f",
351354
now.seconds(), latest_exported_object_time_.seconds());
352-
latest_exported_object_time_ =
353-
now - rclcpp::Duration::from_seconds(
354-
3.0); // reset the latest exported object time to 3 seconds ago
355+
// reset the latest exported object time to 3 seconds ago,
356+
const rclcpp::Time latest_exported_object_time_default =
357+
now - rclcpp::Duration::from_seconds(3.0);
358+
latest_exported_object_time_ = latest_exported_object_time_default;
355359
} else {
360+
// No objects in the object list, no update for the latest exported object time
356361
RCLCPP_DEBUG(
357362
node_.get_logger(),
358363
"InputManager::getObjects No objects in the object list, object time band from %f to %f",

0 commit comments

Comments
 (0)