Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(autonomous_emergency_braking): add param update support for AEB #6956

Merged
merged 1 commit into from
May 10, 2024
Merged

feat(autonomous_emergency_braking): add param update support for AEB #6956

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 9 additions & 0 deletions control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,6 +87,11 @@ class CollisionDataKeeper
previous_obstacle_keep_time_ = previous_obstacle_keep_time;
}

std::pair<double, double> getTimeout()
{
return {collision_keep_time_, previous_obstacle_keep_time_};
}

bool checkObjectDataExpired(std::optional<ObjectData> & data, const double timeout)
{
if (!data.has_value()) return true;
Expand Down Expand Up @@ -246,6 +251,8 @@ class AEB : public rclcpp::Node
void onTimer();
void onPredictedTrajectory(const Trajectory::ConstSharedPtr input_msg);
void onAutowareState(const AutowareState::ConstSharedPtr input_msg);
rcl_interfaces::msg::SetParametersResult onParameter(
const std::vector<rclcpp::Parameter> & parameters);

bool isDataReady();

Expand Down Expand Up @@ -321,6 +328,8 @@ class AEB : public rclcpp::Node
double mpc_prediction_time_horizon_;
double mpc_prediction_time_interval_;
CollisionDataKeeper collision_data_keeper_;
// Parameter callback
OnSetParametersCallbackHandle::SharedPtr set_param_res_;
};
} // namespace autoware::motion::control::autonomous_emergency_braking

Expand Down
49 changes: 48 additions & 1 deletion control/autonomous_emergency_braking/src/node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
#include <tier4_autoware_utils/geometry/geometry.hpp>
#include <tier4_autoware_utils/ros/marker_helper.hpp>
#include <tier4_autoware_utils/ros/update_param.hpp>

#include <boost/geometry/algorithms/convex_hull.hpp>
#include <boost/geometry/algorithms/within.hpp>
Expand All @@ -34,7 +35,6 @@
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/surface/convex_hull.h>
#include <tf2/utils.h>

#ifdef ROS_DISTRO_GALACTIC
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
Expand Down Expand Up @@ -171,12 +171,59 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
collision_data_keeper_.setTimeout(collision_keeping_sec, previous_obstacle_keep_time);
}

// Parameter Callback
set_param_res_ =
add_on_set_parameters_callback(std::bind(&AEB::onParameter, this, std::placeholders::_1));

// start time
const double aeb_hz = declare_parameter<double>("aeb_hz");
const auto period_ns = rclcpp::Rate(aeb_hz).period();
timer_ = rclcpp::create_timer(this, this->get_clock(), period_ns, std::bind(&AEB::onTimer, this));
}

rcl_interfaces::msg::SetParametersResult AEB::onParameter(
const std::vector<rclcpp::Parameter> & parameters)
{
using tier4_autoware_utils::updateParam;
updateParam<bool>(parameters, "publish_debug_pointcloud", publish_debug_pointcloud_);
updateParam<bool>(parameters, "use_predicted_trajectory", use_predicted_trajectory_);
updateParam<bool>(parameters, "use_imu_path", use_imu_path_);
updateParam<double>(parameters, "path_footprint_extra_margin", path_footprint_extra_margin_);
updateParam<double>(parameters, "detection_range_min_height", detection_range_min_height_);
updateParam<double>(
parameters, "detection_range_max_height_margin", detection_range_max_height_margin_);
updateParam<double>(parameters, "voxel_grid_x", voxel_grid_x_);
updateParam<double>(parameters, "voxel_grid_y", voxel_grid_y_);
updateParam<double>(parameters, "voxel_grid_z", voxel_grid_z_);
updateParam<double>(parameters, "min_generated_path_length", min_generated_path_length_);
updateParam<double>(parameters, "expand_width", expand_width_);
updateParam<double>(parameters, "longitudinal_offset", longitudinal_offset_);
updateParam<double>(parameters, "t_response", t_response_);
updateParam<double>(parameters, "a_ego_min", a_ego_min_);
updateParam<double>(parameters, "a_obj_min", a_obj_min_);

updateParam<double>(parameters, "cluster_tolerance", cluster_tolerance_);
updateParam<int>(parameters, "minimum_cluster_size", minimum_cluster_size_);
updateParam<int>(parameters, "maximum_cluster_size", maximum_cluster_size_);

updateParam<double>(parameters, "imu_prediction_time_horizon", imu_prediction_time_horizon_);
updateParam<double>(parameters, "imu_prediction_time_interval", imu_prediction_time_interval_);
updateParam<double>(parameters, "mpc_prediction_time_horizon", mpc_prediction_time_horizon_);
updateParam<double>(parameters, "mpc_prediction_time_interval", mpc_prediction_time_interval_);

{ // Object history data keeper setup
auto [previous_obstacle_keep_time, collision_keeping_sec] = collision_data_keeper_.getTimeout();
updateParam<double>(parameters, "previous_obstacle_keep_time", previous_obstacle_keep_time);
updateParam<double>(parameters, "collision_keeping_sec", collision_keeping_sec);
collision_data_keeper_.setTimeout(collision_keeping_sec, previous_obstacle_keep_time);
}

rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
result.reason = "success";
return result;
}

void AEB::onTimer()
{
updater_.force_update();
Expand Down
Loading