Merge branch 'master' of http://116.63.46.168:3000/ADPilot/modularization

src/common/include/ivtype/ivSteeringReport.h

@@ -9,7 +9,7 @@ namespace  iv {
 class ADCSteeringReport
 {
 public:
-    double steering_tire_angle;
+    float steering_tire_angle;
 
 
 };

src/common/include/ivtype/ivpose.h

@@ -318,9 +318,15 @@ struct ADCPose
 };
 
 
+struct ADCTwist
+{
+    double linear_x;
+};
+
 struct ADCOdometry
 {
     ADCPose pose;
+    ADCTwist twist;
 };
 
 }

src/decition/mpccontroller/mpc.cpp

@@ -15,9 +15,9 @@
 #include "mpc_lateral_controller/mpc.hpp"
 
 #include "interpolation/linear_interpolation.hpp"
-#include "motion_utils/trajectory/trajectory.hpp"
+//#include "motion_utils/trajectory/trajectory.hpp"
 #include "mpc_lateral_controller/mpc_utils.hpp"
-#include "tier4_autoware_utils/math/unit_conversion.hpp"
+//#include "tier4_autoware_utils/math/unit_conversion.hpp"
 
 #include <algorithm>
 #include <limits>
@@ -25,25 +25,107 @@
 
 namespace autoware::motion::control::mpc_lateral_controller
 {
-using tier4_autoware_utils::calcDistance2d;
-using tier4_autoware_utils::normalizeRadian;
-using tier4_autoware_utils::rad2deg;
+//using tier4_autoware_utils::calcDistance2d;
+//using tier4_autoware_utils::normalizeRadian;
+//using tier4_autoware_utils::rad2deg;
+
+//bool MPC::calculateMPC(
+//  const SteeringReport & current_steer, const Odometry & current_kinematics,
+//  AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_trajectory,
+//  Float32MultiArrayStamped & diagnostic)
+//{
+//  // since the reference trajectory does not take into account the current velocity of the ego
+//  // vehicle, it needs to calculate the trajectory velocity considering the longitudinal dynamics.
+//  const auto reference_trajectory =
+//    applyVelocityDynamicsFilter(m_reference_trajectory, current_kinematics);
+
+//  // get the necessary data
+//  const auto [success_data, mpc_data] =
+//    getData(reference_trajectory, current_steer, current_kinematics);
+//  if (!success_data) {
+//    return fail_warn_throttle("fail to get MPC Data. Stop MPC.");
+//  }
+
+//  // calculate initial state of the error dynamics
+//  const auto x0 = getInitialState(mpc_data);
+
+//  // apply time delay compensation to the initial state
+//  const auto [success_delay, x0_delayed] =
+//    updateStateForDelayCompensation(reference_trajectory, mpc_data.nearest_time, x0);
+//  if (!success_delay) {
+//    return fail_warn_throttle("delay compensation failed. Stop MPC.");
+//  }
+
+//  // resample reference trajectory with mpc sampling time
+//  const double mpc_start_time = mpc_data.nearest_time + m_param.input_delay;
+//  const double prediction_dt =
+//    getPredictionDeltaTime(mpc_start_time, reference_trajectory, current_kinematics);
+
+//  const auto [success_resample, mpc_resampled_ref_trajectory] =
+//    resampleMPCTrajectoryByTime(mpc_start_time, prediction_dt, reference_trajectory);
+//  if (!success_resample) {
+//    return fail_warn_throttle("trajectory resampling failed. Stop MPC.");
+//  }
+
+//  // generate mpc matrix : predict equation Xec = Aex * x0 + Bex * Uex + Wex
+//  const auto mpc_matrix = generateMPCMatrix(mpc_resampled_ref_trajectory, prediction_dt);
+
+//  // solve Optimization problem
+//  const auto [success_opt, Uex] = executeOptimization(
+//    mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory,
+//    current_kinematics.twist.twist.linear.x);
+//  if (!success_opt) {
+//    return fail_warn_throttle("optimization failed. Stop MPC.");
+//  }
+
+//  // apply filters for the input limitation and low pass filter
+//  const double u_saturated = std::clamp(Uex(0), -m_steer_lim, m_steer_lim);
+//  const double u_filtered = m_lpf_steering_cmd.filter(u_saturated);
+
+//  // set control command
+//  ctrl_cmd.steering_tire_angle = static_cast<float>(u_filtered);
+//  ctrl_cmd.steering_tire_rotation_rate = static_cast<float>(calcDesiredSteeringRate(
+//    mpc_matrix, x0_delayed, Uex, u_filtered, current_steer.steering_tire_angle, prediction_dt));
+
+//  // save the control command for the steering prediction
+//  m_steering_predictor->storeSteerCmd(u_filtered);
+
+//  // save input to buffer for delay compensation
+//  m_input_buffer.push_back(ctrl_cmd.steering_tire_angle);
+//  m_input_buffer.pop_front();
+
+//  // save previous input for the mpc rate limit
+//  m_raw_steer_cmd_pprev = m_raw_steer_cmd_prev;
+//  m_raw_steer_cmd_prev = Uex(0);
+
+//  // calculate predicted trajectory
+//  predicted_trajectory =
+//    calcPredictedTrajectory(mpc_resampled_ref_trajectory, mpc_matrix, x0_delayed, Uex);
+
+//  // prepare diagnostic message
+//  diagnostic =
+//    generateDiagData(reference_trajectory, mpc_data, mpc_matrix, ctrl_cmd, Uex, current_kinematics);
+
+//  return true;
+//}
 
 bool MPC::calculateMPC(
-  const SteeringReport & current_steer, const Odometry & current_kinematics,
-  AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_trajectory,
-  Float32MultiArrayStamped & diagnostic)
+    const iv::ADCSteeringReport & current_steer, const iv::ADCOdometry & current_kinematics,
+    ADCAckermannLateralCommand & ctrl_cmd, MPCTrajectory & predicted_trajectory
+    )
 {
   // since the reference trajectory does not take into account the current velocity of the ego
   // vehicle, it needs to calculate the trajectory velocity considering the longitudinal dynamics.
   const auto reference_trajectory =
-    applyVelocityDynamicsFilter(m_reference_trajectory, current_kinematics);
+      applyVelocityDynamicsFilter(m_reference_trajectory, current_kinematics);
 
   // get the necessary data
   const auto [success_data, mpc_data] =
-    getData(reference_trajectory, current_steer, current_kinematics);
+      getData(reference_trajectory, current_steer, current_kinematics);
   if (!success_data) {
-    return fail_warn_throttle("fail to get MPC Data. Stop MPC.");
+      std::cout<<"fail to get MPC Data. Stop MPC."<<std::endl;
+      return false;
+ //   return fail_warn_throttle("fail to get MPC Data. Stop MPC.");
   }
 
   // calculate initial state of the error dynamics
@@ -51,20 +133,24 @@ bool MPC::calculateMPC(
 
   // apply time delay compensation to the initial state
   const auto [success_delay, x0_delayed] =
-    updateStateForDelayCompensation(reference_trajectory, mpc_data.nearest_time, x0);
+      updateStateForDelayCompensation(reference_trajectory, mpc_data.nearest_time, x0);
   if (!success_delay) {
-    return fail_warn_throttle("delay compensation failed. Stop MPC.");
+      std::cout<<"delay compensation failed. Stop MPC."<<std::endl;
+      return false;
+//    return fail_warn_throttle("delay compensation failed. Stop MPC.");
   }
 
   // resample reference trajectory with mpc sampling time
   const double mpc_start_time = mpc_data.nearest_time + m_param.input_delay;
   const double prediction_dt =
-    getPredictionDeltaTime(mpc_start_time, reference_trajectory, current_kinematics);
+      getPredictionDeltaTime(mpc_start_time, reference_trajectory, current_kinematics);
 
   const auto [success_resample, mpc_resampled_ref_trajectory] =
-    resampleMPCTrajectoryByTime(mpc_start_time, prediction_dt, reference_trajectory);
+      resampleMPCTrajectoryByTime(mpc_start_time, prediction_dt, reference_trajectory);
   if (!success_resample) {
-    return fail_warn_throttle("trajectory resampling failed. Stop MPC.");
+      std::cout<<"trajectory resampling failed. Stop MPC."<<std::endl;
+      return false;
+//    return fail_warn_throttle("trajectory resampling failed. Stop MPC.");
   }
 
   // generate mpc matrix : predict equation Xec = Aex * x0 + Bex * Uex + Wex
@@ -72,10 +158,12 @@ bool MPC::calculateMPC(
 
   // solve Optimization problem
   const auto [success_opt, Uex] = executeOptimization(
-    mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory,
-    current_kinematics.twist.twist.linear.x);
+      mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory,
+      current_kinematics.twist.linear_x);
   if (!success_opt) {
-    return fail_warn_throttle("optimization failed. Stop MPC.");
+      std::cout<<"optimization failed. Stop MPC."<<std::endl;
+      return false;
+//    return fail_warn_throttle("optimization failed. Stop MPC.");
   }
 
   // apply filters for the input limitation and low pass filter
@@ -85,7 +173,7 @@ bool MPC::calculateMPC(
   // set control command
   ctrl_cmd.steering_tire_angle = static_cast<float>(u_filtered);
   ctrl_cmd.steering_tire_rotation_rate = static_cast<float>(calcDesiredSteeringRate(
-    mpc_matrix, x0_delayed, Uex, u_filtered, current_steer.steering_tire_angle, prediction_dt));
+      mpc_matrix, x0_delayed, Uex, u_filtered, current_steer.steering_tire_angle, prediction_dt));
 
   // save the control command for the steering prediction
   m_steering_predictor->storeSteerCmd(u_filtered);
@@ -100,18 +188,42 @@ bool MPC::calculateMPC(
 
   // calculate predicted trajectory
   predicted_trajectory =
-    calcPredictedTrajectory(mpc_resampled_ref_trajectory, mpc_matrix, x0_delayed, Uex);
+      calcPredictedTrajectory(mpc_resampled_ref_trajectory, mpc_matrix, x0_delayed, Uex);
 
   // prepare diagnostic message
-  diagnostic =
-    generateDiagData(reference_trajectory, mpc_data, mpc_matrix, ctrl_cmd, Uex, current_kinematics);
+//  diagnostic =
+//      generateDiagData(reference_trajectory, mpc_data, mpc_matrix, ctrl_cmd, Uex, current_kinematics);
 
   return true;
 }
 
-Trajectory MPC::calcPredictedTrajectory(
-  const MPCTrajectory & mpc_resampled_ref_trajectory, const MPCMatrix & mpc_matrix,
-  const VectorXd & x0_delayed, const VectorXd & Uex) const
+//Trajectory MPC::calcPredictedTrajectory(
+//  const MPCTrajectory & mpc_resampled_ref_trajectory, const MPCMatrix & mpc_matrix,
+//  const VectorXd & x0_delayed, const VectorXd & Uex) const
+//{
+//  const VectorXd Xex = mpc_matrix.Aex * x0_delayed + mpc_matrix.Bex * Uex + mpc_matrix.Wex;
+//  MPCTrajectory mpc_predicted_traj;
+//  const auto & traj = mpc_resampled_ref_trajectory;
+//  for (int i = 0; i < m_param.prediction_horizon; ++i) {
+//    const int DIM_X = m_vehicle_model_ptr->getDimX();
+//    const double lat_error = Xex(i * DIM_X);
+//    const double yaw_error = Xex(i * DIM_X + 1);
+//    const double x = traj.x.at(i) - std::sin(traj.yaw.at(i)) * lat_error;
+//    const double y = traj.y.at(i) + std::cos(traj.yaw.at(i)) * lat_error;
+//    const double z = traj.z.at(i);
+//    const double yaw = traj.yaw.at(i) + yaw_error;
+//    const double vx = traj.vx.at(i);
+//    const double k = traj.k.at(i);
+//    const double smooth_k = traj.smooth_k.at(i);
+//    const double relative_time = traj.relative_time.at(i);
+//    mpc_predicted_traj.push_back(x, y, z, yaw, vx, k, smooth_k, relative_time);
+//  }
+//  return MPCUtils::convertToAutowareTrajectory(mpc_predicted_traj);
+//}
+
+MPCTrajectory MPC::calcPredictedTrajectory(
+    const MPCTrajectory & mpc_resampled_ref_trajectory, const MPCMatrix & mpc_matrix,
+    const VectorXd & x0_delayed, const VectorXd & Uex) const
 {
   const VectorXd Xex = mpc_matrix.Aex * x0_delayed + mpc_matrix.Bex * Uex + mpc_matrix.Wex;
   MPCTrajectory mpc_predicted_traj;
@@ -130,89 +242,171 @@ Trajectory MPC::calcPredictedTrajectory(
     const double relative_time = traj.relative_time.at(i);
     mpc_predicted_traj.push_back(x, y, z, yaw, vx, k, smooth_k, relative_time);
   }
-  return MPCUtils::convertToAutowareTrajectory(mpc_predicted_traj);
+  return mpc_predicted_traj;
+ // return MPCUtils::convertToAutowareTrajectory(mpc_predicted_traj);
 }
 
-Float32MultiArrayStamped MPC::generateDiagData(
-  const MPCTrajectory & reference_trajectory, const MPCData & mpc_data,
-  const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex,
-  const Odometry & current_kinematics) const
-{
-  Float32MultiArrayStamped diagnostic;
+//Float32MultiArrayStamped MPC::generateDiagData(
+//  const MPCTrajectory & reference_trajectory, const MPCData & mpc_data,
+//  const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex,
+//  const Odometry & current_kinematics) const
+//{
+//  Float32MultiArrayStamped diagnostic;
+
+//  // prepare diagnostic message
+//  const double nearest_k = reference_trajectory.k.at(mpc_data.nearest_idx);
+//  const double nearest_smooth_k = reference_trajectory.smooth_k.at(mpc_data.nearest_idx);
+//  const double wb = m_vehicle_model_ptr->getWheelbase();
+//  const double current_velocity = current_kinematics.twist.twist.linear.x;
+//  const double wz_predicted = current_velocity * std::tan(mpc_data.predicted_steer) / wb;
+//  const double wz_measured = current_velocity * std::tan(mpc_data.steer) / wb;
+//  const double wz_command = current_velocity * std::tan(ctrl_cmd.steering_tire_angle) / wb;
+
+//  typedef decltype(diagnostic.data)::value_type DiagnosticValueType;
+//  const auto append_diag = [&](const auto & val) -> void {
+//    diagnostic.data.push_back(static_cast<DiagnosticValueType>(val));
+//  };
+//  append_diag(ctrl_cmd.steering_tire_angle);      // [0] final steering command (MPC + LPF)
+//  append_diag(Uex(0));                            // [1] mpc calculation result
+//  append_diag(mpc_matrix.Uref_ex(0));             // [2] feed-forward steering value
+//  append_diag(std::atan(nearest_smooth_k * wb));  // [3] feed-forward steering value raw
+//  append_diag(mpc_data.steer);                    // [4] current steering angle
+//  append_diag(mpc_data.lateral_err);              // [5] lateral error
+//  append_diag(tf2::getYaw(current_kinematics.pose.pose.orientation));  // [6] current_pose yaw
+//  append_diag(tf2::getYaw(mpc_data.nearest_pose.orientation));         // [7] nearest_pose yaw
+//  append_diag(mpc_data.yaw_err);                                       // [8] yaw error
+//  append_diag(reference_trajectory.vx.at(mpc_data.nearest_idx));       // [9] reference velocity
+//  append_diag(current_velocity);                                       // [10] measured velocity
+//  append_diag(wz_command);                           // [11] angular velocity from steer command
+//  append_diag(wz_measured);                          // [12] angular velocity from measured steer
+//  append_diag(current_velocity * nearest_smooth_k);  // [13] angular velocity from path curvature
+//  append_diag(nearest_smooth_k);          // [14] nearest path curvature (used for feed-forward)
+//  append_diag(nearest_k);                 // [15] nearest path curvature (not smoothed)
+//  append_diag(mpc_data.predicted_steer);  // [16] predicted steer
+//  append_diag(wz_predicted);              // [17] angular velocity from predicted steer
+
+//  return diagnostic;
+//}
 
-  // prepare diagnostic message
-  const double nearest_k = reference_trajectory.k.at(mpc_data.nearest_idx);
-  const double nearest_smooth_k = reference_trajectory.smooth_k.at(mpc_data.nearest_idx);
-  const double wb = m_vehicle_model_ptr->getWheelbase();
-  const double current_velocity = current_kinematics.twist.twist.linear.x;
-  const double wz_predicted = current_velocity * std::tan(mpc_data.predicted_steer) / wb;
-  const double wz_measured = current_velocity * std::tan(mpc_data.steer) / wb;
-  const double wz_command = current_velocity * std::tan(ctrl_cmd.steering_tire_angle) / wb;
-
-  typedef decltype(diagnostic.data)::value_type DiagnosticValueType;
-  const auto append_diag = [&](const auto & val) -> void {
-    diagnostic.data.push_back(static_cast<DiagnosticValueType>(val));
-  };
-  append_diag(ctrl_cmd.steering_tire_angle);      // [0] final steering command (MPC + LPF)
-  append_diag(Uex(0));                            // [1] mpc calculation result
-  append_diag(mpc_matrix.Uref_ex(0));             // [2] feed-forward steering value
-  append_diag(std::atan(nearest_smooth_k * wb));  // [3] feed-forward steering value raw
-  append_diag(mpc_data.steer);                    // [4] current steering angle
-  append_diag(mpc_data.lateral_err);              // [5] lateral error
-  append_diag(tf2::getYaw(current_kinematics.pose.pose.orientation));  // [6] current_pose yaw
-  append_diag(tf2::getYaw(mpc_data.nearest_pose.orientation));         // [7] nearest_pose yaw
-  append_diag(mpc_data.yaw_err);                                       // [8] yaw error
-  append_diag(reference_trajectory.vx.at(mpc_data.nearest_idx));       // [9] reference velocity
-  append_diag(current_velocity);                                       // [10] measured velocity
-  append_diag(wz_command);                           // [11] angular velocity from steer command
-  append_diag(wz_measured);                          // [12] angular velocity from measured steer
-  append_diag(current_velocity * nearest_smooth_k);  // [13] angular velocity from path curvature
-  append_diag(nearest_smooth_k);          // [14] nearest path curvature (used for feed-forward)
-  append_diag(nearest_k);                 // [15] nearest path curvature (not smoothed)
-  append_diag(mpc_data.predicted_steer);  // [16] predicted steer
-  append_diag(wz_predicted);              // [17] angular velocity from predicted steer
-
-  return diagnostic;
-}
+//void MPC::setReferenceTrajectory(
+//  const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param,
+//  const Odometry & current_kinematics)
+//{
+//  const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
+//    trajectory_msg.points, current_kinematics.pose.pose, ego_nearest_dist_threshold,
+//    ego_nearest_yaw_threshold);
+//  const double ego_offset_to_segment = motion_utils::calcLongitudinalOffsetToSegment(
+//    trajectory_msg.points, nearest_seg_idx, current_kinematics.pose.pose.position);
+
+//  const auto mpc_traj_raw = MPCUtils::convertToMPCTrajectory(trajectory_msg);
+
+//  // resampling
+//  const auto [success_resample, mpc_traj_resampled] = MPCUtils::resampleMPCTrajectoryByDistance(
+//    mpc_traj_raw, param.traj_resample_dist, nearest_seg_idx, ego_offset_to_segment);
+//  if (!success_resample) {
+//    warn_throttle("[setReferenceTrajectory] spline error when resampling by distance");
+//    return;
+//  }
+
+//  const auto is_forward_shift =
+//    motion_utils::isDrivingForward(mpc_traj_resampled.toTrajectoryPoints());
+
+//  // if driving direction is unknown, use previous value
+//  m_is_forward_shift = is_forward_shift ? is_forward_shift.get() : m_is_forward_shift;
+
+//  // path smoothing
+//  MPCTrajectory mpc_traj_smoothed = mpc_traj_resampled;  // smooth filtered trajectory
+//  const int mpc_traj_resampled_size = static_cast<int>(mpc_traj_resampled.size());
+//  if (
+//    param.enable_path_smoothing && mpc_traj_resampled_size > 2 * param.path_filter_moving_ave_num) {
+//    using MoveAverageFilter::filt_vector;
+//    if (
+//      !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.x) ||
+//      !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.y) ||
+//      !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.yaw) ||
+//      !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.vx)) {
+//      RCLCPP_DEBUG(m_logger, "path callback: filtering error. stop filtering.");
+//      mpc_traj_smoothed = mpc_traj_resampled;
+//    }
+//  }
+
+//  /*
+//   * Extend terminal points
+//   * Note: The current MPC does not properly take into account the attitude angle at the end of the
+//   * path. By extending the end of the path in the attitude direction, the MPC can consider the
+//   * attitude angle well, resulting in improved control performance. If the trajectory is
+//   * well-defined considering the end point attitude angle, this feature is not necessary.
+//   */
+//  if (param.extend_trajectory_for_end_yaw_control) {
+//    MPCUtils::extendTrajectoryInYawDirection(
+//      mpc_traj_raw.yaw.back(), param.traj_resample_dist, m_is_forward_shift, mpc_traj_smoothed);
+//  }
+
+//  // calculate yaw angle
+//  MPCUtils::calcTrajectoryYawFromXY(mpc_traj_smoothed, m_is_forward_shift);
+//  MPCUtils::convertEulerAngleToMonotonic(mpc_traj_smoothed.yaw);
+
+//  // calculate curvature
+//  MPCUtils::calcTrajectoryCurvature(
+//    param.curvature_smoothing_num_traj, param.curvature_smoothing_num_ref_steer, mpc_traj_smoothed);
+
+//  // stop velocity at a terminal point
+//  mpc_traj_smoothed.vx.back() = 0.0;
+
+//  // add a extra point on back with extended time to make the mpc stable.
+//  auto last_point = mpc_traj_smoothed.back();
+//  last_point.relative_time += 100.0;  // extra time to prevent mpc calc failure due to short time
+//  last_point.vx = 0.0;                // stop velocity at a terminal point
+//  mpc_traj_smoothed.push_back(last_point);
+
+//  if (!mpc_traj_smoothed.size()) {
+//    RCLCPP_DEBUG(m_logger, "path callback: trajectory size is undesired.");
+//    return;
+//  }
+
+//  m_reference_trajectory = mpc_traj_smoothed;
+//}
 
 void MPC::setReferenceTrajectory(
-  const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param,
-  const Odometry & current_kinematics)
+    const MPCTrajectory & trajectory_msg, const TrajectoryFilteringParam & param,
+    const iv::ADCOdometry & current_kinematics)
 {
-  const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
-    trajectory_msg.points, current_kinematics.pose.pose, ego_nearest_dist_threshold,
-    ego_nearest_yaw_threshold);
-  const double ego_offset_to_segment = motion_utils::calcLongitudinalOffsetToSegment(
-    trajectory_msg.points, nearest_seg_idx, current_kinematics.pose.pose.position);
+  const size_t nearest_seg_idx = MPCUtils::findFirstNearestSegmentIndexWithSoftConstraints(
+      trajectory_msg, current_kinematics.pose, ego_nearest_dist_threshold,
+      ego_nearest_yaw_threshold);
+  const double ego_offset_to_segment = MPCUtils::calcLongitudinalOffsetToSegment(
+      trajectory_msg, nearest_seg_idx, current_kinematics.pose.position);
 
-  const auto mpc_traj_raw = MPCUtils::convertToMPCTrajectory(trajectory_msg);
+  const auto mpc_traj_raw = trajectory_msg;// MPCUtils::convertToMPCTrajectory(trajectory_msg);
 
   // resampling
   const auto [success_resample, mpc_traj_resampled] = MPCUtils::resampleMPCTrajectoryByDistance(
-    mpc_traj_raw, param.traj_resample_dist, nearest_seg_idx, ego_offset_to_segment);
+      mpc_traj_raw, param.traj_resample_dist, nearest_seg_idx, ego_offset_to_segment);
   if (!success_resample) {
-    warn_throttle("[setReferenceTrajectory] spline error when resampling by distance");
+    std::cout<<"[setReferenceTrajectory] spline error when resampling by distance"<<std::endl;
+//    warn_throttle("[setReferenceTrajectory] spline error when resampling by distance");
     return;
   }
 
   const auto is_forward_shift =
-    motion_utils::isDrivingForward(mpc_traj_resampled.toTrajectoryPoints());
+      MPCUtils::isDrivingForward(mpc_traj_resampled);
 
   // if driving direction is unknown, use previous value
-  m_is_forward_shift = is_forward_shift ? is_forward_shift.get() : m_is_forward_shift;
+  m_is_forward_shift = is_forward_shift;// ? is_forward_shift.get() : m_is_forward_shift;
 
   // path smoothing
   MPCTrajectory mpc_traj_smoothed = mpc_traj_resampled;  // smooth filtered trajectory
   const int mpc_traj_resampled_size = static_cast<int>(mpc_traj_resampled.size());
   if (
-    param.enable_path_smoothing && mpc_traj_resampled_size > 2 * param.path_filter_moving_ave_num) {
+      param.enable_path_smoothing && mpc_traj_resampled_size > 2 * param.path_filter_moving_ave_num) {
     using MoveAverageFilter::filt_vector;
     if (
-      !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.x) ||
-      !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.y) ||
-      !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.yaw) ||
-      !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.vx)) {
-      RCLCPP_DEBUG(m_logger, "path callback: filtering error. stop filtering.");
+        !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.x) ||
+        !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.y) ||
+        !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.yaw) ||
+        !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.vx)) {
+        std::cout<<"path callback: filtering error. stop filtering."<<std::endl;
+  //    RCLCPP_DEBUG(m_logger, "path callback: filtering error. stop filtering.");
       mpc_traj_smoothed = mpc_traj_resampled;
     }
   }
@@ -226,7 +420,7 @@ void MPC::setReferenceTrajectory(
    */
   if (param.extend_trajectory_for_end_yaw_control) {
     MPCUtils::extendTrajectoryInYawDirection(
-      mpc_traj_raw.yaw.back(), param.traj_resample_dist, m_is_forward_shift, mpc_traj_smoothed);
+        mpc_traj_raw.yaw.back(), param.traj_resample_dist, m_is_forward_shift, mpc_traj_smoothed);
   }
 
   // calculate yaw angle
@@ -235,7 +429,7 @@ void MPC::setReferenceTrajectory(
 
   // calculate curvature
   MPCUtils::calcTrajectoryCurvature(
-    param.curvature_smoothing_num_traj, param.curvature_smoothing_num_ref_steer, mpc_traj_smoothed);
+      param.curvature_smoothing_num_traj, param.curvature_smoothing_num_ref_steer, mpc_traj_smoothed);
 
   // stop velocity at a terminal point
   mpc_traj_smoothed.vx.back() = 0.0;
@@ -247,18 +441,29 @@ void MPC::setReferenceTrajectory(
   mpc_traj_smoothed.push_back(last_point);
 
   if (!mpc_traj_smoothed.size()) {
-    RCLCPP_DEBUG(m_logger, "path callback: trajectory size is undesired.");
+    std::cout<<"path callback: trajectory size is undesired."<<std::endl;
+ //   RCLCPP_DEBUG(m_logger, "path callback: trajectory size is undesired.");
     return;
   }
 
   m_reference_trajectory = mpc_traj_smoothed;
 }
 
-void MPC::resetPrevResult(const SteeringReport & current_steer)
+//void MPC::resetPrevResult(const SteeringReport & current_steer)
+//{
+//  // Consider limit. The prev value larger than limitation brakes the optimization constraint and
+//  // results in optimization failure.
+//  const float steer_lim_f = static_cast<float>(m_steer_lim);
+//  m_raw_steer_cmd_prev = std::clamp(current_steer.steering_tire_angle, -steer_lim_f, steer_lim_f);
+//  m_raw_steer_cmd_pprev = std::clamp(current_steer.steering_tire_angle, -steer_lim_f, steer_lim_f);
+//}
+
+
+void MPC::resetPrevResult(const iv::ADCSteeringReport & current_steer)
 {
   // Consider limit. The prev value larger than limitation brakes the optimization constraint and
   // results in optimization failure.
-  const float steer_lim_f = static_cast<float>(m_steer_lim);
+  const float steer_lim_f = static_cast<float>(m_steer_lim); //
   m_raw_steer_cmd_prev = std::clamp(current_steer.steering_tire_angle, -steer_lim_f, steer_lim_f);
   m_raw_steer_cmd_pprev = std::clamp(current_steer.steering_tire_angle, -steer_lim_f, steer_lim_f);
 }
@@ -320,7 +525,8 @@ std::pair<bool, MPCData> MPC::getData(
   if (!MPCUtils::calcNearestPoseInterp(
           traj, current_pose, &(data.nearest_pose), &(data.nearest_idx), &(data.nearest_time),
           ego_nearest_dist_threshold, ego_nearest_yaw_threshold)) {
-    warn_throttle("calculateMPC: error in calculating nearest pose. stop mpc.");
+    std::cout<<"calculateMPC: error in calculating nearest pose. stop mpc."<<std::endl;
+//    warn_throttle("calculateMPC: error in calculating nearest pose. stop mpc.");
     return {false, MPCData{}};
   }
 
@@ -336,13 +542,15 @@ std::pair<bool, MPCData> MPC::getData(
   // check error limit
   const double dist_err = MPCUtils::ADCcalcDistance2d(current_pose.position, data.nearest_pose.position);
   if (dist_err > m_admissible_position_error) {
-    warn_throttle("Too large position error: %fm > %fm", dist_err, m_admissible_position_error);
+    printf("Too large position error: %fm > %fm", dist_err, m_admissible_position_error);
+  //  warn_throttle("Too large position error: %fm > %fm", dist_err, m_admissible_position_error);
     return {false, MPCData{}};
   }
 
   // check yaw error limit
   if (std::fabs(data.yaw_err) > m_admissible_yaw_error_rad) {
-    warn_throttle("Too large yaw error: %f > %f", data.yaw_err, m_admissible_yaw_error_rad);
+    printf("Too large yaw error: %f > %f", data.yaw_err, m_admissible_yaw_error_rad);
+//    warn_throttle("Too large yaw error: %f > %f", data.yaw_err, m_admissible_yaw_error_rad);
     return {false, MPCData{}};
   }
 
@@ -351,7 +559,8 @@ std::pair<bool, MPCData> MPC::getData(
       m_param.min_prediction_length / static_cast<double>(m_param.prediction_horizon - 1);
   auto end_time = data.nearest_time + m_param.input_delay + m_ctrl_period + max_prediction_time;
   if (end_time > traj.relative_time.back()) {
-    warn_throttle("path is too short for prediction.");
+    printf("path is too short for prediction.");
+   // warn_throttle("path is too short for prediction.");
     return {false, MPCData{}};
   }
   return {true, data};
@@ -366,7 +575,8 @@ std::pair<bool, MPCTrajectory> MPC::resampleMPCTrajectoryByTime(
     mpc_time_v.push_back(ts + i * prediction_dt);
   }
   if (!MPCUtils::linearInterpMPCTrajectory(input.relative_time, input, mpc_time_v, output)) {
-    warn_throttle("calculateMPC: mpc resample error. stop mpc calculation. check code!");
+    printf("calculateMPC: mpc resample error. stop mpc calculation. check code!");
+//    warn_throttle("calculateMPC: mpc resample error. stop mpc calculation. check code!");
     return {false, {}};
   }
   return {true, output};
@@ -394,10 +604,13 @@ VectorXd MPC::getInitialState(const MPCData & data)
     dlat = m_lpf_lateral_error.filter(dlat);
     dyaw = m_lpf_yaw_error.filter(dyaw);
     x0 << lat_err, dlat, yaw_err, dyaw;
-    RCLCPP_DEBUG(m_logger, "(before lpf) dot_lat_err = %f, dot_yaw_err = %f", dlat, dyaw);
-    RCLCPP_DEBUG(m_logger, "(after lpf) dot_lat_err = %f, dot_yaw_err = %f", dlat, dyaw);
+    std::cout<<"(before lpf) dot_lat_err = "<<dlat<<", dot_yaw_err = "<<dyaw<<std::endl;
+    std::cout<<"(after lpf) dot_lat_err = "<<dlat<<", dot_yaw_err = "<<dyaw<<std::endl;
+//    RCLCPP_DEBUG(m_logger, "(before lpf) dot_lat_err = %f, dot_yaw_err = %f", dlat, dyaw);
+//    RCLCPP_DEBUG(m_logger, "(after lpf) dot_lat_err = %f, dot_yaw_err = %f", dlat, dyaw);
   } else {
-    RCLCPP_ERROR(m_logger, "vehicle_model_type is undefined");
+    std::cout<<"vehicle_model_type is undefined"<<std::endl;
+//    RCLCPP_ERROR(m_logger, "vehicle_model_type is undefined");
   }
   return x0;
 }
@@ -422,7 +635,8 @@ std::pair<bool, VectorXd> MPC::updateStateForDelayCompensation(
       k = interpolation::lerp(traj.relative_time, traj.k, mpc_curr_time);
       v = interpolation::lerp(traj.relative_time, traj.vx, mpc_curr_time);
     } catch (const std::exception & e) {
-      RCLCPP_ERROR(m_logger, "mpc resample failed at delay compensation, stop mpc: %s", e.what());
+      std::cout<<"mpc resample failed at delay compensation, stop mpc: "<<e.what()<<std::endl;
+ //     RCLCPP_ERROR(m_logger, "mpc resample failed at delay compensation, stop mpc: %s", e.what());
       return {false, {}};
     }
 
@@ -438,22 +652,46 @@ std::pair<bool, VectorXd> MPC::updateStateForDelayCompensation(
   return {true, x_curr};
 }
 
+//MPCTrajectory MPC::applyVelocityDynamicsFilter(
+//  const MPCTrajectory & input, const Odometry & current_kinematics) const
+//{
+//  const auto autoware_traj = MPCUtils::convertToAutowareTrajectory(input);
+//  if (autoware_traj.points.empty()) {
+//    return input;
+//  }
+
+//  const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
+//    autoware_traj.points, current_kinematics.pose.pose, ego_nearest_dist_threshold,
+//    ego_nearest_yaw_threshold);
+
+//  MPCTrajectory output = input;
+//  MPCUtils::dynamicSmoothingVelocity(
+//    nearest_seg_idx, current_kinematics.twist.twist.linear.x, m_param.acceleration_limit,
+//    m_param.velocity_time_constant, output);
+
+//  auto last_point = output.back();
+//  last_point.relative_time += 100.0;  // extra time to prevent mpc calc failure due to short time
+//  last_point.vx = 0.0;                // stop velocity at a terminal point
+//  output.push_back(last_point);
+//  return output;
+//}
+
 MPCTrajectory MPC::applyVelocityDynamicsFilter(
-  const MPCTrajectory & input, const Odometry & current_kinematics) const
+    const MPCTrajectory & trajectory, const iv::ADCOdometry & current_kinematics) const
 {
-  const auto autoware_traj = MPCUtils::convertToAutowareTrajectory(input);
-  if (autoware_traj.points.empty()) {
-    return input;
+ // const auto autoware_traj = MPCUtils::convertToAutowareTrajectory(input);
+  if (trajectory.size() == 0) {
+    return trajectory;
   }
 
-  const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
-    autoware_traj.points, current_kinematics.pose.pose, ego_nearest_dist_threshold,
-    ego_nearest_yaw_threshold);
+  const size_t nearest_seg_idx = MPCUtils::findFirstNearestSegmentIndexWithSoftConstraints(
+      trajectory, current_kinematics.pose, ego_nearest_dist_threshold,
+      ego_nearest_yaw_threshold);
 
-  MPCTrajectory output = input;
+  MPCTrajectory output = trajectory;
   MPCUtils::dynamicSmoothingVelocity(
-    nearest_seg_idx, current_kinematics.twist.twist.linear.x, m_param.acceleration_limit,
-    m_param.velocity_time_constant, output);
+      nearest_seg_idx, current_kinematics.twist.linear_x, m_param.acceleration_limit,
+      m_param.velocity_time_constant, output);
 
   auto last_point = output.back();
   last_point.relative_time += 100.0;  // extra time to prevent mpc calc failure due to short time
@@ -555,7 +793,7 @@ MPCMatrix MPC::generateMPCMatrix(
     // get reference input (feed-forward)
     m_vehicle_model_ptr->setCurvature(ref_smooth_k);
     m_vehicle_model_ptr->calculateReferenceInput(Uref);
-    if (std::fabs(Uref(0, 0)) < tier4_autoware_utils::deg2rad(m_param.zero_ff_steer_deg)) {
+    if (std::fabs(Uref(0, 0)) < MPCUtils::deg2rad(m_param.zero_ff_steer_deg)) {
       Uref(0, 0) = 0.0;  // ignore curvature noise
     }
     m.Uref_ex.block(i * DIM_U, 0, DIM_U, 1) = Uref;
@@ -604,7 +842,8 @@ std::pair<bool, VectorXd> MPC::executeOptimization(
   VectorXd Uex;
 
   if (!isValid(m)) {
-    warn_throttle("model matrix is invalid. stop MPC.");
+    printf("model matrix is invalid. stop MPC.");
+//    warn_throttle("model matrix is invalid. stop MPC.");
     return {false, {}};
   }
 
@@ -642,17 +881,20 @@ std::pair<bool, VectorXd> MPC::executeOptimization(
   bool solve_result = m_qpsolver_ptr->solve(H, f.transpose(), A, lb, ub, lbA, ubA, Uex);
   auto t_end = std::chrono::system_clock::now();
   if (!solve_result) {
-    warn_throttle("qp solver error");
+    printf("qp solver error");
+//    warn_throttle("qp solver error");
     return {false, {}};
   }
 
   {
     auto t = std::chrono::duration_cast<std::chrono::milliseconds>(t_end - t_start).count();
-    RCLCPP_DEBUG(m_logger, "qp solver calculation time = %ld [ms]", t);
+    std::cout<<"qp solver calculation time = "<<t<<"[ms]";
+//    RCLCPP_DEBUG(m_logger, "qp solver calculation time = %ld [ms]", t);
   }
 
   if (Uex.array().isNaN().any()) {
-    warn_throttle("model Uex includes NaN, stop MPC.");
+    printf("model Uex includes NaN, stop MPC.");
+//    warn_throttle("model Uex includes NaN, stop MPC.");
     return {false, {}};
   }
   return {true, Uex};
@@ -727,36 +969,71 @@ void MPC::addSteerWeightF(const double prediction_dt, MatrixXd & f) const
   f(0, 1) += (2.0 * m_raw_steer_cmd_prev * steer_acc_r_cp1) * 0.5;
 }
 
+//double MPC::getPredictionDeltaTime(
+//  const double start_time, const MPCTrajectory & input, const Odometry & current_kinematics) const
+//{
+//  // Calculate the time min_prediction_length ahead from current_pose
+//  const auto autoware_traj = MPCUtils::convertToAutowareTrajectory(input);
+//  const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
+//    autoware_traj.points, current_kinematics.pose.pose, ego_nearest_dist_threshold,
+//    ego_nearest_yaw_threshold);
+//  double sum_dist = 0;
+//  const double target_time = [&]() {
+//    const double t_ext = 100.0;  // extra time to prevent mpc calculation failure due to short time
+//    for (size_t i = nearest_idx + 1; i < input.relative_time.size(); i++) {
+//      const double segment_dist = MPCUtils::calcDistance2d(input, i, i - 1);
+//      sum_dist += segment_dist;
+//      if (m_param.min_prediction_length < sum_dist) {
+//        const double prev_sum_dist = sum_dist - segment_dist;
+//        const double ratio = (m_param.min_prediction_length - prev_sum_dist) / segment_dist;
+//        const double relative_time_at_i = i == input.relative_time.size() - 1
+//                                            ? input.relative_time.at(i) - t_ext
+//                                            : input.relative_time.at(i);
+//        return input.relative_time.at(i - 1) +
+//               (relative_time_at_i - input.relative_time.at(i - 1)) * ratio;
+//      }
+//    }
+//    return input.relative_time.back() - t_ext;
+//  }();
+
+//  // Calculate delta time for min_prediction_length
+//  const double dt =
+//    (target_time - start_time) / static_cast<double>(m_param.prediction_horizon - 1);
+
+//  return std::max(dt, m_param.prediction_dt);
+//}
+
 double MPC::getPredictionDeltaTime(
-  const double start_time, const MPCTrajectory & input, const Odometry & current_kinematics) const
+    const double start_time, const MPCTrajectory & input,
+    const iv::ADCOdometry & current_kinematics) const
 {
   // Calculate the time min_prediction_length ahead from current_pose
-  const auto autoware_traj = MPCUtils::convertToAutowareTrajectory(input);
-  const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
-    autoware_traj.points, current_kinematics.pose.pose, ego_nearest_dist_threshold,
-    ego_nearest_yaw_threshold);
+  const size_t nearest_idx = MPCUtils::findFirstNearestIndexWithSoftConstraints(
+      input, current_kinematics.pose, ego_nearest_dist_threshold,
+      ego_nearest_yaw_threshold);
+
   double sum_dist = 0;
   const double target_time = [&]() {
-    const double t_ext = 100.0;  // extra time to prevent mpc calculation failure due to short time
-    for (size_t i = nearest_idx + 1; i < input.relative_time.size(); i++) {
-      const double segment_dist = MPCUtils::calcDistance2d(input, i, i - 1);
-      sum_dist += segment_dist;
-      if (m_param.min_prediction_length < sum_dist) {
-        const double prev_sum_dist = sum_dist - segment_dist;
-        const double ratio = (m_param.min_prediction_length - prev_sum_dist) / segment_dist;
-        const double relative_time_at_i = i == input.relative_time.size() - 1
-                                            ? input.relative_time.at(i) - t_ext
-                                            : input.relative_time.at(i);
-        return input.relative_time.at(i - 1) +
-               (relative_time_at_i - input.relative_time.at(i - 1)) * ratio;
+      const double t_ext = 100.0;  // extra time to prevent mpc calculation failure due to short time
+      for (size_t i = nearest_idx + 1; i < input.relative_time.size(); i++) {
+          const double segment_dist = MPCUtils::calcDistance2d(input, i, i - 1);
+          sum_dist += segment_dist;
+          if (m_param.min_prediction_length < sum_dist) {
+              const double prev_sum_dist = sum_dist - segment_dist;
+              const double ratio = (m_param.min_prediction_length - prev_sum_dist) / segment_dist;
+              const double relative_time_at_i = i == input.relative_time.size() - 1
+                                                    ? input.relative_time.at(i) - t_ext
+                                                    : input.relative_time.at(i);
+              return input.relative_time.at(i - 1) +
+                     (relative_time_at_i - input.relative_time.at(i - 1)) * ratio;
+          }
       }
-    }
-    return input.relative_time.back() - t_ext;
+      return input.relative_time.back() - t_ext;
   }();
 
   // Calculate delta time for min_prediction_length
   const double dt =
-    (target_time - start_time) / static_cast<double>(m_param.prediction_horizon - 1);
+      (target_time - start_time) / static_cast<double>(m_param.prediction_horizon - 1);
 
   return std::max(dt, m_param.prediction_dt);
 }

src/decition/mpccontroller/mpc_lateral_controller.cpp

@@ -35,14 +35,131 @@
 namespace autoware::motion::control::mpc_lateral_controller
 {
 
-MpcLateralController::MpcLateralController(rclcpp::Node & node)
-: clock_(node.get_clock()), logger_(node.get_logger().get_child("lateral_controller"))
+//MpcLateralController::MpcLateralController(rclcpp::Node & node)
+//: clock_(node.get_clock()), logger_(node.get_logger().get_child("lateral_controller"))
+//{
+//  const auto dp_int = [&](const std::string & s) { return node.declare_parameter<int>(s); };
+//  const auto dp_bool = [&](const std::string & s) { return node.declare_parameter<bool>(s); };
+//  const auto dp_double = [&](const std::string & s) { return node.declare_parameter<double>(s); };
+
+//  m_mpc.m_ctrl_period = node.get_parameter("ctrl_period").as_double();
+
+//  auto & p_filt = m_trajectory_filtering_param;
+//  p_filt.enable_path_smoothing = dp_bool("enable_path_smoothing");
+//  p_filt.path_filter_moving_ave_num = dp_int("path_filter_moving_ave_num");
+//  p_filt.curvature_smoothing_num_traj = dp_int("curvature_smoothing_num_traj");
+//  p_filt.curvature_smoothing_num_ref_steer = dp_int("curvature_smoothing_num_ref_steer");
+//  p_filt.traj_resample_dist = dp_double("traj_resample_dist");
+//  p_filt.extend_trajectory_for_end_yaw_control = dp_bool("extend_trajectory_for_end_yaw_control");
+
+//  m_mpc.m_admissible_position_error = dp_double("admissible_position_error");
+//  m_mpc.m_admissible_yaw_error_rad = dp_double("admissible_yaw_error_rad");
+//  m_mpc.m_use_steer_prediction = dp_bool("use_steer_prediction");
+//  m_mpc.m_param.steer_tau = dp_double("vehicle_model_steer_tau");
+
+//  /* stop state parameters */
+//  m_stop_state_entry_ego_speed = dp_double("stop_state_entry_ego_speed");
+//  m_stop_state_entry_target_speed = dp_double("stop_state_entry_target_speed");
+//  m_converged_steer_rad = dp_double("converged_steer_rad");
+//  m_keep_steer_control_until_converged = dp_bool("keep_steer_control_until_converged");
+//  m_new_traj_duration_time = dp_double("new_traj_duration_time");            // [s]
+//  m_new_traj_end_dist = dp_double("new_traj_end_dist");                      // [m]
+//  m_mpc_converged_threshold_rps = dp_double("mpc_converged_threshold_rps");  // [rad/s]
+
+//  /* mpc parameters */
+//  const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo();
+//  const double wheelbase = vehicle_info.wheel_base_m;
+//  constexpr double deg2rad = static_cast<double>(M_PI) / 180.0;
+//  m_mpc.m_steer_lim = vehicle_info.max_steer_angle_rad;
+
+//  // steer rate limit depending on curvature
+//  const auto steer_rate_lim_dps_list_by_curvature =
+//    node.declare_parameter<std::vector<double>>("steer_rate_lim_dps_list_by_curvature");
+//  const auto curvature_list_for_steer_rate_lim =
+//    node.declare_parameter<std::vector<double>>("curvature_list_for_steer_rate_lim");
+//  for (size_t i = 0; i < steer_rate_lim_dps_list_by_curvature.size(); ++i) {
+//    m_mpc.m_steer_rate_lim_map_by_curvature.emplace_back(
+//      curvature_list_for_steer_rate_lim.at(i),
+//      steer_rate_lim_dps_list_by_curvature.at(i) * deg2rad);
+//  }
+
+//  // steer rate limit depending on velocity
+//  const auto steer_rate_lim_dps_list_by_velocity =
+//    node.declare_parameter<std::vector<double>>("steer_rate_lim_dps_list_by_velocity");
+//  const auto velocity_list_for_steer_rate_lim =
+//    node.declare_parameter<std::vector<double>>("velocity_list_for_steer_rate_lim");
+//  for (size_t i = 0; i < steer_rate_lim_dps_list_by_velocity.size(); ++i) {
+//    m_mpc.m_steer_rate_lim_map_by_velocity.emplace_back(
+//      velocity_list_for_steer_rate_lim.at(i), steer_rate_lim_dps_list_by_velocity.at(i) * deg2rad);
+//  }
+
+//  /* vehicle model setup */
+//  auto vehicle_model_ptr =
+//    createVehicleModel(wheelbase, m_mpc.m_steer_lim, m_mpc.m_param.steer_tau, node);
+//  m_mpc.setVehicleModel(vehicle_model_ptr);
+
+//  /* QP solver setup */
+//  m_mpc.setVehicleModel(vehicle_model_ptr);
+//  auto qpsolver_ptr = createQPSolverInterface(node);
+//  m_mpc.setQPSolver(qpsolver_ptr);
+
+//  /* delay compensation */
+//  {
+//    const double delay_tmp = dp_double("input_delay");
+//    const double delay_step = std::round(delay_tmp / m_mpc.m_ctrl_period);
+//    m_mpc.m_param.input_delay = delay_step * m_mpc.m_ctrl_period;
+//    m_mpc.m_input_buffer = std::deque<double>(static_cast<size_t>(delay_step), 0.0);
+//  }
+
+//  /* steering offset compensation */
+//  enable_auto_steering_offset_removal_ =
+//    dp_bool("steering_offset.enable_auto_steering_offset_removal");
+//  steering_offset_ = createSteerOffsetEstimator(wheelbase, node);
+
+//  /* initialize low-pass filter */
+//  {
+//    const double steering_lpf_cutoff_hz = dp_double("steering_lpf_cutoff_hz");
+//    const double error_deriv_lpf_cutoff_hz = dp_double("error_deriv_lpf_cutoff_hz");
+//    m_mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz);
+//  }
+
+//  // ego nearest index search
+//  const auto check_and_get_param = [&](const auto & param) {
+//    return node.has_parameter(param) ? node.get_parameter(param).as_double() : dp_double(param);
+//  };
+//  m_ego_nearest_dist_threshold = check_and_get_param("ego_nearest_dist_threshold");
+//  m_ego_nearest_yaw_threshold = check_and_get_param("ego_nearest_yaw_threshold");
+//  m_mpc.ego_nearest_dist_threshold = m_ego_nearest_dist_threshold;
+//  m_mpc.ego_nearest_yaw_threshold = m_ego_nearest_yaw_threshold;
+
+//  m_pub_predicted_traj = node.create_publisher<Trajectory>("~/output/predicted_trajectory", 1);
+//  m_pub_debug_values =
+//    node.create_publisher<Float32MultiArrayStamped>("~/output/lateral_diagnostic", 1);
+//  m_pub_steer_offset = node.create_publisher<Float32Stamped>("~/output/estimated_steer_offset", 1);
+
+//  declareMPCparameters(node);
+
+//  /* get parameter updates */
+//  using std::placeholders::_1;
+//  m_set_param_res =
+//    node.add_on_set_parameters_callback(std::bind(&MpcLateralController::paramCallback, this, _1));
+
+//  m_mpc.initializeSteeringPredictor();
+
+//  m_mpc.setLogger(logger_);
+//  m_mpc.setClock(clock_);
+//}
+
+
+MpcLateralController::MpcLateralController()
 {
-  const auto dp_int = [&](const std::string & s) { return node.declare_parameter<int>(s); };
-  const auto dp_bool = [&](const std::string & s) { return node.declare_parameter<bool>(s); };
-  const auto dp_double = [&](const std::string & s) { return node.declare_parameter<double>(s); };
+    iv::xmlparam::Xmlparam xp("./mpc.xml");
+  const auto dp_int = [&](const std::string & s) { return xp.GetParam(s,1); };
+  const auto dp_bool = [&](const std::string & s) { return xp.GetParam(s,false); };
+  const auto dp_double = [&](const std::string & s) { return xp.GetParam(s,1.0); };
 
-  m_mpc.m_ctrl_period = node.get_parameter("ctrl_period").as_double();
+  m_mpc.m_ctrl_period = xp.GetParam("ctrl_period",1.0);
+//  m_mpc.m_ctrl_period = node.get_parameter("ctrl_period").as_double();
 
   auto & p_filt = m_trajectory_filtering_param;
   p_filt.enable_path_smoothing = dp_bool("enable_path_smoothing");
@@ -67,35 +184,36 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node)
   m_mpc_converged_threshold_rps = dp_double("mpc_converged_threshold_rps");  // [rad/s]
 
   /* mpc parameters */
-  const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo();
+ // const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo();
+  const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(xp).getVehicleInfo();
   const double wheelbase = vehicle_info.wheel_base_m;
   constexpr double deg2rad = static_cast<double>(M_PI) / 180.0;
   m_mpc.m_steer_lim = vehicle_info.max_steer_angle_rad;
 
   // steer rate limit depending on curvature
   const auto steer_rate_lim_dps_list_by_curvature =
-    node.declare_parameter<std::vector<double>>("steer_rate_lim_dps_list_by_curvature");
+      node.declare_parameter<std::vector<double>>("steer_rate_lim_dps_list_by_curvature");
   const auto curvature_list_for_steer_rate_lim =
-    node.declare_parameter<std::vector<double>>("curvature_list_for_steer_rate_lim");
+      node.declare_parameter<std::vector<double>>("curvature_list_for_steer_rate_lim");
   for (size_t i = 0; i < steer_rate_lim_dps_list_by_curvature.size(); ++i) {
     m_mpc.m_steer_rate_lim_map_by_curvature.emplace_back(
-      curvature_list_for_steer_rate_lim.at(i),
-      steer_rate_lim_dps_list_by_curvature.at(i) * deg2rad);
+        curvature_list_for_steer_rate_lim.at(i),
+        steer_rate_lim_dps_list_by_curvature.at(i) * deg2rad);
   }
 
   // steer rate limit depending on velocity
   const auto steer_rate_lim_dps_list_by_velocity =
-    node.declare_parameter<std::vector<double>>("steer_rate_lim_dps_list_by_velocity");
+      node.declare_parameter<std::vector<double>>("steer_rate_lim_dps_list_by_velocity");
   const auto velocity_list_for_steer_rate_lim =
-    node.declare_parameter<std::vector<double>>("velocity_list_for_steer_rate_lim");
+      node.declare_parameter<std::vector<double>>("velocity_list_for_steer_rate_lim");
   for (size_t i = 0; i < steer_rate_lim_dps_list_by_velocity.size(); ++i) {
     m_mpc.m_steer_rate_lim_map_by_velocity.emplace_back(
-      velocity_list_for_steer_rate_lim.at(i), steer_rate_lim_dps_list_by_velocity.at(i) * deg2rad);
+        velocity_list_for_steer_rate_lim.at(i), steer_rate_lim_dps_list_by_velocity.at(i) * deg2rad);
   }
 
   /* vehicle model setup */
   auto vehicle_model_ptr =
-    createVehicleModel(wheelbase, m_mpc.m_steer_lim, m_mpc.m_param.steer_tau, node);
+      createVehicleModel(wheelbase, m_mpc.m_steer_lim, m_mpc.m_param.steer_tau, node);
   m_mpc.setVehicleModel(vehicle_model_ptr);
 
   /* QP solver setup */
@@ -113,7 +231,7 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node)
 
   /* steering offset compensation */
   enable_auto_steering_offset_removal_ =
-    dp_bool("steering_offset.enable_auto_steering_offset_removal");
+      dp_bool("steering_offset.enable_auto_steering_offset_removal");
   steering_offset_ = createSteerOffsetEstimator(wheelbase, node);
 
   /* initialize low-pass filter */
@@ -125,7 +243,7 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node)
 
   // ego nearest index search
   const auto check_and_get_param = [&](const auto & param) {
-    return node.has_parameter(param) ? node.get_parameter(param).as_double() : dp_double(param);
+      return node.has_parameter(param) ? node.get_parameter(param).as_double() : dp_double(param);
   };
   m_ego_nearest_dist_threshold = check_and_get_param("ego_nearest_dist_threshold");
   m_ego_nearest_yaw_threshold = check_and_get_param("ego_nearest_yaw_threshold");
@@ -134,7 +252,7 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node)
 
   m_pub_predicted_traj = node.create_publisher<Trajectory>("~/output/predicted_trajectory", 1);
   m_pub_debug_values =
-    node.create_publisher<Float32MultiArrayStamped>("~/output/lateral_diagnostic", 1);
+      node.create_publisher<Float32MultiArrayStamped>("~/output/lateral_diagnostic", 1);
   m_pub_steer_offset = node.create_publisher<Float32Stamped>("~/output/estimated_steer_offset", 1);
 
   declareMPCparameters(node);
@@ -142,7 +260,7 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node)
   /* get parameter updates */
   using std::placeholders::_1;
   m_set_param_res =
-    node.add_on_set_parameters_callback(std::bind(&MpcLateralController::paramCallback, this, _1));
+      node.add_on_set_parameters_callback(std::bind(&MpcLateralController::paramCallback, this, _1));
 
   m_mpc.initializeSteeringPredictor();
 

src/decition/mpccontroller/mpc_lateral_controller/mpc.hpp

@@ -20,14 +20,14 @@
 #include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp"
 #include "mpc_lateral_controller/steering_predictor.hpp"
 #include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp"
-#include "rclcpp/rclcpp.hpp"
+//#include "rclcpp/rclcpp.hpp"
 
-#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp"
-#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
-#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
-#include "geometry_msgs/msg/pose.hpp"
-#include "nav_msgs/msg/odometry.hpp"
-#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp"
+//#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp"
+//#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
+//#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
+//#include "geometry_msgs/msg/pose.hpp"
+//#include "nav_msgs/msg/odometry.hpp"
+//#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp"
 
 #include <deque>
 #include <memory>
@@ -41,12 +41,12 @@
 namespace autoware::motion::control::mpc_lateral_controller
 {
 
-using autoware_auto_control_msgs::msg::AckermannLateralCommand;
-using autoware_auto_planning_msgs::msg::Trajectory;
-using autoware_auto_vehicle_msgs::msg::SteeringReport;
-using geometry_msgs::msg::Pose;
-using nav_msgs::msg::Odometry;
-using tier4_debug_msgs::msg::Float32MultiArrayStamped;
+//using autoware_auto_control_msgs::msg::AckermannLateralCommand;
+//using autoware_auto_planning_msgs::msg::Trajectory;
+//using autoware_auto_vehicle_msgs::msg::SteeringReport;
+//using geometry_msgs::msg::Pose;
+//using nav_msgs::msg::Odometry;
+//using tier4_debug_msgs::msg::Float32MultiArrayStamped;
 
 using Eigen::MatrixXd;
 using Eigen::VectorXd;
@@ -201,8 +201,8 @@ struct MPCMatrix
 class MPC
 {
 private:
-  rclcpp::Logger m_logger = rclcpp::get_logger("mpc_logger");  // ROS logger used for debug logging.
-  rclcpp::Clock::SharedPtr m_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);  // ROS clock.
+//  rclcpp::Logger m_logger = rclcpp::get_logger("mpc_logger");  // ROS logger used for debug logging.
+//  rclcpp::Clock::SharedPtr m_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);  // ROS clock.
 
   // Vehicle model used for MPC.
   std::shared_ptr<VehicleModelInterface> m_vehicle_model_ptr;
@@ -297,8 +297,11 @@ private:
    * @param current_kinematics The current vehicle kinematics.
    * @return The filtered trajectory.
    */
+//  MPCTrajectory applyVelocityDynamicsFilter(
+//    const MPCTrajectory & trajectory, const Odometry & current_kinematics) const;
+
   MPCTrajectory applyVelocityDynamicsFilter(
-    const MPCTrajectory & trajectory, const Odometry & current_kinematics) const;
+      const MPCTrajectory & trajectory, const iv::ADCOdometry & current_kinematics) const;
 
   /**
    * @brief Get the prediction time step for MPC. If the trajectory length is shorter than
@@ -308,9 +311,13 @@ private:
    * @param current_kinematics The current vehicle kinematics.
    * @return The prediction time step.
    */
+//  double getPredictionDeltaTime(
+//    const double start_time, const MPCTrajectory & input,
+//    const Odometry & current_kinematics) const;
+
   double getPredictionDeltaTime(
-    const double start_time, const MPCTrajectory & input,
-    const Odometry & current_kinematics) const;
+      const double start_time, const MPCTrajectory & input,
+      const iv::ADCOdometry & current_kinematics) const;
 
   /**
    * @brief Add weights related to lateral jerk, steering rate, and steering acceleration to the R
@@ -369,9 +376,13 @@ private:
    * @param Uex The optimized input vector.
    * @return The predicted trajectory.
    */
-  Trajectory calcPredictedTrajectory(
-    const MPCTrajectory & mpc_resampled_reference_trajectory, const MPCMatrix & mpc_matrix,
-    const VectorXd & x0_delayed, const VectorXd & Uex) const;
+//  Trajectory calcPredictedTrajectory(
+//    const MPCTrajectory & mpc_resampled_reference_trajectory, const MPCMatrix & mpc_matrix,
+//    const VectorXd & x0_delayed, const VectorXd & Uex) const;
+
+  MPCTrajectory calcPredictedTrajectory(
+      const MPCTrajectory & mpc_resampled_reference_trajectory, const MPCMatrix & mpc_matrix,
+      const VectorXd & x0_delayed, const VectorXd & Uex) const;
 
   /**
    * @brief Generate diagnostic data for debugging purposes.
@@ -383,10 +394,10 @@ private:
    * @param current_kinematics The current vehicle kinematics.
    * @return The generated diagnostic data.
    */
-  Float32MultiArrayStamped generateDiagData(
-    const MPCTrajectory & reference_trajectory, const MPCData & mpc_data,
-    const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex,
-    const Odometry & current_kinematics) const;
+//  Float32MultiArrayStamped generateDiagData(
+//    const MPCTrajectory & reference_trajectory, const MPCData & mpc_data,
+//    const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex,
+//    const Odometry & current_kinematics) const;
 
   /**
    * @brief calculate steering rate limit along with the target trajectory
@@ -397,19 +408,20 @@ private:
     const MPCTrajectory & trajectory, const double current_velocity) const;
 
   //!< @brief logging with warn and return false
-  template <typename... Args>
-  inline bool fail_warn_throttle(Args &&... args) const
-  {
-    RCLCPP_WARN_THROTTLE(m_logger, *m_clock, 3000, args...);
-    return false;
-  }
+//  template <typename... Args>
+//  inline bool fail_warn_throttle(Args &&... args) const
+//  {
+//    RCLCPP_WARN_THROTTLE(m_logger, *m_clock, 3000, args...);
+//    return false;
+//  }
 
   //!< @brief logging with warn
-  template <typename... Args>
-  inline void warn_throttle(Args &&... args) const
-  {
-    RCLCPP_WARN_THROTTLE(m_logger, *m_clock, 3000, args...);
-  }
+//  template <typename... Args>
+//  inline void warn_throttle(Args &&... args) const
+//  {
+
+//    RCLCPP_WARN_THROTTLE(m_logger, *m_clock, 3000, args...);
+//  }
 
 public:
   MPCTrajectory m_reference_trajectory;  // Reference trajectory to be followed.
@@ -445,25 +457,37 @@ public:
    * @param diagnostic Diagnostic data for debugging purposes.
    * @return True if the MPC calculation is successful, false otherwise.
    */
+//  bool calculateMPC(
+//    const SteeringReport & current_steer, const Odometry & current_kinematics,
+//    AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_trajectory,
+//    Float32MultiArrayStamped & diagnostic);
+
   bool calculateMPC(
-    const SteeringReport & current_steer, const Odometry & current_kinematics,
-    AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_trajectory,
-    Float32MultiArrayStamped & diagnostic);
+      const iv::ADCSteeringReport & current_steer, const iv::ADCOdometry & current_kinematics,
+      ADCAckermannLateralCommand & ctrl_cmd, MPCTrajectory & predicted_trajectory
+      );
 
   /**
    * @brief Set the reference trajectory to be followed.
    * @param trajectory_msg The reference trajectory message.
    * @param param Trajectory filtering parameters.
    */
+//  void setReferenceTrajectory(
+//    const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param,
+//    const Odometry & current_kinematics);
+
   void setReferenceTrajectory(
-    const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param,
-    const Odometry & current_kinematics);
+      const MPCTrajectory & trajectory_msg, const TrajectoryFilteringParam & param,
+      const iv::ADCOdometry & current_kinematics);
 
   /**
    * @brief Reset the previous result of MPC.
    * @param current_steer Current steering report.
    */
-  void resetPrevResult(const SteeringReport & current_steer);
+
+ // void resetPrevResult(const SteeringReport & current_steer);
+
+  void resetPrevResult(const iv::ADCSteeringReport & current_steer);
 
   /**
    * @brief Set the vehicle model for this MPC.
@@ -521,13 +545,13 @@ public:
    * @brief Set the RCLCPP logger to be used for logging.
    * @param logger The RCLCPP logger object.
    */
-  inline void setLogger(rclcpp::Logger logger) { m_logger = logger; }
+ // inline void setLogger(rclcpp::Logger logger) { m_logger = logger; }
 
   /**
    * @brief Set the RCLCPP clock to be used for time keeping.
    * @param clock The shared pointer to the RCLCPP clock.
    */
-  inline void setClock(rclcpp::Clock::SharedPtr clock) { m_clock = clock; }
+//  inline void setClock(rclcpp::Clock::SharedPtr clock) { m_clock = clock; }
 };  // class MPC
 }  // namespace autoware::motion::control::mpc_lateral_controller
 

src/decition/mpccontroller/mpc_lateral_controller/mpc_lateral_controller.hpp

@@ -19,18 +19,18 @@
 #include "mpc_lateral_controller/mpc_trajectory.hpp"
 #include "mpc_lateral_controller/mpc_utils.hpp"
 #include "mpc_lateral_controller/steering_offset/steering_offset.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "trajectory_follower_base/lateral_controller_base.hpp"
-
-#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp"
-#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
-#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
-#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp"
-#include "geometry_msgs/msg/pose.hpp"
-#include "geometry_msgs/msg/pose_stamped.hpp"
-#include "nav_msgs/msg/odometry.hpp"
-#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp"
-#include "tier4_debug_msgs/msg/float32_stamped.hpp"
+//#include "rclcpp/rclcpp.hpp"
+//#include "trajectory_follower_base/lateral_controller_base.hpp"
+
+//#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp"
+//#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
+//#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
+//#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp"
+//#include "geometry_msgs/msg/pose.hpp"
+//#include "geometry_msgs/msg/pose_stamped.hpp"
+//#include "nav_msgs/msg/odometry.hpp"
+//#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp"
+//#include "tier4_debug_msgs/msg/float32_stamped.hpp"
 
 #include <deque>
 #include <memory>
@@ -38,6 +38,8 @@
 #include <utility>
 #include <vector>
 
+#include "xmlparam.h"
+
 namespace autoware::motion::control::mpc_lateral_controller
 {
 
@@ -49,11 +51,13 @@ using nav_msgs::msg::Odometry;
 using tier4_debug_msgs::msg::Float32MultiArrayStamped;
 using tier4_debug_msgs::msg::Float32Stamped;
 
-class MpcLateralController : public trajectory_follower::LateralControllerBase
+//class MpcLateralController : public trajectory_follower::LateralControllerBase
+class MpcLateralController
 {
 public:
   /// \param node Reference to the node used only for the component and parameter initialization.
-  explicit MpcLateralController(rclcpp::Node & node);
+ // explicit MpcLateralController(rclcpp::Node & node);
+  explicit MpcLateralController();
   virtual ~MpcLateralController();
 
 private:
@@ -91,6 +95,7 @@ private:
   // trajectory buffer for detecting new trajectory
   std::deque<Trajectory> m_trajectory_buffer;
 
+
   MPC m_mpc;  // MPC object for trajectory following.
 
   // Check is mpc output converged

src/decition/mpccontroller/mpc_lateral_controller/mpc_utils.hpp

@@ -60,6 +60,24 @@ double ADCcalcYawDeviation(
 
 double ADCcalcDistance2d(iv::ADCPoint p1, iv::ADCPoint p2);
 
+double calcLongitudinalOffsetToSegment(
+    const MPCTrajectory & currenttraj, const size_t seg_idx, const iv::ADCPoint & p_target,
+    const bool throw_exception = false);
+
+
+size_t findFirstNearestSegmentIndexWithSoftConstraints(
+    const MPCTrajectory & currenttraj, const iv::ADCPose & pose,
+    const double dist_threshold = std::numeric_limits<double>::max(),
+    const double yaw_threshold = std::numeric_limits<double>::max());
+
+size_t findFirstNearestIndexWithSoftConstraints(
+    const MPCTrajectory & currenttraj, const iv::ADCPose & pose,
+    const double dist_threshold = std::numeric_limits<double>::max(),
+    const double yaw_threshold = std::numeric_limits<double>::max());
+
+bool isDrivingForward(const MPCTrajectory & currenttraj);
+
+constexpr double deg2rad(const double deg);
 /**
  * @brief calculate 2d distance from trajectory[idx1] to trajectory[idx2]
  */

src/decition/mpccontroller/mpc_lateral_controller/steering_predictor.hpp

@@ -28,6 +28,7 @@ class ADCAckermannLateralCommand
 public:
     double stamp;
     double steering_tire_angle;
+    double steering_tire_rotation_rate;
 };
 
 namespace autoware::motion::control::mpc_lateral_controller

src/decition/mpccontroller/mpc_utils.cpp

@@ -207,8 +207,8 @@ size_t findNearestIndex(const MPCTrajectory & currenttraj, const iv::ADCPoint &
  */
 size_t findFirstNearestIndexWithSoftConstraints(
     const MPCTrajectory & currenttraj, const iv::ADCPose & pose,
-    const double dist_threshold = std::numeric_limits<double>::max(),
-    const double yaw_threshold = std::numeric_limits<double>::max())
+    const double dist_threshold,
+    const double yaw_threshold )
 {
 
   {  // with dist and yaw thresholds
@@ -282,69 +282,70 @@ size_t findFirstNearestIndexWithSoftConstraints(
 }
 
 
-//MPCTrajectory removeOverlapPoints(const MPCTrajectory & points, const size_t start_idx = 0)
-//{
-//  if (points.size() < start_idx + 1) {
-//      return points;
-//  }
+MPCTrajectory removeOverlapPoints(const MPCTrajectory & points, const size_t start_idx = 0)
+{
+  if (points.size() < start_idx + 1) {
+      return points;
+  }
 
-//  MPCTrajectory dst;
+  MPCTrajectory dst;
 
-//  for (size_t i = 0; i <= start_idx; ++i) {
-//      dst.push_back(points.x[i],points.y[i],points.z[i],points.yaw[i],points.vx[i],points.k[i],points.smooth_k[i],points.relative_time[i]);
-//  }
+  for (size_t i = 0; i <= start_idx; ++i) {
+      dst.push_back(points.x[i],points.y[i],points.z[i],points.yaw[i],points.vx[i],points.k[i],points.smooth_k[i],points.relative_time[i]);
+  }
 
-//  constexpr double eps = 1.0E-08;
-//  for (size_t i = start_idx + 1; i < points.size(); ++i) {
-//      const auto prev_p = tier4_autoware_utils::getPoint(dst.back());
-//      const auto curr_p = tier4_autoware_utils::getPoint(points.at(i));
-//      const double dist =    tier4_autoware_utils::calcDistance2d(prev_p, curr_p);
-//      if (dist < eps) {
-//          continue;
-//      }
-//      dst.push_back(points.at(i));
-//  }
+  constexpr double eps = 1.0E-08;
+  for (size_t i = start_idx + 1; i < points.size(); ++i) {
+//      const auto prev_p = dst.back(); tier4_autoware_utils::getPoint(dst.back());
+//      const auto curr_p = points.x tier4_autoware_utils::getPoint(points.at(i));
+      const double dist = sqrt(pow(points.x[i] - dst.back().x,2)+pow(points.y[i] - dst.back().y,2));    // tier4_autoware_utils::calcDistance2d(prev_p, curr_p);
+      if (dist < eps) {
+          continue;
+      }
+      dst.push_back(points.x[i],points.y[i],points.z[i],points.yaw[i],points.vx[i],points.k[i],points.smooth_k[i],points.relative_time[i]);
+  //    dst.push_back(points.at(i));
+  }
 
-//  return dst;
-//}
+  return dst;
+}
 
-//double calcLongitudinalOffsetToSegment(
-//    MPCTrajectory & currenttraj, const size_t seg_idx, const iv::ADCPoint & p_target,
-//    const bool throw_exception = false)
-//{
-//  if (seg_idx >= currenttraj.size() - 1) {
-//      const std::out_of_range e("Segment index is invalid.");
-// //     tier4_autoware_utils::print_backtrace();
-//      std::cout<<" seg_idx > trajsize."<<std::endl;
-//      if (throw_exception) {
-//          throw e;
-//      }
-//      std::cerr << e.what() << std::endl;
-//      return std::nan("");
-//  }
+double calcLongitudinalOffsetToSegment(
+    const MPCTrajectory & currenttraj, const size_t seg_idx, const iv::ADCPoint & p_target,
+    const bool throw_exception)
+{
+  if (seg_idx >= currenttraj.size() - 1) {
+      const std::out_of_range e("Segment index is invalid.");
+ //     tier4_autoware_utils::print_backtrace();
+      std::cout<<" seg_idx > trajsize."<<std::endl;
+      if (throw_exception) {
+          throw e;
+      }
+      std::cerr << e.what() << std::endl;
+      return std::nan("");
+  }
 
-//  const auto overlap_removed_points = removeOverlapPoints(points, seg_idx);
+  const auto overlap_removed_points = removeOverlapPoints(currenttraj, seg_idx);
 
-//  if (throw_exception) {
-////      validateNonEmpty(overlap_removed_points);
-//  } else {
-//      try {
-//  //        validateNonEmpty(overlap_removed_points);
-//      } catch (const std::exception & e) {
-//          std::cerr << e.what() << std::endl;
-//          return std::nan("");
-//      }
-//  }
+  if (throw_exception) {
+//      validateNonEmpty(overlap_removed_points);
+  } else {
+      try {
+  //        validateNonEmpty(overlap_removed_points);
+      } catch (const std::exception & e) {
+          std::cerr << e.what() << std::endl;
+          return std::nan("");
+      }
+  }
 
-//  if (seg_idx >= overlap_removed_points.size() - 1) {
-//      const std::runtime_error e("Same points are given.");
-//  //    tier4_autoware_utils::print_backtrace();
-//      if (throw_exception) {
-//          throw e;
-//      }
-//      std::cerr << e.what() << std::endl;
-//      return std::nan("");
-//  }
+  if (seg_idx >= overlap_removed_points.size() - 1) {
+      const std::runtime_error e("Same points are given.");
+  //    tier4_autoware_utils::print_backtrace();
+      if (throw_exception) {
+          throw e;
+      }
+      std::cerr << e.what() << std::endl;
+      return std::nan("");
+  }
 
 //  const auto p_front = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx));
 //  const auto p_back = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx + 1));
@@ -352,34 +353,52 @@ size_t findFirstNearestIndexWithSoftConstraints(
 //  const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0};
 //  const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0};
 
-//  return segment_vec.dot(target_vec) / segment_vec.norm();
-//}
+  const Eigen::Vector3d segment_vec{overlap_removed_points.x[seg_idx + 1] - overlap_removed_points.x[seg_idx],
+                                    overlap_removed_points.y[seg_idx + 1] - overlap_removed_points.y[seg_idx], 0};
+  const Eigen::Vector3d target_vec{p_target.x - overlap_removed_points.x[seg_idx],
+                                   p_target.y - overlap_removed_points.y[seg_idx], 0};
 
-//size_t findFirstNearestSegmentIndexWithSoftConstraints(
-//    const MPCTrajectory & currenttraj, const iv::ADCPose & pose,
-//    const double dist_threshold = std::numeric_limits<double>::max(),
-//    const double yaw_threshold = std::numeric_limits<double>::max())
-//{
-//  // find first nearest index with soft constraints (not segment index)
-//  const size_t nearest_idx =
-//      findFirstNearestIndexWithSoftConstraints(currenttraj, pose, dist_threshold, yaw_threshold);
+  return segment_vec.dot(target_vec) / segment_vec.norm();
+}
 
-//  // calculate segment index
-//  if (nearest_idx == 0) {
-//      return 0;
-//  }
-//  if (nearest_idx == currenttraj.size() - 1) {
-//      return currenttraj.size() - 2;
-//  }
+size_t findFirstNearestSegmentIndexWithSoftConstraints(
+    const MPCTrajectory & currenttraj, const iv::ADCPose & pose,
+    const double dist_threshold ,
+    const double yaw_threshold )
+{
+  // find first nearest index with soft constraints (not segment index)
+  const size_t nearest_idx =
+      findFirstNearestIndexWithSoftConstraints(currenttraj, pose, dist_threshold, yaw_threshold);
 
-//  const double signed_length = calcLongitudinalOffsetToSegment(currenttraj, nearest_idx, pose.position);
+  // calculate segment index
+  if (nearest_idx == 0) {
+      return 0;
+  }
+  if (nearest_idx == currenttraj.size() - 1) {
+      return currenttraj.size() - 2;
+  }
 
-//  if (signed_length <= 0) {
-//      return nearest_idx - 1;
-//  }
+  const double signed_length = calcLongitudinalOffsetToSegment(currenttraj, nearest_idx, pose.position);
 
-//  return nearest_idx;
-//}
+  if (signed_length <= 0) {
+      return nearest_idx - 1;
+  }
+
+  return nearest_idx;
+}
+
+bool isDrivingForward(const MPCTrajectory & currenttraj)
+{
+  if(currenttraj.size() <2)return false;
+  const double src_yaw =  currenttraj.yaw[0];
+  const double pose_direction_yaw = currenttraj.yaw[1];
+  return std::fabs(ADCnormalizeRadian(src_yaw - pose_direction_yaw)) < M_PI/ 2.0;
+}
+
+constexpr double deg2rad(const double deg)
+{
+  return deg * M_PI / 180.0;
+}
 
 
 double calcDistance2d(const MPCTrajectory & trajectory, const size_t idx1, const size_t idx2)

src/decition/mpccontroller/testmpc.pro

@@ -19,6 +19,8 @@ SOURCES += \
         qp_solver/qp_solver_unconstraint_fast.cpp \
         steering_offset/steering_offset.cpp \
         steering_predictor.cpp \
+        vehicle_info.cpp \
+        vehicle_info_util.cpp \
         vehicle_model/vehicle_model_bicycle_dynamics.cpp \
         vehicle_model/vehicle_model_bicycle_kinematics.cpp \
         vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp \
@@ -49,6 +51,12 @@ HEADERS += \
     mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp \
     mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp \
     mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp \
-    mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp
+    mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp \
+    vehicle_info_util/vehicle_info.hpp \
+    vehicle_info_util/vehicle_info_util.hpp
 
 INCLUDEPATH += $$PWD/../../common/include
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}

src/decition/mpccontroller/vehicle_info.cpp

@@ -0,0 +1,88 @@
+// Copyright 2015-2021 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "vehicle_info_util/vehicle_info.hpp"
+
+namespace vehicle_info_util
+{
+//tier4_autoware_utils::LinearRing2d VehicleInfo::createFootprint(const double margin) const
+//{
+//  return createFootprint(margin, margin);
+//}
+
+//tier4_autoware_utils::LinearRing2d VehicleInfo::createFootprint(
+//  const double lat_margin, const double lon_margin) const
+//{
+//  using tier4_autoware_utils::LinearRing2d;
+//  using tier4_autoware_utils::Point2d;
+
+//  const double x_front = front_overhang_m + wheel_base_m + lon_margin;
+//  const double x_center = wheel_base_m / 2.0;
+//  const double x_rear = -(rear_overhang_m + lon_margin);
+//  const double y_left = wheel_tread_m / 2.0 + left_overhang_m + lat_margin;
+//  const double y_right = -(wheel_tread_m / 2.0 + right_overhang_m + lat_margin);
+
+//  LinearRing2d footprint;
+//  footprint.push_back(Point2d{x_front, y_left});
+//  footprint.push_back(Point2d{x_front, y_right});
+//  footprint.push_back(Point2d{x_center, y_right});
+//  footprint.push_back(Point2d{x_rear, y_right});
+//  footprint.push_back(Point2d{x_rear, y_left});
+//  footprint.push_back(Point2d{x_center, y_left});
+//  footprint.push_back(Point2d{x_front, y_left});
+
+//  return footprint;
+//}
+
+VehicleInfo createVehicleInfo(
+  const double wheel_radius_m, const double wheel_width_m, const double wheel_base_m,
+  const double wheel_tread_m, const double front_overhang_m, const double rear_overhang_m,
+  const double left_overhang_m, const double right_overhang_m, const double vehicle_height_m,
+  const double max_steer_angle_rad)
+{
+  // Calculate derived parameters
+  const double vehicle_length_m_ = front_overhang_m + wheel_base_m + rear_overhang_m;
+  const double vehicle_width_m_ = wheel_tread_m + left_overhang_m + right_overhang_m;
+  const double min_longitudinal_offset_m_ = -rear_overhang_m;
+  const double max_longitudinal_offset_m_ = front_overhang_m + wheel_base_m;
+  const double min_lateral_offset_m_ = -(wheel_tread_m / 2.0 + right_overhang_m);
+  const double max_lateral_offset_m_ = wheel_tread_m / 2.0 + left_overhang_m;
+  const double min_height_offset_m_ = 0.0;
+  const double max_height_offset_m_ = vehicle_height_m;
+
+  return VehicleInfo{
+    // Base parameters
+    wheel_radius_m,
+    wheel_width_m,
+    wheel_base_m,
+    wheel_tread_m,
+    front_overhang_m,
+    rear_overhang_m,
+    left_overhang_m,
+    right_overhang_m,
+    vehicle_height_m,
+    max_steer_angle_rad,
+    // Derived parameters
+    vehicle_length_m_,
+    vehicle_width_m_,
+    min_longitudinal_offset_m_,
+    max_longitudinal_offset_m_,
+    min_lateral_offset_m_,
+    max_lateral_offset_m_,
+    min_height_offset_m_,
+    max_height_offset_m_,
+  };
+}
+
+}  // namespace vehicle_info_util

src/decition/mpccontroller/vehicle_info_util.cpp

@@ -0,0 +1,83 @@
+// Copyright 2015-2021 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "vehicle_info_util/vehicle_info_util.hpp"
+
+#include <string>
+
+namespace
+{
+//template <class T>
+//T getParameter(rclcpp::Node & node, const std::string & name)
+//{
+//  if (node.has_parameter(name)) {
+//    return node.get_parameter(name).get_value<T>();
+//  }
+
+//  try {
+//    return node.declare_parameter<T>(name);
+//  } catch (const rclcpp::ParameterTypeException & ex) {
+//    RCLCPP_ERROR(
+//      node.get_logger(), "Failed to get parameter `%s`, please set it when you launch the node.",
+//      name.c_str());
+//    throw(ex);
+//  }
+//}
+
+double getParameter(iv::xmlparam::Xmlparam & xp, const std::string & name)
+{
+    return xp.GetParam(name,0.0);
+}
+
+}  // namespace
+
+namespace vehicle_info_util
+{
+//VehicleInfoUtil::VehicleInfoUtil(rclcpp::Node & node)
+//{
+//  vehicle_info_.wheel_radius_m = getParameter<double>(node, "wheel_radius");
+//  vehicle_info_.wheel_width_m = getParameter<double>(node, "wheel_width");
+//  vehicle_info_.wheel_base_m = getParameter<double>(node, "wheel_base");
+//  vehicle_info_.wheel_tread_m = getParameter<double>(node, "wheel_tread");
+//  vehicle_info_.front_overhang_m = getParameter<double>(node, "front_overhang");
+//  vehicle_info_.rear_overhang_m = getParameter<double>(node, "rear_overhang");
+//  vehicle_info_.left_overhang_m = getParameter<double>(node, "left_overhang");
+//  vehicle_info_.right_overhang_m = getParameter<double>(node, "right_overhang");
+//  vehicle_info_.vehicle_height_m = getParameter<double>(node, "vehicle_height");
+//  vehicle_info_.max_steer_angle_rad = getParameter<double>(node, "max_steer_angle");
+//}
+
+VehicleInfoUtil::VehicleInfoUtil(iv::xmlparam::Xmlparam & xp)
+{
+  vehicle_info_.wheel_radius_m = getParameter(xp,"wheel_radius");
+  vehicle_info_.wheel_width_m = getParameter(xp,"wheel_width");
+  vehicle_info_.wheel_base_m = getParameter(xp,"wheel_base");
+  vehicle_info_.wheel_tread_m = getParameter(xp,"wheel_tread");
+  vehicle_info_.front_overhang_m = getParameter(xp,"front_overhang");
+  vehicle_info_.rear_overhang_m = getParameter(xp,"rear_overhang");
+  vehicle_info_.left_overhang_m = getParameter(xp,"left_overhang");
+  vehicle_info_.right_overhang_m = getParameter(xp,"right_overhang");
+  vehicle_info_.vehicle_height_m = getParameter(xp,"vehicle_height");
+  vehicle_info_.max_steer_angle_rad = getParameter(xp,"max_steer_angle");
+}
+
+VehicleInfo VehicleInfoUtil::getVehicleInfo()
+{
+  return createVehicleInfo(
+    vehicle_info_.wheel_radius_m, vehicle_info_.wheel_width_m, vehicle_info_.wheel_base_m,
+    vehicle_info_.wheel_tread_m, vehicle_info_.front_overhang_m, vehicle_info_.rear_overhang_m,
+    vehicle_info_.left_overhang_m, vehicle_info_.right_overhang_m, vehicle_info_.vehicle_height_m,
+    vehicle_info_.max_steer_angle_rad);
+}
+}  // namespace vehicle_info_util

src/decition/mpccontroller/vehicle_info_util/vehicle_info.hpp

@@ -0,0 +1,64 @@
+// Copyright 2015-2021 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef VEHICLE_INFO_UTIL__VEHICLE_INFO_HPP_
+#define VEHICLE_INFO_UTIL__VEHICLE_INFO_HPP_
+
+//#include "tier4_autoware_utils/geometry/boost_geometry.hpp"
+
+namespace vehicle_info_util
+{
+/// Data class for vehicle info
+struct VehicleInfo
+{
+  // Base parameters. These describe the vehicle's bounding box and the
+  // position and radius of the wheels.
+  double wheel_radius_m;
+  double wheel_width_m;
+  double wheel_base_m;
+  double wheel_tread_m;
+  double front_overhang_m;
+  double rear_overhang_m;
+  double left_overhang_m;
+  double right_overhang_m;
+  double vehicle_height_m;
+  double max_steer_angle_rad;
+
+  // Derived parameters, i.e. calculated from base parameters
+  // The offset values are relative to the base frame origin, which is located
+  // on the ground below the middle of the rear axle, and can be negative.
+  double vehicle_length_m;
+  double vehicle_width_m;
+  double min_longitudinal_offset_m;
+  double max_longitudinal_offset_m;
+  double min_lateral_offset_m;
+  double max_lateral_offset_m;
+  double min_height_offset_m;
+  double max_height_offset_m;
+
+//  tier4_autoware_utils::LinearRing2d createFootprint(const double margin = 0.0) const;
+//  tier4_autoware_utils::LinearRing2d createFootprint(
+//    const double lat_margin, const double lon_margin) const;
+};
+
+/// Create vehicle info from base parameters
+VehicleInfo createVehicleInfo(
+  const double wheel_radius_m, const double wheel_width_m, const double wheel_base_m,
+  const double wheel_tread_m, const double front_overhang_m, const double rear_overhang_m,
+  const double left_overhang_m, const double right_overhang_m, const double vehicle_height_m,
+  const double max_steer_angle_rad);
+
+}  // namespace vehicle_info_util
+
+#endif  // VEHICLE_INFO_UTIL__VEHICLE_INFO_HPP_

src/decition/mpccontroller/vehicle_info_util/vehicle_info_util.hpp

@@ -0,0 +1,47 @@
+// Copyright 2015-2021 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef VEHICLE_INFO_UTIL__VEHICLE_INFO_UTIL_HPP_
+#define VEHICLE_INFO_UTIL__VEHICLE_INFO_UTIL_HPP_
+
+#include "vehicle_info_util/vehicle_info.hpp"
+
+//#include <rclcpp/rclcpp.hpp>
+
+#include "xmlparam.h"
+
+namespace vehicle_info_util
+{
+/// This is a convenience class for saving you from declaring all parameters
+/// manually and calculating derived parameters.
+/// This class supposes that necessary parameters are set when the node is launched.
+class VehicleInfoUtil
+{
+public:
+  /// Constructor
+//  explicit VehicleInfoUtil(rclcpp::Node & node);
+
+  explicit VehicleInfoUtil(iv::xmlparam::Xmlparam & xp);
+
+  /// Get vehicle info
+  VehicleInfo getVehicleInfo();
+
+private:
+  /// Buffer for base parameters
+  VehicleInfo vehicle_info_;
+};
+
+}  // namespace vehicle_info_util
+
+#endif  // VEHICLE_INFO_UTIL__VEHICLE_INFO_UTIL_HPP_

src/detection/detection_lidar_ukf_pda/detection_lidar_ukf_pda.pro

@@ -1,6 +1,6 @@
 QT -= gui
 
-CONFIG += c++11 console
+CONFIG += c++14 console
 CONFIG -= app_bundle
 
 # The following define makes your compiler emit warnings if you use

src/detection/laneATT_trt/laneATT_trt.pro

@@ -49,7 +49,7 @@ LIBS += /usr/lib/aarch64-linux-gnu/libopencv_highgui.so \
         /usr/lib/aarch64-linux-gnu/libopencv_videoio.so \
         /usr/lib/aarch64-linux-gnu/libopencv_video.so \
         /usr/lib/aarch64-linux-gnu/libopencv_calib3d.so \
-        /usr/local/cuda-10.2/lib64/*.so
+        /usr/local/cuda/lib64/*.so
 #LIBS += -L/usr/local/lib -lboost_system \
 
 

src/fusion/lidar_radar_fusion_cnn/lidar_radar_fusion_cnn.pro

@@ -77,9 +77,9 @@ INCLUDEPATH += $$PWD/../../include/msgtype
 #}
 
 INCLUDEPATH += /usr/include/opencv4/
-#LIBS += /usr/lib/aarch64-linux-gnu/libopencv*.so
+LIBS += /usr/lib/aarch64-linux-gnu/libopencv*.so
 
-LIBS += /usr/lib/x86_64-linux-gnu/libopencv*.so
+#LIBS += /usr/lib/x86_64-linux-gnu/libopencv*.so
 
 
 #LIBS += /usr/lib/libopencv*.so

src/tool/picview/picview.pro

@@ -11,6 +11,8 @@ greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
 TARGET = picview
 TEMPLATE = app
 
+CONFIG += c++14
+
 
 # The following define makes your compiler emit warnings if you use
 # any feature of Qt which has been marked as deprecated (the exact warnings

src/tool/view_ndtmatching/view_ndtmatching.pro

@@ -2,7 +2,7 @@ QT       += core gui
 
 greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
 
-CONFIG += c++11 # console
+CONFIG += c++14 # console
 CONFIG -= app_bundle
 
 # The following define makes your compiler emit warnings if you use

src/tool/view_pcdmap/view_pcdmap.pro

@@ -55,5 +55,5 @@ LIBS += -lboost_system
 #LIBS += -lvtkCommonExecutionModel-6.3 -lvtkCommonCore-6.3 -lvtkRenderingLOD-6.3 -lvtkRenderingCore-6.3 \
 #        -lvtkFiltersSources-6.3
 
-#LIBS += -lvtkCommonExecutionModel-7.1 -lvtkCommonCore-7.1 -lvtkRenderingLOD-7.1 -lvtkRenderingCore-7.1 \
-#        -lvtkFiltersSources-7.1
+LIBS += -lvtkCommonExecutionModel-7.1 -lvtkCommonCore-7.1 -lvtkRenderingLOD-7.1 -lvtkRenderingCore-7.1 \
+        -lvtkFiltersSources-7.1

src/tool/view_showxodrinvtk/showxodrinvtk.pro

@@ -13,7 +13,7 @@ DEFINES += NDT_CPU_LIBRARY
 
 VERSION = 1.0.1
 CONFIG += plugin
-CONFIG += c++11
+CONFIG += c++14
 
 
 SOURCES +=  const.cpp \