|
19 | 19 | #include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
|
20 | 20 | #include <tier4_autoware_utils/geometry/geometry.hpp>
|
21 | 21 | #include <tier4_autoware_utils/ros/marker_helper.hpp>
|
| 22 | +#include <tier4_autoware_utils/ros/update_param.hpp> |
22 | 23 |
|
23 | 24 | #include <boost/geometry/algorithms/convex_hull.hpp>
|
24 | 25 | #include <boost/geometry/algorithms/within.hpp>
|
|
34 | 35 | #include <pcl/segmentation/extract_clusters.h>
|
35 | 36 | #include <pcl/surface/convex_hull.h>
|
36 | 37 | #include <tf2/utils.h>
|
37 |
| - |
38 | 38 | #ifdef ROS_DISTRO_GALACTIC
|
39 | 39 | #include <tf2_eigen/tf2_eigen.h>
|
40 | 40 | #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
@@ -171,12 +171,59 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
|
171 | 171 | collision_data_keeper_.setTimeout(collision_keeping_sec, previous_obstacle_keep_time);
|
172 | 172 | }
|
173 | 173 |
|
| 174 | + // Parameter Callback |
| 175 | + set_param_res_ = |
| 176 | + add_on_set_parameters_callback(std::bind(&AEB::onParameter, this, std::placeholders::_1)); |
| 177 | + |
174 | 178 | // start time
|
175 | 179 | const double aeb_hz = declare_parameter<double>("aeb_hz");
|
176 | 180 | const auto period_ns = rclcpp::Rate(aeb_hz).period();
|
177 | 181 | timer_ = rclcpp::create_timer(this, this->get_clock(), period_ns, std::bind(&AEB::onTimer, this));
|
178 | 182 | }
|
179 | 183 |
|
| 184 | +rcl_interfaces::msg::SetParametersResult AEB::onParameter( |
| 185 | + const std::vector<rclcpp::Parameter> & parameters) |
| 186 | +{ |
| 187 | + using tier4_autoware_utils::updateParam; |
| 188 | + updateParam<bool>(parameters, "publish_debug_pointcloud", publish_debug_pointcloud_); |
| 189 | + updateParam<bool>(parameters, "use_predicted_trajectory", use_predicted_trajectory_); |
| 190 | + updateParam<bool>(parameters, "use_imu_path", use_imu_path_); |
| 191 | + updateParam<double>(parameters, "path_footprint_extra_margin", path_footprint_extra_margin_); |
| 192 | + updateParam<double>(parameters, "detection_range_min_height", detection_range_min_height_); |
| 193 | + updateParam<double>( |
| 194 | + parameters, "detection_range_max_height_margin", detection_range_max_height_margin_); |
| 195 | + updateParam<double>(parameters, "voxel_grid_x", voxel_grid_x_); |
| 196 | + updateParam<double>(parameters, "voxel_grid_y", voxel_grid_y_); |
| 197 | + updateParam<double>(parameters, "voxel_grid_z", voxel_grid_z_); |
| 198 | + updateParam<double>(parameters, "min_generated_path_length", min_generated_path_length_); |
| 199 | + updateParam<double>(parameters, "expand_width", expand_width_); |
| 200 | + updateParam<double>(parameters, "longitudinal_offset", longitudinal_offset_); |
| 201 | + updateParam<double>(parameters, "t_response", t_response_); |
| 202 | + updateParam<double>(parameters, "a_ego_min", a_ego_min_); |
| 203 | + updateParam<double>(parameters, "a_obj_min", a_obj_min_); |
| 204 | + |
| 205 | + updateParam<double>(parameters, "cluster_tolerance", cluster_tolerance_); |
| 206 | + updateParam<int>(parameters, "minimum_cluster_size", minimum_cluster_size_); |
| 207 | + updateParam<int>(parameters, "maximum_cluster_size", maximum_cluster_size_); |
| 208 | + |
| 209 | + updateParam<double>(parameters, "imu_prediction_time_horizon", imu_prediction_time_horizon_); |
| 210 | + updateParam<double>(parameters, "imu_prediction_time_interval", imu_prediction_time_interval_); |
| 211 | + updateParam<double>(parameters, "mpc_prediction_time_horizon", mpc_prediction_time_horizon_); |
| 212 | + updateParam<double>(parameters, "mpc_prediction_time_interval", mpc_prediction_time_interval_); |
| 213 | + |
| 214 | + { // Object history data keeper setup |
| 215 | + auto [previous_obstacle_keep_time, collision_keeping_sec] = collision_data_keeper_.getTimeout(); |
| 216 | + updateParam<double>(parameters, "previous_obstacle_keep_time", previous_obstacle_keep_time); |
| 217 | + updateParam<double>(parameters, "collision_keeping_sec", collision_keeping_sec); |
| 218 | + collision_data_keeper_.setTimeout(collision_keeping_sec, previous_obstacle_keep_time); |
| 219 | + } |
| 220 | + |
| 221 | + rcl_interfaces::msg::SetParametersResult result; |
| 222 | + result.successful = true; |
| 223 | + result.reason = "success"; |
| 224 | + return result; |
| 225 | +} |
| 226 | + |
180 | 227 | void AEB::onTimer()
|
181 | 228 | {
|
182 | 229 | updater_.force_update();
|
|
0 commit comments