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.

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(map_based_prediction): enable to control lateral path convergence speed by setting control time horizon #5285

Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
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
Next Next commit
enable to control lateral path convergence speed by setting control t…
…ime horizon

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
  • Loading branch information
YoshiRi committed Oct 19, 2023
commit 52360d3fb5a2b85880c5b2ae4e219f60ff2e00c0
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
ros__parameters:
enable_delay_compensation: true
prediction_time_horizon: 10.0 #[s]
lateral_control_time_horizon: 5.0 #[s]
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@YoshiRi
could you add explanation in the README.md?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

[NITS]
time_to_center_line is easier for me to understand the feature.

prediction_sampling_delta_time: 0.5 #[s]
min_velocity_for_map_based_prediction: 1.39 #[m/s]
min_crosswalk_user_velocity: 1.39 #[m/s]
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -135,6 +135,7 @@ class MapBasedPredictionNode : public rclcpp::Node
// Parameters
bool enable_delay_compensation_;
double prediction_time_horizon_;
double lateral_control_time_horizon_;
double prediction_time_horizon_rate_for_validate_lane_length_;
double prediction_sampling_time_interval_;
double min_velocity_for_map_based_prediction_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -81,8 +81,8 @@ class PathGenerator
{
public:
PathGenerator(
const double time_horizon, const double sampling_time_interval,
const double min_crosswalk_user_velocity);
const double time_horizon, const double lateral_time_horizon,
const double sampling_time_interval, const double min_crosswalk_user_velocity);

PredictedPath generatePathForNonVehicleObject(const TrackedObject & object);

Expand All @@ -102,6 +102,7 @@ class PathGenerator
private:
// Parameters
double time_horizon_;
double lateral_time_horizon_;
double sampling_time_interval_;
double min_crosswalk_user_velocity_;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -694,6 +694,8 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
{
enable_delay_compensation_ = declare_parameter<bool>("enable_delay_compensation");
prediction_time_horizon_ = declare_parameter<double>("prediction_time_horizon");
lateral_control_time_horizon_ =
declare_parameter<double>("lateral_control_time_horizon"); // [s] for lateral control point
prediction_sampling_time_interval_ = declare_parameter<double>("prediction_sampling_delta_time");
min_velocity_for_map_based_prediction_ =
declare_parameter<double>("min_velocity_for_map_based_prediction");
Expand Down Expand Up @@ -740,7 +742,8 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
declare_parameter<double>("prediction_time_horizon_rate_for_validate_shoulder_lane_length");

path_generator_ = std::make_shared<PathGenerator>(
prediction_time_horizon_, prediction_sampling_time_interval_, min_crosswalk_user_velocity_);
prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_,
min_crosswalk_user_velocity_);

sub_objects_ = this->create_subscription<TrackedObjects>(
"~/input/objects", 1,
Expand Down
15 changes: 10 additions & 5 deletions perception/map_based_prediction/src/path_generator.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -23,9 +23,10 @@
namespace map_based_prediction
{
PathGenerator::PathGenerator(
const double time_horizon, const double sampling_time_interval,
const double time_horizon, const double lateral_time_horizon, const double sampling_time_interval,
const double min_crosswalk_user_velocity)
: time_horizon_(time_horizon),
lateral_time_horizon_(lateral_time_horizon),
sampling_time_interval_(sampling_time_interval),
min_crosswalk_user_velocity_(min_crosswalk_user_velocity)
{
Expand Down Expand Up @@ -213,16 +214,20 @@ FrenetPath PathGenerator::generateFrenetPath(
{
FrenetPath path;
const double duration = time_horizon_;
const double lateral_duration = lateral_time_horizon_;

// Compute Lateral and Longitudinal Coefficients to generate the trajectory
const Eigen::Vector3d lat_coeff = calcLatCoefficients(current_point, target_point, duration);
const Eigen::Vector3d lat_coeff =
calcLatCoefficients(current_point, target_point, lateral_duration);
const Eigen::Vector2d lon_coeff = calcLonCoefficients(current_point, target_point, duration);

path.reserve(static_cast<size_t>(duration / sampling_time_interval_));
for (double t = 0.0; t <= duration; t += sampling_time_interval_) {
const double d_next = current_point.d + current_point.d_vel * t + 0 * 2 * std::pow(t, 2) +
lat_coeff(0) * std::pow(t, 3) + lat_coeff(1) * std::pow(t, 4) +
lat_coeff(2) * std::pow(t, 5);
const double d_next_ = current_point.d + current_point.d_vel * t + 0 * 2 * std::pow(t, 2) +
lat_coeff(0) * std::pow(t, 3) + lat_coeff(1) * std::pow(t, 4) +
lat_coeff(2) * std::pow(t, 5);
// t > lateral_duration: 0.0, else d_next_
const double d_next = t > lateral_duration ? 0.0 : d_next_;
const double s_next = current_point.s + current_point.s_vel * t + 2 * 0 * std::pow(t, 2) +
lon_coeff(0) * std::pow(t, 3) + lon_coeff(1) * std::pow(t, 4);
YoshiRi marked this conversation as resolved.
Show resolved Hide resolved
if (s_next > max_length) {
Expand Down