diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model_base.hpp new file mode 100644 index 0000000000000..bb075f59f20cf --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model_base.hpp @@ -0,0 +1,169 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Taekjin Lee +// + +#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ +#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ + +#include + +namespace +{ + constexpr double const_g = 9.81; + double deg2rad(const double deg) + { + return deg * M_PI / 180.0; + } +} // namespace + +namespace object_model +{ + +enum class ObjectModelType +{ + NORMAL_VEHICLE, + BIG_VEHICLE, + BICYCLE, + PEDESTRIAN, + STATIC, + UNKNOWN +}; +struct ObjectSize +{ + double length; // [m] + double width; // [m] + double height; // [m] +}; +struct ObjectSizeLimit +{ + double length_min; // [m] + double length_max; // [m] + double width_min; // [m] + double width_max; // [m] + double height_min; // [m] + double height_max; // [m] +}; +struct MotionProcessNoise +{ + double acc_long; // [m/s^2] + double acc_lat; // [m/s^2] + double yaw_rate_min; // [rad/s] + double yaw_rate_max; // [rad/s] + double slip_rate_min; + double slip_rate_max; + +}; +struct MotionProcessLimit +{ + double slip_angle_max; + double acc_long_max; + double acc_lat_max; + double vel_long_max; +}; +struct StateCovariance +{ + double pos_x; // [m^2] + double pos_y; // [m^2] + double yaw; // [rad^2] + double yaw_rate; // [rad^2/s^2] + double acc_long; // [m^2/s^4] + double acc_lat; // [m^2/s^4] + double vel_long; // [m^2/s^2] + double vel_lat; // [m^2/s^2] +}; +struct BicycleModelState +{ + double slip_angle; // [rad^2] + double slip_rate; // [rad^2/s^2] + double wheel_pos_ratio_front; // [-] + double wheel_pos_ratio_rear; // [-] + double wheel_pos_front_min; // [m] + double wheel_pos_rear_min; // [m] +}; + +class ObjectModel +{ +public: + ObjectSize size; + ObjectSizeLimit size_limit; + MotionProcessNoise process_noise; + MotionProcessLimit process_limit; + StateCovariance initial_covariance; + StateCovariance state_covariance; + BicycleModelState bicycle_model_state; + + ObjectModel(const ObjectModelType & type) + { + switch (type) + { + case ObjectModelType::NORMAL_VEHICLE: + size.length = 3.0; + size.width = 2.0; + size.height = 1.8; + size_limit.length_min = 2.0; + size_limit.length_max = 5.0; + size_limit.width_min = 1.5; + size_limit.width_max = 3.0; + size_limit.height_min = 1.5; + size_limit.height_max = 2.0; + + process_noise.acc_long = const_g * 0.35; + process_noise.acc_lat = const_g * 0.15; + process_noise.yaw_rate_min = 1.5; + process_noise.yaw_rate_max = 15.0; + process_noise.slip_rate_min = 0.3; + process_noise.slip_rate_max = 10.0; + + process_limit.slip_angle_max = 30; + process_limit.acc_long_max = 2.0; + process_limit.acc_lat_max = 2.0; + process_limit.vel_long_max = 30.0; + + // initial covariance + initial_covariance.pos_x = 0.5; + initial_covariance.pos_y = 0.4; + initial_covariance.yaw = deg2rad(20.0); + initial_covariance.yaw_rate = deg2rad(10.0); + initial_covariance.acc_long = 1.0; + initial_covariance.acc_lat = 1.0; + initial_covariance.vel_long = 1.0; + initial_covariance.vel_lat = 1.0; + + // noise model + state_covariance.pos_x = 0.5; + state_covariance.pos_y = 0.4; + state_covariance.yaw = 0.1; + state_covariance.yaw_rate = 0.1; + state_covariance.acc_long = 0.1; + state_covariance.acc_lat = 0.1; + state_covariance.vel_long = 0.1; + state_covariance.vel_lat = 0.1; + break; + } + } + virtual ~ObjectModel() = default; +}; + + + +ObjectModel normal_vehicle(ObjectModelType::NORMAL_VEHICLE); + + + +} // namespace object_model + +#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ \ No newline at end of file