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

fix(pid_longitudinal_control): not check steering convergence when moving #999

Merged
merged 3 commits into from
Nov 1, 2023
Merged

fix(pid_longitudinal_control): not check steering convergence when moving #999

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
827959e
fix(pid_longitudinal_control): not check steering convergence when mo…
TakaHoribe Nov 1, 2023
3e8fccc
Merge branch 'beta/v0.10.4' into vel-con-steer-converge
tkimura4 Nov 1, 2023
6ce1cfd
style(pre-commit): autofix
pre-commit-ci[bot] Nov 1, 2023
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
11 changes: 7 additions & 4 deletions control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -489,8 +489,13 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
stop_dist > p.drive_state_stop_dist + p.drive_state_offset_stop_dist;
const bool departure_condition_from_stopped = stop_dist > p.drive_state_stop_dist;

const bool keep_stopped_condition =
m_enable_keep_stopped_until_steer_convergence && !lateral_sync_data_.is_steer_converged;
// NOTE: the same velocity threshold as motion_utils::searchZeroVelocity
static constexpr double vel_epsilon = 1e-3;

// Let vehicle start after the steering is converged for control accuracy
const bool keep_stopped_condition = std::fabs(current_vel) < vel_epsilon &&
m_enable_keep_stopped_until_steer_convergence &&
!lateral_sync_data_.is_steer_converged;

const bool stopping_condition = stop_dist < p.stopping_state_stop_dist;
if (
Expand All @@ -503,8 +508,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
? (node_->now() - *m_last_running_time).seconds() > p.stopped_state_entry_duration_time
: false;

static constexpr double vel_epsilon =
1e-3; // NOTE: the same velocity threshold as motion_utils::searchZeroVelocity
const double current_vel_cmd =
std::fabs(m_trajectory.points.at(control_data.nearest_idx).longitudinal_velocity_mps);
const bool emergency_condition = m_enable_overshoot_emergency &&
Expand Down
5 changes: 2 additions & 3 deletions perception/map_based_prediction/src/map_based_prediction_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -227,7 +227,7 @@
for (auto & candidate : right_lane_candidates) {
// exclude self lanelet
if (candidate == current_lanelet) continue;
// if candidate has linestring as leftbound, assign it to output

Check warning on line 230 in perception/map_based_prediction/src/map_based_prediction_node.cpp

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (leftbound)
if (candidate.leftBound() == current_lanelet.rightBound()) {
output_lanelets.push_back(candidate);
}
Expand All @@ -254,7 +254,7 @@
for (auto & candidate : left_lane_candidates) {
// exclude self lanelet
if (candidate == current_lanelet) continue;
// if candidate has linestring as rightbound, assign it to output

Check warning on line 257 in perception/map_based_prediction/src/map_based_prediction_node.cpp

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (rightbound)
if (candidate.rightBound() == current_lanelet.leftBound()) {
output_lanelets.push_back(candidate);
}
Expand Down Expand Up @@ -287,7 +287,7 @@
lanelet::ConstLanelets possible_lanelets;
possible_lanelets.push_back(lanelet);
lanelet::routing::LaneletPaths possible_paths;
// need to init path with constlanelets

Check warning on line 290 in perception/map_based_prediction/src/map_based_prediction_node.cpp

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (constlanelets)
lanelet::routing::LaneletPath possible_path(possible_lanelets);
possible_paths.push_back(possible_path);
return possible_paths;
Expand Down Expand Up @@ -550,7 +550,7 @@
label == ObjectClassification::MOTORCYCLE || label == ObjectClassification::BICYCLE) {
// if object is within road lanelet and satisfies yaw constraints
const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true);
const float high_speed_threshold = 25.0 / 18.0 * 5.0; // High speed bycicle 25 km/h

Check warning on line 553 in perception/map_based_prediction/src/map_based_prediction_node.cpp

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (bycicle)
const bool high_speed_object =
object.kinematics.twist_with_covariance.twist.linear.x > high_speed_threshold;

Expand Down Expand Up @@ -611,7 +611,6 @@
pose_with_cov.pose.orientation = tf2::toMsg(filtered_quaternion);
}


MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_options)
: Node("map_based_prediction", node_options), debug_accumulated_time_(0.0)
{
Expand Down Expand Up @@ -860,8 +859,8 @@
// Generate Predicted Path
std::vector<PredictedPath> predicted_paths;
for (const auto & ref_path : ref_paths) {
PredictedPath predicted_path =
path_generator_->generatePathForOnLaneVehicle(yaw_fixed_transformed_object, ref_path.path);
PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle(
yaw_fixed_transformed_object, ref_path.path);
if (predicted_path.path.empty()) {
continue;
}
Expand Down
Loading