Skip to content

Commit

Permalink
Merge pull request #1620 from tier4/fix/merge-fix-from-v0.29.0-1
Browse files Browse the repository at this point in the history
feat: merge beta/v0.29.0 1 to beta/x2_gen2_v0.29.0
  • Loading branch information
TomohitoAndo authored Nov 5, 2024
2 parents 9b6db8c + ab69381 commit 8f03868
Show file tree
Hide file tree
Showing 6 changed files with 11 additions and 7 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -488,6 +488,8 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData
// calculate the target motion for delay compensation
constexpr double min_running_dist = 0.01;
if (control_data.state_after_delay.running_distance > min_running_dist) {
control_data.interpolated_traj.points =
autoware::motion_utils::removeOverlapPoints(control_data.interpolated_traj.points);
const auto target_pose = longitudinal_utils::findTrajectoryPoseAfterDistance(
control_data.nearest_idx, control_data.state_after_delay.running_distance,
control_data.interpolated_traj);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
<!-- Parameter files -->
<arg name="localization_error_monitor_param_path"/>
<arg name="ekf_localizer_param_path"/>
<arg name="stop_filter_param_path"/>
<arg name="pose_initializer_param_path"/>
<arg name="eagleye_param_path"/>
<arg name="ar_tag_based_localizer_param_path"/>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
<arg name="input_odom_name" value="/localization/pose_twist_fusion_filter/kinematic_state"/>
<arg name="input_twist_with_covariance_name" value="/localization/pose_twist_fusion_filter/twist_with_covariance"/>
<arg name="output_odom_name" value="/localization/kinematic_state"/>
<arg name="param_path" value="$(var stop_filter_param_path)"/>
</include>
</group>

Expand Down
8 changes: 6 additions & 2 deletions planning/autoware_path_optimizer/src/mpt_optimizer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -477,8 +477,13 @@ std::vector<TrajectoryPoint> MPTOptimizer::optimizeTrajectory(const PlannerData

const auto get_prev_optimized_traj_points = [&]() {
if (prev_optimized_traj_points_ptr_) {
RCLCPP_WARN(logger_, "return the previous optimized_trajectory as exceptional behavior.");
return *prev_optimized_traj_points_ptr_;
}
RCLCPP_WARN(
logger_,
"Try to return the previous optimized_trajectory as exceptional behavior, "
"but this failure also. Then return path_smoother output.");
return traj_points;
};

Expand All @@ -505,8 +510,7 @@ std::vector<TrajectoryPoint> MPTOptimizer::optimizeTrajectory(const PlannerData
// 6. optimize steer angles
const auto optimized_variables = calcOptimizedSteerAngles(ref_points, obj_mat, const_mat);
if (!optimized_variables) {
RCLCPP_INFO_EXPRESSION(
logger_, enable_debug_info_, "return std::nullopt since could not solve qp");
RCLCPP_WARN(logger_, "return std::nullopt since could not solve qp");
return get_prev_optimized_traj_points();
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -1259,8 +1259,6 @@ bool GoalPlannerModule::hasNotDecidedPath(
const std::shared_ptr<SafetyCheckParams> & safety_check_params,
const std::shared_ptr<GoalSearcherBase> goal_searcher) const
{
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

return checkDecidingPathStatus(
planner_data, occupancy_grid_map, parameters, ego_predicted_path_params,
objects_filtering_params, safety_check_params, goal_searcher)
Expand Down Expand Up @@ -2371,8 +2369,6 @@ std::pair<bool, bool> GoalPlannerModule::isSafePath(
const std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params,
const std::shared_ptr<SafetyCheckParams> & safety_check_params) const
{
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

if (!thread_safe_data_.get_pull_over_path()) {
return {false, false};
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -293,7 +293,7 @@ PointCloudConcatenateDataSynchronizerComponent::computeTransformToAdjustForOldTi

// return identity if old_stamp is newer than new_stamp
if (old_stamp > new_stamp) {
RCLCPP_WARN_STREAM_THROTTLE(
RCLCPP_DEBUG_STREAM_THROTTLE(
get_logger(), *get_clock(), std::chrono::milliseconds(10000).count(),
"old_stamp is newer than new_stamp,");
return Eigen::Matrix4f::Identity();
Expand Down

0 comments on commit 8f03868

Please sign in to comment.