Skip to content

Commit

Permalink
feat: revise object time range logic
Browse files Browse the repository at this point in the history
  • Loading branch information
@technolojin
technolojin committed May 8, 2024
1 parent fadf553 commit 68dff92
Show file tree
Hide file tree
Showing 4 changed files with 73 additions and 50 deletions.
24 changes: 12 additions & 12 deletions perception/multi_object_tracker/config/input_channels.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,79 +3,79 @@
input_channels:
detected_objects:
topic: "/perception/object_recognition/detection/objects"
expected_frequency: 10.0
expected_interval: 0.1
optional:
name: "detected_objects"
short_name: "all"
# LIDAR - rule-based
lidar_clustering:
topic: "/perception/object_recognition/detection/clustering/objects"
expected_frequency: 10.0
expected_interval: 0.1
optional:
name: "clustering"
short_name: "Lcl"
# LIDAR - DNN
lidar_centerpoint:
topic: "/perception/object_recognition/detection/centerpoint/objects"
expected_frequency: 10.0
expected_interval: 0.1
optional:
name: "centerpoint"
short_name: "Lcp"
lidar_centerpoint_validated:
topic: "/perception/object_recognition/detection/centerpoint/validation/objects"
expected_frequency: 10.0
expected_interval: 0.1
optional:
name: "centerpoint"
short_name: "Lcp"
lidar_apollo:
topic: "/perception/object_recognition/detection/apollo/objects"
expected_frequency: 10.0
expected_interval: 0.1
optional:
name: "apollo"
short_name: "Lap"
lidar_apollo_validated:
topic: "/perception/object_recognition/detection/apollo/validation/objects"
expected_frequency: 10.0
expected_interval: 0.1
optional:
name: "apollo"
short_name: "Lap"
# LIDAR-CAMERA - DNN
lidar_pointpainitng:
topic: "/perception/object_recognition/detection/pointpainting/objects"
expected_frequency: 10.0
expected_interval: 0.1
optional:
name: "pointpainting"
short_name: "Lpp"
lidar_pointpainting_validated:
topic: "/perception/object_recognition/detection/pointpainting/validation/objects"
expected_frequency: 10.0
expected_interval: 0.1
optional:
name: "pointpainting"
short_name: "Lpp"
# CAMERA-LIDAR
camera_lidar_fusion:
topic: "/perception/object_recognition/detection/clustering/camera_lidar_fusion/objects"
expected_frequency: 10.0
expected_interval: 0.1
optional:
name: "camera_lidar_fusion"
short_name: "CLf"
# CAMERA-LIDAR+TRACKER
detection_by_tracker:
topic: "/perception/object_recognition/detection/detection_by_tracker/objects"
expected_frequency: 10.0
expected_interval: 0.1
optional:
name: "detection_by_tracker"
short_name: "dbT"
# RADAR
radar:
topic: "/sensing/radar/detected_objects"
expected_frequency: 30.0
expected_interval: 0.0333
optional:
name: "radar"
short_name: "R"
radar_far:
topic: "/perception/object_recognition/detection/radar/far_objects"
expected_frequency: 30.0
expected_interval: 0.0333
optional:
name: "radar_far"
short_name: "Rf"
17 changes: 13 additions & 4 deletions perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ struct InputChannel
std::string input_topic; // topic name of the detection, e.g. "/detection/lidar"
std::string long_name = "Detected Object"; // full name of the detection
std::string short_name = "DET"; // abbreviation of the name
double expected_rate = 10.0; // [Hz]
double expected_interval = 0.1; // [s]
double expected_latency = 0.2; // [s]
};

Expand Down Expand Up @@ -77,6 +77,11 @@ class InputStream
interval_mean = interval_mean_;
interval_var = interval_var_;
}
void getLatencyStatistics(double & latency_mean, double & latency_var) const
{
latency_mean = latency_mean_;
latency_var = latency_var_;
}
bool getTimestamps(
rclcpp::Time & latest_measurement_time, rclcpp::Time & latest_message_time) const;
rclcpp::Time getLatestMeasurementTime() const { return latest_measurement_time_; }
Expand All @@ -95,7 +100,7 @@ class InputStream
std::function<void(const uint &)> func_trigger_;

bool is_time_initialized_{false};
double expected_rate_;
double expected_interval_;
double latency_mean_;
double latency_var_;
double interval_mean_;
Expand Down Expand Up @@ -133,8 +138,12 @@ class InputManager

std::function<void()> func_trigger_;
uint target_stream_idx_{0};
double target_latency_{0.2};
double target_latency_band_{0.14};
double target_stream_latency_{0.2}; // [s]
double target_stream_latency_std_{0.04}; // [s]
double target_stream_interval_{0.1}; // [s]
double target_stream_interval_std_{0.02}; // [s]
double target_latency_{0.2}; // [s]
double target_latency_band_{1.0}; // [s]
};

} // namespace multi_object_tracker
Expand Down
12 changes: 6 additions & 6 deletions perception/multi_object_tracker/src/multi_object_tracker_core.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,7 +92,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
std::vector<std::string> input_topic_names;
std::vector<std::string> input_names_long;
std::vector<std::string> input_names_short;
std::vector<double> input_expected_rates;
std::vector<double> input_expected_intervals;

if (selected_input_channels.empty()) {
RCLCPP_ERROR(this->get_logger(), "No input topics are specified.");
Expand All @@ -106,10 +106,10 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
input_topic_names.push_back(input_topic_name);

// required parameter, but can set a default value
const double default_expected_rate = 10.0;
const double expected_rate = declare_parameter<double>(
"input_channels." + selected_input_channel + ".expected_rate", default_expected_rate);
input_expected_rates.push_back(expected_rate);
const double default_expected_interval = 0.1; // [s]
const double expected_interval = declare_parameter<double>(
"input_channels." + selected_input_channel + ".expected_interval", default_expected_interval);
input_expected_intervals.push_back(expected_interval);

// optional parameters
const std::string default_name = selected_input_channel;
Expand All @@ -130,7 +130,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
input_channels_[i].input_topic = input_topic_names[i];
input_channels_[i].long_name = input_names_long[i];
input_channels_[i].short_name = input_names_short[i];
input_channels_[i].expected_rate = input_expected_rates[i];
input_channels_[i].expected_interval = input_expected_intervals[i];
}

// Initialize input manager
Expand Down
70 changes: 42 additions & 28 deletions perception/multi_object_tracker/src/processor/input_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,15 +31,15 @@ void InputStream::init(const InputChannel & input_channel)
input_topic_ = input_channel.input_topic;
long_name_ = input_channel.long_name;
short_name_ = input_channel.short_name;
expected_interval_ = input_channel.expected_interval; // [s]

// Initialize queue
objects_que_.clear();

// Initialize latency statistics
expected_rate_ = input_channel.expected_rate; // [Hz]
latency_mean_ = input_channel.expected_latency; // [s]
latency_var_ = 0.0;
interval_mean_ = 1 / expected_rate_;
interval_mean_ = expected_interval_; // [s] (initial value)
interval_var_ = 0.0;

latest_measurement_time_ = node_.now();
Expand Down Expand Up @@ -84,13 +84,11 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim

// Calculate interval, Update interval statistics
if (is_time_initialized_) {
bool is_interval_regular = true;
const double interval = (now - latest_message_time_).seconds();
// check if it is outlier
if (interval > 1.5 * interval_mean_ || interval < 0.5 * interval_mean_) {
// no update for the time statistics
is_interval_regular = false;
}
// Check if the interval is regular
// The interval is considered regular if it is within 0.5 and 1.5 times the expected interval
bool is_interval_regular =
interval > 0.5 * expected_interval_ && interval < 1.5 * expected_interval_;

if (is_interval_regular) {
interval_mean_ = (1.0 - gain) * interval_mean_ + gain * interval;
Expand Down Expand Up @@ -189,17 +187,33 @@ void InputManager::getObjectTimeInterval(
const rclcpp::Time & now, rclcpp::Time & object_latest_time,
rclcpp::Time & object_oldest_time) const
{
object_latest_time = now - rclcpp::Duration::from_seconds(target_latency_);
object_oldest_time = now - rclcpp::Duration::from_seconds(target_latency_ + target_latency_band_);

// try to include the latest message of the target stream
object_latest_time =
now - rclcpp::Duration::from_seconds(
target_stream_latency_ -
0.1 * target_stream_latency_std_); // object_latest_time with 0.1 sigma safety margin
// check the target stream can be included in the object time interval
if (input_streams_.at(target_stream_idx_)->isTimeInitialized()) {
rclcpp::Time latest_measurement_time =
input_streams_.at(target_stream_idx_)->getLatestMeasurementTime();
// if the object_latest_time is newer than the next expected message time, set it older
// than the next expected message time
rclcpp::Time next_expected_message_time =
latest_measurement_time +
rclcpp::Duration::from_seconds(
target_stream_interval_ -
1.0 *
target_stream_interval_std_); // next expected message time with 1 sigma safety margin
object_latest_time = object_latest_time > next_expected_message_time
? next_expected_message_time
: object_latest_time;

// if the object_latest_time is older than the latest measurement time, set it as the latest
// object time
object_latest_time =
object_latest_time > latest_measurement_time ? object_latest_time : latest_measurement_time;
object_latest_time < latest_measurement_time ? latest_measurement_time : object_latest_time;
}

object_oldest_time = object_latest_time - rclcpp::Duration::from_seconds(1.0);
// if the object_oldest_time is older than the latest object time, set it to the latest object
// time
object_oldest_time =
Expand All @@ -209,21 +223,24 @@ void InputManager::getObjectTimeInterval(
void InputManager::optimizeTimings()
{
double max_latency_mean = 0.0;
uint stream_selected_idx = 0;
double selected_idx_latency_std = 0.0;
double selected_idx_interval = 0.0;
uint selected_stream_idx = 0;
double selected_stream_latency_std = 0.1;
double selected_stream_interval = 0.1;
double selected_stream_interval_std = 0.01;

{
// ANALYSIS: Get the streams statistics
// select the stream that has the maximum latency
double latency_mean, latency_var, interval_mean, interval_var;
for (const auto & input_stream : input_streams_) {
if (!input_stream->isTimeInitialized()) continue;
input_stream->getTimeStatistics(latency_mean, latency_var, interval_mean, interval_var);
if (latency_mean > max_latency_mean) {
max_latency_mean = latency_mean;
selected_idx_latency_std = std::sqrt(latency_var);
stream_selected_idx = input_stream->getIndex();
selected_idx_interval = interval_mean;
selected_stream_idx = input_stream->getIndex();
selected_stream_latency_std = std::sqrt(latency_var);
selected_stream_interval = interval_mean;
selected_stream_interval_std = std::sqrt(interval_var);
}

/* DEBUG */
Expand All @@ -244,15 +261,12 @@ void InputManager::optimizeTimings()

// Set the target stream index, which has the maximum latency
// trigger will be called next time
target_stream_idx_ = stream_selected_idx;
target_latency_ = max_latency_mean - selected_idx_latency_std;
target_latency_band_ = 2 * selected_idx_latency_std + selected_idx_interval;

/* DEBUG */
RCLCPP_INFO(
node_.get_logger(), "InputManager::getObjects Target stream: %s, target latency: %f, band: %f",
input_streams_.at(target_stream_idx_)->getLongName().c_str(), target_latency_,
target_latency_band_);
// if no stream is initialized, the target stream index will be 0 and wait for the initialization
target_stream_idx_ = selected_stream_idx;
target_stream_latency_ = max_latency_mean;
target_stream_latency_std_ = selected_stream_latency_std;
target_stream_interval_ = selected_stream_interval;
target_stream_interval_std_ = selected_stream_interval_std;
}

bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_list)
Expand Down

0 comments on commit 68dff92

Please sign in to comment.