Skip to content

Commit 04faf5e

Browse files
feat(autonomous_emergency_braking): add param update support for AEB (#6956)
add param update support for AEB Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
1 parent 35d39e7 commit 04faf5e

File tree

2 files changed

+57
-1
lines changed
  • control/autonomous_emergency_braking

2 files changed

+57
-1
lines changed

control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp

+9
Original file line numberDiff line numberDiff line change
@@ -87,6 +87,11 @@ class CollisionDataKeeper
8787
previous_obstacle_keep_time_ = previous_obstacle_keep_time;
8888
}
8989

90+
std::pair<double, double> getTimeout()
91+
{
92+
return {collision_keep_time_, previous_obstacle_keep_time_};
93+
}
94+
9095
bool checkObjectDataExpired(std::optional<ObjectData> & data, const double timeout)
9196
{
9297
if (!data.has_value()) return true;
@@ -246,6 +251,8 @@ class AEB : public rclcpp::Node
246251
void onTimer();
247252
void onPredictedTrajectory(const Trajectory::ConstSharedPtr input_msg);
248253
void onAutowareState(const AutowareState::ConstSharedPtr input_msg);
254+
rcl_interfaces::msg::SetParametersResult onParameter(
255+
const std::vector<rclcpp::Parameter> & parameters);
249256

250257
bool isDataReady();
251258

@@ -321,6 +328,8 @@ class AEB : public rclcpp::Node
321328
double mpc_prediction_time_horizon_;
322329
double mpc_prediction_time_interval_;
323330
CollisionDataKeeper collision_data_keeper_;
331+
// Parameter callback
332+
OnSetParametersCallbackHandle::SharedPtr set_param_res_;
324333
};
325334
} // namespace autoware::motion::control::autonomous_emergency_braking
326335

control/autonomous_emergency_braking/src/node.cpp

+48-1
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
2020
#include <tier4_autoware_utils/geometry/geometry.hpp>
2121
#include <tier4_autoware_utils/ros/marker_helper.hpp>
22+
#include <tier4_autoware_utils/ros/update_param.hpp>
2223

2324
#include <boost/geometry/algorithms/convex_hull.hpp>
2425
#include <boost/geometry/algorithms/within.hpp>
@@ -34,7 +35,6 @@
3435
#include <pcl/segmentation/extract_clusters.h>
3536
#include <pcl/surface/convex_hull.h>
3637
#include <tf2/utils.h>
37-
3838
#ifdef ROS_DISTRO_GALACTIC
3939
#include <tf2_eigen/tf2_eigen.h>
4040
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
@@ -171,12 +171,59 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
171171
collision_data_keeper_.setTimeout(collision_keeping_sec, previous_obstacle_keep_time);
172172
}
173173

174+
// Parameter Callback
175+
set_param_res_ =
176+
add_on_set_parameters_callback(std::bind(&AEB::onParameter, this, std::placeholders::_1));
177+
174178
// start time
175179
const double aeb_hz = declare_parameter<double>("aeb_hz");
176180
const auto period_ns = rclcpp::Rate(aeb_hz).period();
177181
timer_ = rclcpp::create_timer(this, this->get_clock(), period_ns, std::bind(&AEB::onTimer, this));
178182
}
179183

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+
180227
void AEB::onTimer()
181228
{
182229
updater_.force_update();

0 commit comments

Comments
 (0)