add mpc_follower.

src/driver/driver_autoware_computewaypoints/computewaypoints.cpp

@@ -220,7 +220,31 @@ int  ComputeWayPoints::FindWayPoints(iv::map::tracemap &xtracemap)
 
 
     std::cout<<" waypoint size : "<<xtracemap.point_size()<<std::endl;
-     mMutexTraceMap.unlock();
+    mMutexTraceMap.unlock();
+
+    double fyaw = (90 - xgpsimu.heading())*M_PI/180.0;
+    while(fyaw < 0)fyaw = fyaw + 2.0 * M_PI;
+
+    std::cout<<" fyaw : "<<fyaw<< "  0: "<<xtracemap.mutable_point(0)->rel_yaw()<<std::endl;
+
+    bool bChangeToLocal = false;
+    if(bChangeToLocal)
+    {
+        int i;
+        for(i=0;i<xtracemap.point_size();i++)
+        {
+            iv::map::mappoint * ppoint = xtracemap.mutable_point(i);
+            double fdis = sqrt(pow(ppoint->gps_x()-fnowx,2) + pow(ppoint->gps_y()-fnowy,2) );
+            double fyawdiff = ppoint->rel_yaw() - fyaw;
+            while(fyawdiff<0)fyawdiff = fyawdiff + 2.0*M_PI;
+            while(fyawdiff>=(2.0*M_PI))fyawdiff = fyawdiff - 2.0*M_PI;
+            double fx = fdis * cos(fyawdiff);
+            double fy = fdis * sin(fyawdiff);
+            ppoint->set_rel_x(fx);
+            ppoint->set_rel_y(fy);
+            ppoint->set_rel_yaw(fyawdiff);
+        }
+    }
 
     qint64 ncalctime =  std::chrono::system_clock::now().time_since_epoch().count() - ncompstart;
     double fcalc = ncalctime/(1000000.0);

src/driver/driver_gpsimutoodom/main.cpp

@@ -115,7 +115,7 @@ void ListenGPSIMU(const char * strdata,const unsigned int nSize,const unsigned i
     pitch = xgpsimu.pitch() * M_PI/180.0;
     roll = xgpsimu.roll() * M_PI/180.0;
     double hdg_o = (90 - ghead0)*M_PI/180.0;
-    yaw = (90 - xgpsimu.heading())*M_PI/180.0  - hdg_o;
+    yaw = (90 - xgpsimu.heading())*M_PI/180.0;//  - hdg_o;
     while(yaw< 0)yaw = yaw +2.0*M_PI;
     while(yaw>=(2.0*M_PI))yaw = yaw - 2.0*M_PI;
     Quaternion quat = ToQuaternion(yaw,pitch,roll);

src/ros/catkin/src/mpc_follower/CMakeLists.txt

@@ -0,0 +1,98 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mpc_follower)
+
+find_package(
+  catkin REQUIRED COMPONENTS
+  amathutils_lib
+  autoware_build_flags
+  autoware_health_checker
+  autoware_msgs
+  geometry_msgs
+  qpoases_vendor
+  roscpp
+  sensor_msgs
+  std_msgs
+  tf
+  tf2
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${qpoases_vendor_INCLUDE_DIRS}/qpoases_vendor
+)
+
+set(
+  MPC_FOLLOWER_SRC
+    src/mpc_utils.cpp
+    src/mpc_trajectory.cpp
+    src/lowpass_filter.cpp
+    src/vehicle_model/vehicle_model_interface.cpp
+    src/vehicle_model/vehicle_model_bicycle_kinematics.cpp
+    src/vehicle_model/vehicle_model_bicycle_dynamics.cpp
+    src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp
+    src/qp_solver/qp_solver_unconstr.cpp
+    src/qp_solver/qp_solver_unconstr_fast.cpp
+    src/qp_solver/qp_solver_qpoases.cpp
+)
+
+add_executable(mpc_follower src/mpc_follower_node.cpp src/mpc_follower_core.cpp ${MPC_FOLLOWER_SRC})
+add_dependencies(mpc_follower ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(mpc_follower ${catkin_LIBRARIES})
+
+add_executable(mpc_waypoints_converter src/mpc_waypoints_converter.cpp)
+add_dependencies(mpc_waypoints_converter ${catkin_EXPORTED_TARGETS})
+target_link_libraries(mpc_waypoints_converter ${catkin_LIBRARIES})
+
+install(
+  TARGETS mpc_follower mpc_waypoints_converter
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(
+  DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+install(
+  DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+)
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+
+  add_rostest_gtest(
+    test-mpc_utils
+    test/test_mpc_utils.test
+    test/src/test_mpc_utils.cpp
+    ${MPC_FOLLOWER_SRC}
+  )
+  add_dependencies(test-mpc_utils ${catkin_EXPORTED_TARGETS})
+  target_link_libraries(test-mpc_utils ${catkin_LIBRARIES})
+
+  add_rostest_gtest(
+    test-mpc_follower
+    test/test_mpc_follower.test
+    test/src/test_mpc_follower.cpp
+    src/mpc_follower_core.cpp
+    ${MPC_FOLLOWER_SRC}
+  )
+  add_dependencies(test-mpc_follower ${catkin_EXPORTED_TARGETS})
+  target_link_libraries(test-mpc_follower ${catkin_LIBRARIES})
+
+  add_rostest_gtest(
+    test-mpc_lowpass_filter
+    test/test_mpc_lowpass_filter.test
+    test/src/test_mpc_lowpass_filter.cpp
+    src/lowpass_filter.cpp
+  )
+  add_dependencies(test-mpc_lowpass_filter ${catkin_EXPORTED_TARGETS})
+  target_link_libraries(test-mpc_lowpass_filter ${catkin_LIBRARIES})
+endif()

src/ros/catkin/src/mpc_follower/README.md

@@ -0,0 +1,117 @@
+# MPC follower description
+
+## Overview
+A waypoint follower based on model predictive control (MPC) for accurate path tracking. This can be used as a waypoint_follower, as well as other path following nodes like pure_pursuit.
+
+
+There are 2 nodes related to MPC follower.
+ - `/mpc_waypoint_converter` : converts `/final_waypoints` to `/mpc_waypoints` which includes waypoints behind the self position. This is to solve temporary conflict of planning system and mpc follower so that mpc follower can be used in the same way as pure_pursuit. This will be removed in a future release.
+ - `/mpc_follower` : generates control command (`/twist_raw` or/and `/ctrl_raw`) to follow `/mpc_waypoints`.
+
+[Video](https://www.youtube.com/watch?v=4IO1zxsY4wU&t=18s) : comparison of pure_pursuit and mpc_follower with gazebo simulation. 
+
+
+
+## Input and Output for mpc_follower
+- input
+    - /mpc_waypoints : reference waypoints (generated in mpc_waypoints_converter)
+    - /current_pose : self pose
+    - /vehicle_status : vehicle information (as velocity and steering angle source)
+- output
+    - /twist_raw : command for vehicle
+    - /ctrl_raw : command for vehicle
+
+
+## node graph
+
+<img src="./media/mpc_rqt_graph.png">
+
+
+# Parameter description
+
+## Notation
+The default parameters are adjusted to the Autonomoustuff Lexus RX 450h for under 40 km/h driving.
+
+
+## overall 
+
+|Name|Type|Description|Default value|
+|:---|:---|:---|:---|
+|show_debug_info|bool|display debug info|false|
+|ctrl_period|double|control period [s]|0.03|
+|traj_resample_dist|double|distance of waypoints in resampling [m]|0.1|
+|enable_path_smoothing|bool|path smoothing flag. This should be true when uses path resampling to reduce resampling noise.|true|
+|enable_yaw_recalculation|bool|recalculate yaw angle after resampling. Set true if yaw in received waypoints is noisy.|false|
+|path_filter_moving_ave_num|int|number of data points moving average filter for path smoothing|35|
+|path_smoothing_times|int|number of times of applying path smoothing filter|1|
+|curvature_smoothing_num|double|index distance of points used in curvature calculation: p(i-num), p(i), p(i+num). larger num makes less noisy values.|35|
+|steering_lpf_cutoff_hz|double| cutoff frequency of lowpass filter for steering output command [hz]|3.0|
+|admisible_position_error|double| stop vehicle when following position error is larger than this value [m].|5.0|
+|admisible_yaw_error_deg|double|stop vehicle when following yaw angle error is larger than this value [deg].|90.0|
+
+## mpc algorithm 
+
+|Name|Type|Description|Default value|
+|:---|:---|:---|:---|
+|qp_solver_type|string|QP solver option. described below in detail.|unconstraint_fast|
+|qpoases_max_iter|int|maximum iteration number for convex optimiaztion with qpoases.|500|
+|vehicle_model_type|string|vehicle model option. described below in detail.|kinematics|
+|prediction_horizon|int|total prediction step for MPC|70|
+|prediction_sampling_time|double|prediction period for one step [s]|0.1|
+|weight_lat_error|double|weight for lateral error|0.1|
+|weight_heading_error|double|weight for heading error|0.0|
+|weight_heading_error_squared_vel_coeff|double|weight for heading error * velocity|5.0|
+|weight_steering_input|double|weight for steering error (steer command - reference steer)|1.0|
+|weight_steering_input_squared_vel_coeff|double|weight for steering error (steer command - reference steer) * velocity|0.25|
+|weight_lat_jerk|double|weight for lateral jerk (steer(i) - steer(i-1)) * velocity|0.0|
+|weight_terminal_lat_error|double|terminal cost weight for lateral error|1.0|
+|weight_terminal_heading_error|double|terminal cost weight for heading error|0.1|
+|zero_ff_steer_deg|double|threshold of feedforward angle [deg]. feedforward angle smaller than this value is set to zero.|2.0|
+
+
+## vehicle
+
+|Name|Type|Description|Default value|
+|:---|:---|:---|:---|
+|wheelbase|double|wheel base length for vehicle model [m] |2.9|
+|steering_tau|double|steering dynamics time constant (1d approximation) for vehicle model [s]|0.3|
+|steer_lim_deg|double|steering angle limit for vehicle model [deg]. This is also used for QP constraint.|35.0|
+
+## QP solver type
+
+currently, the options are
+- unconstraint : use least square method to solve unconstraint QP with eigen.
+- unconstraint_fast : similar to unconstraint. This is faster, but lower accuracy for optimization.
+- qpoases_hotstart : use QPOASES with hotstart for constrainted QP.
+
+## vehicle model type
+
+- kinematics : bicycle kinematics model with steering 1st-order delay
+- kinematics_no_delay : bicycle kinematics model without steering delay
+- dynamics : bicycle dynamics model considering slip angle
+
+The `kinematics` model are being used as default. Please see the reference[1] for more detail.
+
+# how to tune MPC parameters
+
+1. Set appropriate vehicle kinematics parameters `wheelbase`, `steering_gear_ratio`, and `steer_lim_deg`. Also check the `/vehicle_status` topic has appropriate values (speed: vehicle rear-wheels-center velocity [km/h], angle: steering (tire) angle [rad]). These values give a vehicle information to the controller for path following. Errors in these values cause fundamental tracking error. Whether these values are correct can be confirmed by comparing the angular velocity obtained from the model (`/mpc_follower/debug/angvel_from_steer`) and the actual angular velocity (such as `/estimate_twist/angular/z`).
+
+2. Set appropriate vehicle dynamics parameters of `steering_tau`, which is approximated delay from steering angle command to actual steering angle.
+
+3. Set `weight_steering_input` = 1.0, `weight_lat_error` = 0.1, and other weights to 0. If the vehicle oscillates when driving with low speed, set `weight_lat_error` smaller.
+
+4. Adjust other weights. One of the simple way for tuning is to increase `weight_lat_error` until oscillation occurs. If the vehicle is unstable with very small `weight_lat_error`, increase terminal weight :  `weight_terminal_lat_error` and `weight_terminal_heading_error` to improve tracking stability.
+ Larger `prediction_horizon` and smaller `prediction_sampling_time` is effective for tracking performance, but it is a trade-off between computational costs.
+Other parameters can be adjusted like below.
+ - `weight_lat_error`: Reduce lateral tracking error. This acts like P gain in PID.
+ - `weight_heading_error`: Make a drive straight. This acts like D gain in PID.
+ - `weight_heading_error_squared_vel_coeff` : Make a drive straight in high speed range.
+ - `weight_steering_input`: Reduce oscillation of tracking.
+ - `weight_steering_input_squared_vel_coeff`: Reduce oscillation of tracking in high speed range.
+ - `weight_lat_jerk`: Reduce lateral jerk.
+ - `weight_terminal_lat_error`: Preferable to set a higher value than normal lateral weight `weight_lat_error` for stability.
+ - `weight_terminal_heading_error`: Preferable to set a higher value than normal heading weight `weight_heading_error` for stability.
+
+ # reference 
+
+ [1] Jarrod M. Snider, "Automatic Steering Methods for Autonomous Automobile Path Tracking", Robotics Institute, Carnegie Mellon University, February 2009.

src/ros/catkin/src/mpc_follower/config/rqt_multiplot_mpc_follower.xml

@@ -0,0 +1,572 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<rqt_multiplot>
+    <table>
+        <background_color>#262626</background_color>
+        <foreground_color>#b1b1b1</foreground_color>
+        <link_cursor>false</link_cursor>
+        <link_scale>false</link_scale>
+        <plots>
+            <row_0>
+                <column_0>
+                    <axes>
+                        <axes>
+                            <x_axis>
+                                <custom_title>Untitled Axis</custom_title>
+                                <title_type>0</title_type>
+                                <title_visible>true</title_visible>
+                            </x_axis>
+                            <y_axis>
+                                <custom_title>Untitled Axis</custom_title>
+                                <title_type>0</title_type>
+                                <title_visible>true</title_visible>
+                            </y_axis>
+                        </axes>
+                    </axes>
+                    <curves>
+                        <curve_0>
+                            <axes>
+                                <x_axis>
+                                    <field></field>
+                                    <field_type>1</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/mpc_follower/debug/debug_values</topic>
+                                    <type>std_msgs/Float64MultiArray</type>
+                                </x_axis>
+                                <y_axis>
+                                    <field>data/5</field>
+                                    <field_type>0</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/mpc_follower/debug/debug_values</topic>
+                                    <type>std_msgs/Float64MultiArray</type>
+                                </y_axis>
+                            </axes>
+                            <color>
+                                <custom_color>#000000</custom_color>
+                                <type>0</type>
+                            </color>
+                            <data>
+                                <circular_buffer_capacity>1000</circular_buffer_capacity>
+                                <time_frame_length>30</time_frame_length>
+                                <type>3</type>
+                            </data>
+                            <style>
+                                <lines_interpolate>false</lines_interpolate>
+                                <pen_style>1</pen_style>
+                                <pen_width>2</pen_width>
+                                <render_antialias>false</render_antialias>
+                                <steps_invert>false</steps_invert>
+                                <sticks_baseline>0</sticks_baseline>
+                                <sticks_orientation>2</sticks_orientation>
+                                <type>0</type>
+                            </style>
+                            <subscriber_queue_size>100</subscriber_queue_size>
+                            <title>lateral</title>
+                        </curve_0>
+                        <curve_1>
+                            <axes>
+                                <x_axis>
+                                    <field></field>
+                                    <field_type>1</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/mpc_follower/debug/debug_values</topic>
+                                    <type>std_msgs/Float64MultiArray</type>
+                                </x_axis>
+                                <y_axis>
+                                    <field>data/8</field>
+                                    <field_type>0</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/mpc_follower/debug/debug_values</topic>
+                                    <type>std_msgs/Float64MultiArray</type>
+                                </y_axis>
+                            </axes>
+                            <color>
+                                <custom_color>#000000</custom_color>
+                                <type>0</type>
+                            </color>
+                            <data>
+                                <circular_buffer_capacity>1000</circular_buffer_capacity>
+                                <time_frame_length>30</time_frame_length>
+                                <type>3</type>
+                            </data>
+                            <style>
+                                <lines_interpolate>false</lines_interpolate>
+                                <pen_style>1</pen_style>
+                                <pen_width>2</pen_width>
+                                <render_antialias>false</render_antialias>
+                                <steps_invert>false</steps_invert>
+                                <sticks_baseline>0</sticks_baseline>
+                                <sticks_orientation>2</sticks_orientation>
+                                <type>0</type>
+                            </style>
+                            <subscriber_queue_size>100</subscriber_queue_size>
+                            <title>yaw error</title>
+                        </curve_1>
+                        <curve_2>
+                            <axes>
+                                <x_axis>
+                                    <field></field>
+                                    <field_type>1</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/mpc_follower/debug/debug_values</topic>
+                                    <type>std_msgs/Float64MultiArray</type>
+                                </x_axis>
+                                <y_axis>
+                                    <field>data/0</field>
+                                    <field_type>0</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/mpc_follower/debug/debug_values</topic>
+                                    <type>std_msgs/Float64MultiArray</type>
+                                </y_axis>
+                            </axes>
+                            <color>
+                                <custom_color>#000000</custom_color>
+                                <type>0</type>
+                            </color>
+                            <data>
+                                <circular_buffer_capacity>1000</circular_buffer_capacity>
+                                <time_frame_length>30</time_frame_length>
+                                <type>3</type>
+                            </data>
+                            <style>
+                                <lines_interpolate>false</lines_interpolate>
+                                <pen_style>1</pen_style>
+                                <pen_width>2</pen_width>
+                                <render_antialias>false</render_antialias>
+                                <steps_invert>false</steps_invert>
+                                <sticks_baseline>0</sticks_baseline>
+                                <sticks_orientation>2</sticks_orientation>
+                                <type>0</type>
+                            </style>
+                            <subscriber_queue_size>100</subscriber_queue_size>
+                            <title>steer cmd</title>
+                        </curve_2>
+                        <curve_3>
+                            <axes>
+                                <x_axis>
+                                    <field></field>
+                                    <field_type>1</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/mpc_follower/debug/debug_values</topic>
+                                    <type>std_msgs/Float64MultiArray</type>
+                                </x_axis>
+                                <y_axis>
+                                    <field>data/2</field>
+                                    <field_type>0</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/mpc_follower/debug/debug_values</topic>
+                                    <type>std_msgs/Float64MultiArray</type>
+                                </y_axis>
+                            </axes>
+                            <color>
+                                <custom_color>#000000</custom_color>
+                                <type>0</type>
+                            </color>
+                            <data>
+                                <circular_buffer_capacity>1000</circular_buffer_capacity>
+                                <time_frame_length>30</time_frame_length>
+                                <type>3</type>
+                            </data>
+                            <style>
+                                <lines_interpolate>false</lines_interpolate>
+                                <pen_style>1</pen_style>
+                                <pen_width>2</pen_width>
+                                <render_antialias>false</render_antialias>
+                                <steps_invert>false</steps_invert>
+                                <sticks_baseline>0</sticks_baseline>
+                                <sticks_orientation>2</sticks_orientation>
+                                <type>0</type>
+                            </style>
+                            <subscriber_queue_size>100</subscriber_queue_size>
+                            <title>steer cmd (FF-filtered)</title>
+                        </curve_3>
+                        <curve_4>
+                            <axes>
+                                <x_axis>
+                                    <field></field>
+                                    <field_type>1</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/mpc_follower/debug/debug_values</topic>
+                                    <type>std_msgs/Float64MultiArray</type>
+                                </x_axis>
+                                <y_axis>
+                                    <field>data/3</field>
+                                    <field_type>0</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/mpc_follower/debug/debug_values</topic>
+                                    <type>std_msgs/Float64MultiArray</type>
+                                </y_axis>
+                            </axes>
+                            <color>
+                                <custom_color>#000000</custom_color>
+                                <type>0</type>
+                            </color>
+                            <data>
+                                <circular_buffer_capacity>1000</circular_buffer_capacity>
+                                <time_frame_length>30</time_frame_length>
+                                <type>3</type>
+                            </data>
+                            <style>
+                                <lines_interpolate>false</lines_interpolate>
+                                <pen_style>1</pen_style>
+                                <pen_width>2</pen_width>
+                                <render_antialias>false</render_antialias>
+                                <steps_invert>false</steps_invert>
+                                <sticks_baseline>0</sticks_baseline>
+                                <sticks_orientation>2</sticks_orientation>
+                                <type>0</type>
+                            </style>
+                            <subscriber_queue_size>100</subscriber_queue_size>
+                            <title>steer cmd (FF-raw)</title>
+                        </curve_4>
+                        <curve_5>
+                            <axes>
+                                <x_axis>
+                                    <field></field>
+                                    <field_type>1</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/mpc_follower/debug/debug_values</topic>
+                                    <type>std_msgs/Float64MultiArray</type>
+                                </x_axis>
+                                <y_axis>
+                                    <field>data/4</field>
+                                    <field_type>0</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/mpc_follower/debug/debug_values</topic>
+                                    <type>std_msgs/Float64MultiArray</type>
+                                </y_axis>
+                            </axes>
+                            <color>
+                                <custom_color>#000000</custom_color>
+                                <type>0</type>
+                            </color>
+                            <data>
+                                <circular_buffer_capacity>1000</circular_buffer_capacity>
+                                <time_frame_length>30</time_frame_length>
+                                <type>3</type>
+                            </data>
+                            <style>
+                                <lines_interpolate>false</lines_interpolate>
+                                <pen_style>1</pen_style>
+                                <pen_width>2</pen_width>
+                                <render_antialias>false</render_antialias>
+                                <steps_invert>false</steps_invert>
+                                <sticks_baseline>0</sticks_baseline>
+                                <sticks_orientation>2</sticks_orientation>
+                                <type>0</type>
+                            </style>
+                            <subscriber_queue_size>100</subscriber_queue_size>
+                            <title>steer measured</title>
+                        </curve_5>
+                    </curves>
+                    <legend>
+                        <visible>true</visible>
+                    </legend>
+                    <plot_rate>30</plot_rate>
+                    <title>error</title>
+                </column_0>
+            </row_0>
+            <row_1>
+                <column_0>
+                    <axes>
+                        <axes>
+                            <x_axis>
+                                <custom_title>Untitled Axis</custom_title>
+                                <title_type>0</title_type>
+                                <title_visible>true</title_visible>
+                            </x_axis>
+                            <y_axis>
+                                <custom_title>Untitled Axis</custom_title>
+                                <title_type>0</title_type>
+                                <title_visible>true</title_visible>
+                            </y_axis>
+                        </axes>
+                    </axes>
+                    <curves>
+                        <curve_0>
+                            <axes>
+                                <x_axis>
+                                    <field></field>
+                                    <field_type>1</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/mpc_follower/debug/debug_values</topic>
+                                    <type>std_msgs/Float64MultiArray</type>
+                                </x_axis>
+                                <y_axis>
+                                    <field>data/11</field>
+                                    <field_type>0</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/mpc_follower/debug/debug_values</topic>
+                                    <type>std_msgs/Float64MultiArray</type>
+                                </y_axis>
+                            </axes>
+                            <color>
+                                <custom_color>#000000</custom_color>
+                                <type>0</type>
+                            </color>
+                            <data>
+                                <circular_buffer_capacity>1000</circular_buffer_capacity>
+                                <time_frame_length>30</time_frame_length>
+                                <type>3</type>
+                            </data>
+                            <style>
+                                <lines_interpolate>false</lines_interpolate>
+                                <pen_style>1</pen_style>
+                                <pen_width>2</pen_width>
+                                <render_antialias>false</render_antialias>
+                                <steps_invert>false</steps_invert>
+                                <sticks_baseline>0</sticks_baseline>
+                                <sticks_orientation>2</sticks_orientation>
+                                <type>0</type>
+                            </style>
+                            <subscriber_queue_size>100</subscriber_queue_size>
+                            <title>steering cmd</title>
+                        </curve_0>
+                        <curve_1>
+                            <axes>
+                                <x_axis>
+                                    <field></field>
+                                    <field_type>1</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/mpc_follower/debug/debug_values</topic>
+                                    <type>std_msgs/Float64MultiArray</type>
+                                </x_axis>
+                                <y_axis>
+                                    <field>data/12</field>
+                                    <field_type>0</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/mpc_follower/debug/debug_values</topic>
+                                    <type>std_msgs/Float64MultiArray</type>
+                                </y_axis>
+                            </axes>
+                            <color>
+                                <custom_color>#000000</custom_color>
+                                <type>0</type>
+                            </color>
+                            <data>
+                                <circular_buffer_capacity>1000</circular_buffer_capacity>
+                                <time_frame_length>30</time_frame_length>
+                                <type>3</type>
+                            </data>
+                            <style>
+                                <lines_interpolate>false</lines_interpolate>
+                                <pen_style>1</pen_style>
+                                <pen_width>2</pen_width>
+                                <render_antialias>false</render_antialias>
+                                <steps_invert>false</steps_invert>
+                                <sticks_baseline>0</sticks_baseline>
+                                <sticks_orientation>2</sticks_orientation>
+                                <type>0</type>
+                            </style>
+                            <subscriber_queue_size>100</subscriber_queue_size>
+                            <title>measured steer</title>
+                        </curve_1>
+                        <curve_2>
+                            <axes>
+                                <x_axis>
+                                    <field></field>
+                                    <field_type>1</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/mpc_follower/debug/debug_values</topic>
+                                    <type>std_msgs/Float64MultiArray</type>
+                                </x_axis>
+                                <y_axis>
+                                    <field>data/13</field>
+                                    <field_type>0</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/mpc_follower/debug/debug_values</topic>
+                                    <type>std_msgs/Float64MultiArray</type>
+                                </y_axis>
+                            </axes>
+                            <color>
+                                <custom_color>#000000</custom_color>
+                                <type>0</type>
+                            </color>
+                            <data>
+                                <circular_buffer_capacity>1000</circular_buffer_capacity>
+                                <time_frame_length>30</time_frame_length>
+                                <type>3</type>
+                            </data>
+                            <style>
+                                <lines_interpolate>false</lines_interpolate>
+                                <pen_style>1</pen_style>
+                                <pen_width>2</pen_width>
+                                <render_antialias>false</render_antialias>
+                                <steps_invert>false</steps_invert>
+                                <sticks_baseline>0</sticks_baseline>
+                                <sticks_orientation>2</sticks_orientation>
+                                <type>0</type>
+                            </style>
+                            <subscriber_queue_size>100</subscriber_queue_size>
+                            <title>path curvature</title>
+                        </curve_2>
+                        <curve_3>
+                            <axes>
+                                <x_axis>
+                                    <field></field>
+                                    <field_type>1</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/mpc_follower/debug/debug_values</topic>
+                                    <type>std_msgs/Float64MultiArray</type>
+                                </x_axis>
+                                <y_axis>
+                                    <field>data/16</field>
+                                    <field_type>0</field_type>
+                                    <scale>
+                                        <absolute_maximum>1000</absolute_maximum>
+                                        <absolute_minimum>0</absolute_minimum>
+                                        <relative_maximum>0</relative_maximum>
+                                        <relative_minimum>-1000</relative_minimum>
+                                        <type>0</type>
+                                    </scale>
+                                    <topic>/mpc_follower/debug/debug_values</topic>
+                                    <type>std_msgs/Float64MultiArray</type>
+                                </y_axis>
+                            </axes>
+                            <color>
+                                <custom_color>#000000</custom_color>
+                                <type>0</type>
+                            </color>
+                            <data>
+                                <circular_buffer_capacity>1000</circular_buffer_capacity>
+                                <time_frame_length>30</time_frame_length>
+                                <type>3</type>
+                            </data>
+                            <style>
+                                <lines_interpolate>false</lines_interpolate>
+                                <pen_style>1</pen_style>
+                                <pen_width>2</pen_width>
+                                <render_antialias>false</render_antialias>
+                                <steps_invert>false</steps_invert>
+                                <sticks_baseline>0</sticks_baseline>
+                                <sticks_orientation>2</sticks_orientation>
+                                <type>0</type>
+                            </style>
+                            <subscriber_queue_size>100</subscriber_queue_size>
+                            <title>estimated twist</title>
+                        </curve_3>
+                    </curves>
+                    <legend>
+                        <visible>true</visible>
+                    </legend>
+                    <plot_rate>30</plot_rate>
+                    <title>angular velocity converted from</title>
+                </column_0>
+            </row_1>
+        </plots>
+        <track_points>false</track_points>
+    </table>
+</rqt_multiplot>

src/ros/catkin/src/mpc_follower/include/mpc_follower/lowpass_filter.h

@@ -0,0 +1,124 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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.
+ */
+
+/**
+ * @file lowpass_filter.h
+ * @brief vehicle model interface class
+ * @author Takamasa Horibe
+ * @date 2019.05.01
+ */
+
+#pragma once
+#include <cmath>
+#include <vector>
+#include <algorithm>
+#include <iostream>
+
+/** 
+ * @class 2nd-order Butterworth Filter
+ * @brief filtering values
+ */
+
+/*
+ * reference : S. Butterworth, "On the Theory of Filter Amplifier", Experimental wireless, 1930.
+ */
+
+class Butterworth2dFilter
+{
+private:
+  double y1_; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
+  double y2_; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
+  double u1_; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
+  double u2_; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
+  double a0_; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
+  double a1_; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
+  double a2_; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
+  double b0_; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
+  double b1_; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
+  double b2_; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
+
+public:
+  /**
+   * @brief constructor with initialization
+   * @param [in] dt sampling time
+   * @param [in] f_cutoff_hz cutoff frequency [Hz]
+   */
+  Butterworth2dFilter(double dt = 0.01, double f_cutoff_hz = 5.0);
+
+  /**
+   * @brief destructor
+   */
+  ~Butterworth2dFilter();
+
+  /**
+   * @brief constructor
+   * @param [in] dt sampling time
+   * @param [in] f_cutoff_hz cutoff frequency [Hz]
+   */
+  void initialize(const double &dt, const double &f_cutoff_hz);
+
+  /**
+   * @brief filtering (call this function at each sampling time with input)
+   * @param [in] u scalar input for filter
+   * @return filtered scalar value
+   */
+  double filter(const double &u);
+
+  /**
+   * @brief filtering for time-series data
+   * @param [in] t time-series data for input vector
+   * @param [out] u object vector
+   */
+  void filt_vector(const std::vector<double> &t, std::vector<double> &u);
+
+  /**
+   * @brief filtering for time-series data from both forward-backward direction for zero phase delay
+   * @param [in] t time-series data for input vector
+   * @param [out] u object vector
+   */
+  void filtfilt_vector(const std::vector<double> &t, std::vector<double> &u); // filtering forward and backward direction
+
+  /**
+   * @brief get filter coefficients
+   * @param [out] coeffs coefficients of filter [a0, a1, a2, b0, b1, b2].
+   */
+  void getCoefficients(std::vector<double> &coeffs);
+};
+
+/** 
+ * @class Move Average Filter
+ * @brief filtering values
+ */
+class MoveAverageFilter
+{
+public:
+  /**
+   * @brief constructor
+   */
+  MoveAverageFilter();
+
+  /**
+   * @brief destructor
+   */
+  ~MoveAverageFilter();
+
+  /**
+   * @brief filtering vector
+   * @param [in] num index distance for moving average filter
+   * @param [out] u object vector
+   */
+  static bool filt_vector(const int num, std::vector<double> &u);
+};

src/ros/catkin/src/mpc_follower/include/mpc_follower/mpc_follower_core.h

@@ -0,0 +1,223 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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.
+ */
+
+/**
+ * @file moc_follower.h
+ * @brief mpc follower class
+ * @author Takamasa Horibe
+ * @date 2019.05.01
+ */
+
+#pragma once
+#include <vector>
+#include <iostream>
+#include <limits>
+#include <chrono>
+#include <unistd.h>
+#include <deque>
+
+#include <ros/ros.h>
+#include <std_msgs/Float64.h>
+#include <std_msgs/Float32.h>
+#include <std_msgs/Float64MultiArray.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <visualization_msgs/Marker.h>
+#include <tf2/utils.h>
+
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/LU>
+
+#include <autoware_msgs/ControlCommandStamped.h>
+#include <autoware_msgs/Lane.h>
+#include <autoware_msgs/VehicleStatus.h>
+
+#include "mpc_follower/mpc_utils.h"
+#include "mpc_follower/mpc_trajectory.h"
+#include "mpc_follower/lowpass_filter.h"
+#include "mpc_follower/vehicle_model/vehicle_model_bicycle_kinematics.h"
+#include "mpc_follower/vehicle_model/vehicle_model_bicycle_dynamics.h"
+#include "mpc_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.h"
+#include "mpc_follower/qp_solver/qp_solver_unconstr.h"
+#include "mpc_follower/qp_solver/qp_solver_unconstr_fast.h"
+#include "mpc_follower/qp_solver/qp_solver_qpoases.h"
+
+/** 
+ * @class MPC-based waypoints follower class
+ * @brief calculate control command to follow reference waypoints
+ */
+class MPCFollower
+{
+public:
+  /**
+   * @brief constructor
+   */
+  MPCFollower();
+
+  /**
+   * @brief destructor
+   */
+  ~MPCFollower();
+
+private:
+  ros::NodeHandle nh_;                    //!< @brief ros node handle
+  ros::NodeHandle pnh_;                   //!< @brief private ros node handle
+  ros::Publisher pub_steer_vel_ctrl_cmd_; //!< @brief topic publisher for control command
+  ros::Publisher pub_twist_cmd_;          //!< @brief topic publisher for twist command
+  ros::Subscriber sub_ref_path_;          //!< @brief topic subscriber for reference waypoints
+  ros::Subscriber sub_pose_;              //!< @brief subscriber for current pose
+  ros::Subscriber sub_vehicle_status_;    //!< @brief subscriber for currrent vehicle status
+  ros::Timer timer_control_;              //!< @brief timer for control command computation
+
+  MPCTrajectory ref_traj_;                                   //!< @brief reference trajectory to be followed
+  Butterworth2dFilter lpf_steering_cmd_;                     //!< @brief lowpass filter for steering command
+  Butterworth2dFilter lpf_lateral_error_;                    //!< @brief lowpass filter for lateral error to calculate derivatie
+  Butterworth2dFilter lpf_yaw_error_;                        //!< @brief lowpass filter for heading error to calculate derivatie
+  autoware_msgs::Lane current_waypoints_;                    //!< @brief current waypoints to be followed
+  std::shared_ptr<VehicleModelInterface> vehicle_model_ptr_; //!< @brief vehicle model for MPC
+  std::string vehicle_model_type_;                           //!< @brief vehicle model type for MPC
+  std::shared_ptr<QPSolverInterface> qpsolver_ptr_;          //!< @brief qp solver for MPC
+  std::string output_interface_;                             //!< @brief output command type
+  std::deque<double> input_buffer_;                          //!< @brief control input (mpc_output) buffer for delay time conpemsation
+
+  /* parameters for control*/
+  double ctrl_period_;              //!< @brief control frequency [s]
+  double steering_lpf_cutoff_hz_;   //!< @brief cutoff frequency of lowpass filter for steering command [Hz]
+  double admisible_position_error_; //!< @brief stop MPC calculation when lateral error is large than this value [m]
+  double admisible_yaw_error_deg_;  //!< @brief stop MPC calculation when heading error is large than this value [deg]
+  double steer_lim_deg_;            //!< @brief steering command limit [rad]
+  double wheelbase_;                //!< @brief vehicle wheelbase length [m] to convert steering angle to angular velocity
+
+  /* parameters for path smoothing */
+  bool enable_path_smoothing_;     //< @brief flag for path smoothing
+  bool enable_yaw_recalculation_;  //< @brief flag for recalculation of yaw angle after resampling
+  int path_filter_moving_ave_num_; //< @brief param of moving average filter for path smoothing
+  int path_smoothing_times_;       //< @brief number of times of applying path smoothing filter
+  int curvature_smoothing_num_;    //< @brief point-to-point index distance used in curvature calculation
+  double traj_resample_dist_;      //< @brief path resampling interval [m]
+
+  struct MPCParam
+  {
+    int prediction_horizon;                         //< @brief prediction horizon step
+    double prediction_sampling_time;                //< @brief prediction horizon period
+    double weight_lat_error;                        //< @brief lateral error weight in matrix Q
+    double weight_heading_error;                    //< @brief heading error weight in matrix Q
+    double weight_heading_error_squared_vel_coeff;  //< @brief heading error * velocity weight in matrix Q
+    double weight_steering_input;                   //< @brief steering error weight in matrix R
+    double weight_steering_input_squared_vel_coeff; //< @brief steering error * velocity weight in matrix R
+    double weight_lat_jerk;                         //< @brief lateral jerk weight in matrix R
+    double weight_terminal_lat_error;               //< @brief terminal lateral error weight in matrix Q
+    double weight_terminal_heading_error;           //< @brief terminal heading error weight in matrix Q
+    double zero_ff_steer_deg;                       //< @brief threshold that feed-forward angle becomes zero
+    double delay_compensation_time;                //< @brief delay time for steering input to be compensated
+  };
+  MPCParam mpc_param_; // for mpc design parameter
+
+  struct VehicleStatus
+  {
+    std_msgs::Header header;    //< @brief header
+    geometry_msgs::Pose pose;   //< @brief vehicle pose
+    geometry_msgs::Twist twist; //< @brief vehicle velocity
+    double tire_angle_rad;      //< @brief vehicle tire angle
+  };
+  VehicleStatus vehicle_status_; //< @brief vehicle status
+
+  double steer_cmd_prev_;     //< @brief steering command calculated in previous period
+  double lateral_error_prev_; //< @brief previous lateral error for derivative
+  double yaw_error_prev_;     //< @brief previous lateral error for derivative
+
+  /* flags */
+  bool my_position_ok_; //< @brief flag for validity of current pose
+  bool my_velocity_ok_; //< @brief flag for validity of current velocity
+  bool my_steering_ok_; //< @brief flag for validity of steering angle
+
+  /**
+   * @brief compute and publish control command for path follow with a constant control period
+   */
+  void timerCallback(const ros::TimerEvent &);
+
+  /**
+   * @brief set current_waypoints_ with receved message
+   */
+  void callbackRefPath(const autoware_msgs::Lane::ConstPtr &);
+
+  /**
+   * @brief set vehicle_status_.pose with receved message 
+   */
+  void callbackPose(const geometry_msgs::PoseStamped::ConstPtr &);
+
+  /**
+   * @brief set vehicle_status_.twist and vehicle_status_.tire_angle_rad with receved message
+   */
+  void callbackVehicleStatus(const autoware_msgs::VehicleStatus &msg);
+
+  /**
+   * @brief publish control command calculated by MPC
+   * @param [in] vel_cmd velocity command [m/s] for vehicle control
+   * @param [in] acc_cmd acceleration command [m/s2] for vehicle control
+   * @param [in] steer_cmd steering angle command [rad] for vehicle control
+   * @param [in] steer_vel_cmd steering angle speed [rad/s] for vehicle control
+   */
+  void publishControlCommands(const double &vel_cmd, const double &acc_cmd,
+                              const double &steer_cmd, const double &steer_vel_cmd);
+
+  /**
+   * @brief publish control command as geometry_msgs/TwistStamped type
+   * @param [in] vel_cmd velocity command [m/s] for vehicle control
+   * @param [in] omega_cmd angular velocity command [rad/s] for vehicle control
+   */
+  void publishTwist(const double &vel_cmd, const double &omega_cmd);
+
+  /**
+   * @brief publish control command as autoware_msgs/ControlCommand type
+   * @param [in] vel_cmd velocity command [m/s] for vehicle control
+   * @param [in] acc_cmd acceleration command [m/s2] for vehicle control
+   * @param [in] steer_cmd steering angle command [rad] for vehicle control
+   */
+  void publishCtrlCmd(const double &vel_cmd, const double &acc_cmd, const double &steer_cmd);
+
+  /**
+   * @brief calculate control command by MPC algorithm
+   * @param [out] vel_cmd velocity command
+   * @param [out] acc_cmd acceleration command
+   * @param [out] steer_cmd steering command
+   * @param [out] steer_vel_cmd steering rotation speed command
+   */
+  bool calculateMPC(double &vel_cmd, double &acc_cmd, double &steer_cmd, double &steer_vel_cmd);
+
+  /* debug */
+  bool show_debug_info_;      //!< @brief flag to display debug info
+
+  ros::Publisher pub_debug_filtered_traj_;        //!< @brief publisher for debug info
+  ros::Publisher pub_debug_predicted_traj_;       //!< @brief publisher for debug info
+  ros::Publisher pub_debug_values_;               //!< @brief publisher for debug info
+  ros::Publisher pub_debug_mpc_calc_time_;        //!< @brief publisher for debug info
+
+  ros::Subscriber sub_estimate_twist_;         //!< @brief subscriber for /estimate_twist for debug
+  geometry_msgs::TwistStamped estimate_twist_; //!< @brief received /estimate_twist for debug
+
+  /**
+   * @brief convert MPCTraj to visualizaton marker for visualization
+   */
+  void convertTrajToMarker(const MPCTrajectory &traj, visualization_msgs::Marker &markers,
+                           std::string ns, double r, double g, double b, double z);
+
+  /**
+   * @brief callback for estimate twist for debug
+   */
+  void callbackEstimateTwist(const geometry_msgs::TwistStamped &msg) { estimate_twist_ = msg; }
+};

src/ros/catkin/src/mpc_follower/include/mpc_follower/mpc_trajectory.h

@@ -0,0 +1,52 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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.
+ */
+
+#pragma once
+#include <vector>
+#include <iostream>
+
+/** 
+ * @class trajectory class for mpc follower
+ * @brief calculate control command to follow reference waypoints
+ */
+class MPCTrajectory
+{
+public:
+  std::vector<double> x;             //!< @brief x position x vector
+  std::vector<double> y;             //!< @brief y position y vector
+  std::vector<double> z;             //!< @brief z position z vector
+  std::vector<double> yaw;           //!< @brief yaw pose yaw vector
+  std::vector<double> vx;            //!< @brief vx velocity vx vector
+  std::vector<double> k;             //!< @brief k curvature k vector
+  std::vector<double> relative_time; //!< @brief relative_time duration time from start point
+
+  /**
+   * @brief push_back for all values
+   */
+  void push_back(const double &xp, const double &yp, const double &zp,
+                 const double &yawp, const double &vxp, const double &kp,
+                 const double &tp);
+  /**
+   * @brief clear for all values
+   */
+  void clear();
+
+  /**
+   * @brief check size of MPCTrajectory
+   * @return size, or 0 if the size for each components are inconsistent
+   */
+  unsigned int size() const;
+};

src/ros/catkin/src/mpc_follower/include/mpc_follower/mpc_utils.h

@@ -0,0 +1,151 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <cmath>
+#include <vector>
+#include <ros/ros.h>
+#include <eigen3/Eigen/Core>
+#include "autoware_msgs/Lane.h"
+#include <tf2/utils.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <std_msgs/Float64MultiArray.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <amathutils_lib/amathutils.hpp>
+
+#include "mpc_follower/mpc_trajectory.h"
+
+namespace MPCUtils
+{
+
+/**
+ * @brief convert eular angle vector including +-2pi to 0 jump to continuous series data 
+ * @param [out] a input angle vector
+ */
+void convertEulerAngleToMonotonic(std::vector<double> &a);
+
+
+/**
+ * @brief interpolate value vector at reference index
+ * @param [in] index index vector related to object value (Eigen::Vector or std::vector)
+ * @param [in] values object value vector (Eigen::Vector or std::vector)
+ * @param [in] ref reference index
+ * @param [out] ret interpolated value
+ * @return bool to check whether it was interpolated properly  
+ */
+template <typename T1, typename T2>
+bool interp1d(const T1 &index, const T2 &values, const double &ref, double &ret);
+
+/**
+ * @brief interpolate MPCTrajectory at index vector
+ * @param [in] index index vector related to MPCTrajectory value 
+ * @param [in] values MPCTrajectory
+ * @param [in] ref reference index
+ * @param [out] ret interpolated MPCTrajectory
+ * @return bool to check whether it was interpolated properly  
+ */
+bool interp1dMPCTraj(const std::vector<double> &index, const MPCTrajectory &values,
+                     const std::vector<double> &ref, MPCTrajectory &ret);
+
+/**
+ * @brief calculate yaw angle in MPCTrajectory from xy vector
+ * @param [inout] traj object trajectory
+ */
+void calcTrajectoryYawFromXY(MPCTrajectory &traj);
+
+/**
+ * @brief Calculate path curvature by 3-points circle fitting with smoothing num (use nearest 3 points when num = 1)
+ * @param [inout] traj object trajectory
+ * @param [in] curvature_smoothing_num index distance for 3 points for curvature calculation
+ */
+void calcTrajectoryCurvature(MPCTrajectory &traj, int curvature_smoothing_num);
+
+/**
+ * @brief convert waypoints to MPCTrajectory
+ * @param [in] path input waypoints
+ * @param [out] mpc_traj converted traj
+ */
+void convertWaypointsToMPCTraj(const autoware_msgs::Lane &path, MPCTrajectory &mpc_traj);
+
+/**
+ * @brief convert waypoints to MPCTraj with interpolation
+ * @param [in] path input waypoints 
+ * @param [in] path_time waypoints time used for MPCTrajectory relative_time
+ * @param [in] ref_index reference index with constant distance
+ * @param [in] d_ref_index constant distance of reference index
+ * @param [out] ref_traj converted reference trajectory
+ */
+void convertWaypointsToMPCTrajWithResample(const autoware_msgs::Lane &path, const std::vector<double> &path_time,
+                                           const std::vector<double> &ref_index, const double &d_ref_index, MPCTrajectory &ref_traj);
+
+/**
+ * @brief convert waypoints to MPCTraj with constant distance interpolation 
+ * @param [in] path input waypoints 
+ * @param [in] path_time waypoints time used for MPCTrajectory relative_time
+ * @param [in] dl distance of interpolated path
+ * @param [out] ref_traj converted reference trajectory
+ */
+void convertWaypointsToMPCTrajWithDistanceResample(const autoware_msgs::Lane &path, const std::vector<double> &path_time,
+                                                   const double &dl, MPCTrajectory &ref_traj);
+
+/**
+ * @brief convert waypoints to MPCTraj with constant time interpolation 
+ * @param [in] path input waypoints 
+ * @param [in] path_time waypoints time used for MPCTrajectory relative_time
+ * @param [in] dt time span of interpolated path
+ * @param [out] ref_traj converted reference trajectory
+ */
+void convertWaypointsToMPCTrajWithTimeResample(const autoware_msgs::Lane &path, const std::vector<double> &path_time,
+                                               const double &dt, MPCTrajectory &ref_traj_);
+
+/**
+ * @brief calculate waypoints time with waypoint velocity
+ * @param [in] path object waypoints
+ * @param [out] path_time calculated waypoints time vector
+ */
+void calcPathRelativeTime(const autoware_msgs::Lane &path, std::vector<double> &path_time);
+
+/**
+ * @brief calculate nearest pose on MPCTrajectory
+ * @param [in] traj reference trajectory
+ * @param [in] self_pose object pose 
+ * @param [out] nearest_pose nearest pose on path
+ * @param [out] nearest_index path index of nearest pose 
+ * @param [out] min_dist_error distance error from nearest pose to self pose
+ * @param [out] nearest_yaw_error yaw angle error from nearest pose to self pose
+ * @param [out] nearest_time time of nearest pose on trajectory
+ * @return false when nearest pose couldn't find for some reasons
+ */
+bool calcNearestPose(const MPCTrajectory &traj, const geometry_msgs::Pose &self_pose, geometry_msgs::Pose &nearest_pose,
+                     unsigned int &nearest_index, double &min_dist_error, double &nearest_yaw_error, double &nearest_time);
+
+/**
+ * @brief calculate nearest pose on MPCTrajectory with linear interpolation
+ * @param [in] traj reference trajectory
+ * @param [in] self_pose object pose 
+ * @param [out] nearest_pose nearest pose on path
+ * @param [out] nearest_index path index of nearest pose 
+ * @param [out] min_dist_error distance error from nearest pose to self pose
+ * @param [out] nearest_yaw_error yaw angle error from nearest pose to self pose
+ * @param [out] nearest_time time of nearest pose on trajectory
+ * @return false when nearest pose couldn't find for some reasons
+ */
+bool calcNearestPoseInterp(const MPCTrajectory &traj, const geometry_msgs::Pose &self_pose, geometry_msgs::Pose &nearest_pose,
+                           unsigned int &nearest_index, double &min_dist_error, double &nearest_yaw_error, double &nearest_time);
+
+}; // namespace MPCUtils

src/ros/catkin/src/mpc_follower/include/mpc_follower/qp_solver/qp_solver_interface.h

@@ -0,0 +1,48 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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.
+ */
+
+/**
+ * @file qp_solver_interface.h
+ * @brief qp solver interface class
+ * @author Takamasa Horibe
+ * @date 2019.05.01
+ */
+
+#pragma once
+
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/LU>
+
+class QPSolverInterface
+{
+public:
+  /**
+   * @brief solve QP problem : minimize J = U' * Hmat * U + fvec' * U without constraint
+   * @param [in] Hmat parameter matrix in object function
+   * @param [in] fvec parameter matrix in object function
+   * @param [in] A parameter matrix for constraint lbA < A*U < ubA
+   * @param [in] lb parameter matrix for constraint lb < U < ub
+   * @param [in] up parameter matrix for constraint lb < U < ub
+   * @param [in] lbA parameter matrix for constraint lbA < A*U < ubA
+   * @param [in] ubA parameter matrix for constraint lbA < A*U < ubA
+   * @param [out] U optimal variable vector
+   * @return bool to check the problem is solved
+   */
+  virtual bool solve(const Eigen::MatrixXd &Hmat, const Eigen::MatrixXd &fvec, const Eigen::MatrixXd &A,
+                     const Eigen::VectorXd &lb, const Eigen::VectorXd &ub, const Eigen::MatrixXd &lbA,
+                     const Eigen::MatrixXd &ubA, Eigen::VectorXd &U) = 0;
+};

src/ros/catkin/src/mpc_follower/include/mpc_follower/qp_solver/qp_solver_qpoases.h

@@ -0,0 +1,69 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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.
+ */
+
+/**
+ * @file qp_solver_qpoases.h
+ * @brief qp solver with QPOASES
+ * @author Takamasa Horibe, Yutaka Shimizu
+ * @date 2019.05.01
+ */
+
+#pragma once
+
+#include <iostream>
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/LU>
+#include <qpoases_vendor/qpOASES.hpp>
+#include <cmath>
+
+#include "mpc_follower/qp_solver/qp_solver_interface.h"
+
+class QPSolverQpoasesHotstart : public QPSolverInterface
+{
+private:
+  bool is_solver_initialized_;  //!< @brief flag to check initialization
+  const int max_iter_;          //!< @brief max iteration number
+  qpOASES::SQProblem solver_;   //!< @brief solver for QP
+
+public:
+  /**
+   * @brief constructor
+   * @param max_iter max iteration for QP
+   */
+  QPSolverQpoasesHotstart(const int max_iter);
+
+  /**
+   * @brief destructor
+   */
+  ~QPSolverQpoasesHotstart() = default;
+
+  /**
+   * @brief solve QP problem : minimize J = U' * Hmat * U + fvec' * U with constraint
+   * @param [in] Hmat parameter matrix in object function
+   * @param [in] fvec parameter matrix in object function
+   * @param [in] A parameter matrix for constraint lbA < A*U < ubA
+   * @param [in] lb parameter matrix for constraint lb < U < ub
+   * @param [in] up parameter matrix for constraint lb < U < ub
+   * @param [in] lbA parameter matrix for constraint lbA < A*U < ubA
+   * @param [in] ubA parameter matrix for constraint lbA < A*U < ubA
+   * @param [out] U optimal variable vector
+   * @return bool to check the problem is solved
+   */
+  bool solve(const Eigen::MatrixXd& Hmat, const Eigen::MatrixXd& fvec, const Eigen::MatrixXd& A,
+             const Eigen::VectorXd& lb, const Eigen::VectorXd& ub, const Eigen::MatrixXd& lbA,
+             const Eigen::MatrixXd& ubA, Eigen::VectorXd& U) override;
+};

src/ros/catkin/src/mpc_follower/include/mpc_follower/qp_solver/qp_solver_unconstr.h

@@ -0,0 +1,60 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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.
+ */
+
+/**
+ * @file qp_solver_unconstr.h
+ * @brief qp solver with Eigen
+ * @author Takamasa Horibe
+ * @date 2019.05.01
+ */
+
+#pragma once
+
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/LU>
+#include <cmath>
+#include "mpc_follower/qp_solver/qp_solver_interface.h"
+
+class QPSolverEigenLeastSquare : public QPSolverInterface
+{
+public:
+  /**
+   * @brief constructor
+   */
+  QPSolverEigenLeastSquare();
+
+  /**
+   * @brief destructor
+   */
+  ~QPSolverEigenLeastSquare() = default;
+
+  /**
+   * @brief solve QP problem : minimize J = U' * Hmat * U + fvec' * U without constraint
+   * @param [in] Hmat parameter matrix in object function
+   * @param [in] fvec parameter matrix in object function
+   * @param [in] A parameter matrix for constraint lbA < A*U < ubA (not used here)
+   * @param [in] lb parameter matrix for constraint lb < U < ub (not used here)
+   * @param [in] up parameter matrix for constraint lb < U < ub (not used here)
+   * @param [in] lbA parameter matrix for constraint lbA < A*U < ubA (not used here)
+   * @param [in] ubA parameter matrix for constraint lbA < A*U < ubA (not used here)
+   * @param [out] U optimal variable vector
+   * @return bool to check the problem is solved
+   */
+  bool solve(const Eigen::MatrixXd &Hmat, const Eigen::MatrixXd &fvec, const Eigen::MatrixXd &A,
+             const Eigen::VectorXd &lb, const Eigen::VectorXd &ub, const Eigen::MatrixXd &lbA,
+             const Eigen::MatrixXd &ubA, Eigen::VectorXd &U) override;
+};

src/ros/catkin/src/mpc_follower/include/mpc_follower/qp_solver/qp_solver_unconstr_fast.h

@@ -0,0 +1,60 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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.
+ */
+
+/**
+ * @file qp_solver_unconstr_fast.h
+ * @brief qp solver with Eigen
+ * @author Takamasa Horibe
+ * @date 2019.05.01
+ */
+
+#pragma once
+
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/LU>
+#include <cmath>
+#include "mpc_follower/qp_solver/qp_solver_interface.h"
+
+class QPSolverEigenLeastSquareLLT : public QPSolverInterface
+{
+public:
+  /**
+   * @brief constructor
+   */
+  QPSolverEigenLeastSquareLLT();
+
+  /**
+   * @brief destructor
+   */
+  ~QPSolverEigenLeastSquareLLT() = default;
+
+  /**
+   * @brief solve QP problem : minimize J = U' * Hmat * U + fvec' * U without constraint
+   * @param [in] Hmat parameter matrix in object function
+   * @param [in] fvec parameter matrix in object function
+   * @param [in] A parameter matrix for constraint lbA < A*U < ubA (not used here)
+   * @param [in] lb parameter matrix for constraint lb < U < ub (not used here)
+   * @param [in] up parameter matrix for constraint lb < U < ub (not used here)
+   * @param [in] lbA parameter matrix for constraint lbA < A*U < ubA (not used here)
+   * @param [in] ubA parameter matrix for constraint lbA < A*U < ubA (not used here)
+   * @param [out] U optimal variable vector
+   * @return bool to check the problem is solved
+   */
+  bool solve(const Eigen::MatrixXd &Hmat, const Eigen::MatrixXd &fvec, const Eigen::MatrixXd &A,
+             const Eigen::VectorXd &lb, const Eigen::VectorXd &ub, const Eigen::MatrixXd &lbA,
+             const Eigen::MatrixXd &ubA, Eigen::VectorXd &U) override;
+};

src/ros/catkin/src/mpc_follower/include/mpc_follower/vehicle_model/vehicle_model_bicycle_dynamics.h

@@ -0,0 +1,108 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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.
+ */
+
+/**
+ * @file vehicle_model_bicycle_dynamics.h
+ * @brief vehicle model class of bicycle dynamics
+ * @author Takamasa Horibe, Yutaka Shimizu
+ * @date 2019.05.01
+ */
+
+/*
+ *    Representation
+ * e      : lateral error
+ * de     : derivative of lateral error
+ * th     : heading angle error 
+ * dth    : derivative of heading angle error 
+ * steer  : steering angle (input)
+ * v      : velocity 
+ * m      : mass
+ * Iz     : inertia
+ * lf     : length from center to front tire
+ * lr     : length from center to rear tire
+ * cf     : front tire cornering power
+ * cr     : rear tire cornering power
+ * k      : curvature on reference trajectory point
+ * 
+ *    State & Input
+ * x = [e, de, th, dth]^T
+ * u = steer
+ * 
+ *    Linearized model around reference point (v=vr)
+ *          [0,                   1,                0,                        0]       [       0]       [                          0]
+ *  dx/dt = [0,       -(cf+cr)/m/vr,        (cf+cr)/m,       (lr*cr-lf*cf)/m/vr] * x + [    cf/m] * u + [(lr*cr-lf*cf)/m/vr*k - vr*k]
+ *          [0,                   0,                0,                        1]       [       0]       [                          0]
+ *          [0, (lr*cr-lf*cf)/Iz/vr, (lf*cf-lr*cr)/Iz, -(lf^2*cf+lr^2*cr)/Iz/vr]       [lf*cf/Iz]       [   -(lf^2*cf+lr^2*cr)/Iz/vr]
+ * 
+ * Reference : Jarrod M. Snider, "Automatic Steering Methods for Autonomous Automobile Path Tracking", Robotics Institute, Carnegie Mellon University, February 2009.
+ */
+
+#pragma once
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/LU>
+#include "mpc_follower/vehicle_model/vehicle_model_interface.h"
+
+/** 
+ * @class vehicle model class of bicycle dynamics
+ * @brief calculate model-related values
+ */
+class DynamicsBicycleModel : public VehicleModelInterface
+{
+public:
+  /**
+   * @brief constructor with parameter initialization
+   * @param [in] wheelbase wheelbase length [m]
+   * @param [in] mass_fl mass applied to front left tire [kg]
+   * @param [in] mass_fr mass applied to front right tire [kg]
+   * @param [in] mass_rl mass applied to rear left tire [kg]
+   * @param [in] mass_rr mass applied to rear right tire [kg]
+   * @param [in] cf front cornering power
+   * @param [in] cr rear cornering power
+   */
+  DynamicsBicycleModel(double &wheelbase, double &mass_fl, double &mass_fr,
+                       double &mass_rl, double &mass_rr, double &cf, double &cr);
+
+  /**
+   * @brief destructor
+   */
+  ~DynamicsBicycleModel() = default;
+
+  /**
+   * @brief calculate discrete model matrix of x_k+1 = Ad * xk + Bd * uk + Wd, yk = Cd * xk 
+   * @param [in] Ad coefficient matrix
+   * @param [in] Bd coefficient matrix
+   * @param [in] Cd coefficient matrix
+   * @param [in] Wd coefficient matrix
+   * @param [in] dt Discretization time
+   */
+  void calculateDiscreteMatrix(Eigen::MatrixXd &Ad, Eigen::MatrixXd &Bd,
+                               Eigen::MatrixXd &Wd, Eigen::MatrixXd &Cd, const double &dt) override;
+
+  /**
+   * @brief calculate reference input
+   * @param [out] reference input
+   */
+  void calculateReferenceInput(Eigen::MatrixXd &Uref) override;
+
+private:
+  double wheelbase_; //!< @brief wheelbase length [m]
+  double lf_;        //!< @brief length from centor of mass to front wheel [m]
+  double lr_;        //!< @brief length from centor of mass to rear wheel [m]
+  double mass_;      //!< @brief total mass of vehicle [kg]
+  double iz_;        //!< @brief moment of inertia [kg * m2]
+  double cf_;        //!< @brief front cornering power [N/rad]
+  double cr_;        //!< @brief rear cornering power [N/rad]
+};

src/ros/catkin/src/mpc_follower/include/mpc_follower/vehicle_model/vehicle_model_bicycle_kinematics.h

@@ -0,0 +1,98 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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.
+ */
+
+/**
+ * @file vehicle_model_bicycle_dynamics.h
+ * @brief vehicle model class of bicycle kinematics
+ * @author Takamasa Horibe
+ * @date 2019.05.01
+ */
+
+
+/*
+ *    Representation
+ * e      : lateral error
+ * th     : heading angle error 
+ * steer  : steering angle
+ * steer_d: desired steering agle (input)
+ * v      : velocity 
+ * W      : wheelbase length
+ * tau    : time constant for steering dynamics
+ * 
+ *    State & Input
+ * x = [e, th, steer]^T
+ * u = steer_d
+ * 
+ *    Nonlinear model
+ * dx1/dt = v * sin(x2)
+ * dx2/dt = v * tan(x3) / W
+ * dx3/dt = -(x3 - u) / tau
+ * 
+ *    Linearized model around reference point (v = v_r, th = th_r, steer = steer_r)
+ *         [0,  vr,                   0]       [    0]       [                           0]
+ * dx/dt = [0,   0, vr/W/cos(steer_r)^2] * x + [    0] * u + [-vr*steer_r/W/cos(steer_r)^2]
+ *         [0,   0,               1/tau]       [1/tau]       [                           0]
+ * 
+ */
+
+
+#pragma once
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/LU>
+#include "mpc_follower/vehicle_model/vehicle_model_interface.h"
+
+/** 
+ * @class vehicle model class of bicycle kinematics
+ * @brief calculate model-related values
+ */
+class KinematicsBicycleModel : public VehicleModelInterface
+{
+public:
+  /**
+   * @brief constructor with parameter initialization
+   * @param [in] wheelbase wheelbase length [m]
+   * @param [in] steer_lim steering angle limit [rad]
+   * @param [in] steer_tau steering time constant for 1d-model
+   */
+  KinematicsBicycleModel(const double &wheelbase, const double &steer_lim, const double &steer_tau);
+
+  /**
+   * @brief destructor
+   */
+  ~KinematicsBicycleModel() = default;
+
+  /**
+   * @brief calculate discrete model matrix of x_k+1 = Ad * xk + Bd * uk + Wd, yk = Cd * xk 
+   * @param [out] Ad coefficient matrix
+   * @param [out] Bd coefficient matrix
+   * @param [out] Cd coefficient matrix
+   * @param [out] Wd coefficient matrix
+   * @param [in] dt Discretization time
+   */
+  void calculateDiscreteMatrix(Eigen::MatrixXd &Ad, Eigen::MatrixXd &Bd,
+                               Eigen::MatrixXd &Cd, Eigen::MatrixXd &Wd, const double &dt) override;
+
+  /**
+   * @brief calculate reference input
+   * @param [out] reference input
+   */
+  void calculateReferenceInput(Eigen::MatrixXd &Uref) override;
+
+private:
+  double wheelbase_; //!< @brief wheelbase length [m]
+  double steer_lim_; //!< @brief steering angle limit [rad]
+  double steer_tau_; //!< @brief steering time constant for 1d-model
+};

src/ros/catkin/src/mpc_follower/include/mpc_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.h

@@ -0,0 +1,94 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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.
+ */
+
+/**
+ * @file vehicle_model_bicycle_dynamics_no_delay.h
+ * @brief vehicle model class of bicycle kinematics without steering delay
+ * @author Takamasa Horibe
+ * @date 2019.05.01
+ */
+
+
+/*
+ *    Representation
+ * e      : lateral error
+ * th     : heading angle error 
+ * steer  : steering angle (input)
+ * v      : velocity 
+ * W      : wheelbase length
+ * tau    : time constant for steering dynamics
+ * 
+ *    State & Input
+ * x = [e, th]^T
+ * u = steer
+ * 
+ *    Nonlinear model
+ * dx1/dt = v * sin(x2)
+ * dx2/dt = v * tan(u) / W
+ * 
+ *    Linearized model around reference point (v = v_r, th = th_r, steer = steer_r)
+ *  dx/dt = [0, vr] * x + [                  0] * u + [                           0]
+ *          [0,  0]       [vr/W/cos(steer_r)^2]       [-vr*steer_r/W/cos(steer_r)^2]
+ * 
+ */
+
+
+
+#pragma once
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/LU>
+#include "mpc_follower/vehicle_model/vehicle_model_interface.h"
+
+/** 
+ * @class vehicle model class of bicycle kinematics without steering delay
+ * @brief calculate model-related values
+ */
+class KinematicsBicycleModelNoDelay : public VehicleModelInterface
+{
+public:
+  /**
+   * @brief constructor with parameter initialization
+   * @param [in] wheelbase wheelbase length [m]
+   * @param [in] steer_lim steering angle limit [rad]
+   */
+  KinematicsBicycleModelNoDelay(const double &wheelbase, const double &steer_lim);
+
+  /**
+   * @brief destructor
+   */
+  ~KinematicsBicycleModelNoDelay() = default;
+
+  /**
+   * @brief calculate discrete model matrix of x_k+1 = Ad * xk + Bd * uk + Wd, yk = Cd * xk 
+   * @param [out] Ad coefficient matrix
+   * @param [out] Bd coefficient matrix
+   * @param [out] Cd coefficient matrix
+   * @param [out] Wd coefficient matrix
+   * @param [in] dt Discretization time
+   */
+  void calculateDiscreteMatrix(Eigen::MatrixXd &Ad, Eigen::MatrixXd &Bd,
+                               Eigen::MatrixXd &Cd, Eigen::MatrixXd &Wd, const double &dt) override;
+
+  /**
+   * @brief calculate reference input
+   * @param [out] reference input
+   */
+  void calculateReferenceInput(Eigen::MatrixXd &Uref) override;
+
+private:
+  double wheelbase_; //!< @brief wheelbase length [m]
+  double steer_lim_; //!< @brief steering angle limit [rad]
+};

src/ros/catkin/src/mpc_follower/include/mpc_follower/vehicle_model/vehicle_model_interface.h

@@ -0,0 +1,95 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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.
+ */
+
+/**
+ * @file vehicle_model_interface.h
+ * @brief vehicle model interface class
+ * @author Takamasa Horibe
+ * @date 2019.05.01
+ */
+
+#pragma once
+#include <eigen3/Eigen/Core>
+
+/** 
+ * @class vehicle model class
+ * @brief calculate model-related values
+ */
+class VehicleModelInterface
+{
+protected:
+  const int dim_x_;  //!< @brief dimension of state x
+  const int dim_u_;  //!< @brief dimension of input u
+  const int dim_y_;  //!< @brief dimension of output y
+  double velocity_;  //!< @brief vehicle velocity
+  double curvature_; //!< @brief curvature on the linearized point on path
+
+public:
+  /**
+   * @brief constructor
+   * @param [in] dim_x dimension of state x
+   * @param [in] dim_u dimension of input u
+   * @param [in] dim_y dimension of output y
+   */
+  VehicleModelInterface(int dim_x, int dim_u, int dim_y);
+
+  /**
+   * @brief get state x dimension
+   * @return state dimension
+   */
+  int getDimX();
+
+  /**
+   * @brief get input u dimension
+   * @return input dimension
+   */
+  int getDimU();
+
+  /**
+   * @brief get output y dimension
+   * @return output dimension
+   */
+  int getDimY();
+
+  /**
+   * @brief set velocity
+   * @param [in] vehicle velocity
+   */
+  void setVelocity(const double &velocity);
+
+  /**
+   * @brief set curvature
+   * @param [in] curvature curvature on the linearized point on path
+   */
+  void setCurvature(const double &curvature);
+
+  /**
+   * @brief calculate discrete model matrix of x_k+1 = Ad * xk + Bd * uk + Wd, yk = Cd * xk 
+   * @param [out] Ad coefficient matrix
+   * @param [out] Bd coefficient matrix
+   * @param [out] Cd coefficient matrix
+   * @param [out] Wd coefficient matrix
+   * @param [in] dt Discretization time
+   */
+  virtual void calculateDiscreteMatrix(Eigen::MatrixXd &Ad, Eigen::MatrixXd &Bd, Eigen::MatrixXd &Cd,
+                                       Eigen::MatrixXd &Wd, const double &dt) = 0;
+
+  /**
+   * @brief calculate reference input
+   * @param [out] reference input
+   */
+  virtual void calculateReferenceInput(Eigen::MatrixXd &Uref) = 0;
+};

src/ros/catkin/src/mpc_follower/launch/mpc_follower.launch

@@ -0,0 +1,87 @@
+<launch>
+
+  <arg name="ns" default="/"/>
+  <arg name="show_debug_info" default="false" doc="flag to display debug info" />
+  <arg name="ctrl_period" default="0.03" doc="control period [s]"/>
+  <arg name="traj_resample_dist" default="0.1" doc="ath resampling interval [m]"/>
+  <arg name="enable_yaw_recalculation" default="true" doc="flag for recalculation of yaw angle after resampling"/>
+  <arg name="admisible_position_error" default="5.0" doc="stop mpc calculation when error is larger than the following value"/>
+  <arg name="admisible_yaw_error_deg" default="90.0" doc="stop mpc calculation when error is larger than the following value"/>
+  <arg name="enable_path_smoothing" default="true" doc="flag for path smoothing"/>
+  <arg name="path_smoothing_times" default="1" doc="number of times of applying path smoothing filter"/>
+  <arg name="path_filter_moving_ave_num" default="35" doc="param of moving average filter for path smoothing "/>
+  <arg name="curvature_smoothing_num" default="35" doc="point-to-point index distance used in curvature calculation : curvature is calculated from three points p(i-num), p(i), p(i+num)"/>
+  <arg name="steering_lpf_cutoff_hz" default="3.0" doc="cutoff frequency of lowpass filter for steering command [Hz]"/>
+  <arg name="qp_solver_type" default="unconstraint_fast" doc="optimization solver type. option is unconstraint_fast, unconstraint, and qpoases_hotstart"/>
+  <arg name="qpoases_max_iter" default="500" doc="max iteration number for quadratic programming"/>
+  <arg name="vehicle_model_type" default="kinematics" doc="vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics"/>
+
+  <!-- MPC design parameters -->
+  <arg name="mpc_prediction_horizon" default="70" doc="prediction horizon step"/>
+  <arg name="mpc_prediction_sampling_time" default="0.1" doc="prediction horizon period [s]"/>
+  <arg name="mpc_weight_lat_error" default="0.1" doc="lateral error weight in matrix Q"/>
+  <arg name="mpc_weight_heading_error" default="0.0" doc="heading error weight in matrix Q"/>
+  <arg name="mpc_weight_heading_error_squared_vel_coeff" default="0.3" doc="heading error * velocity weight in matrix Q"/>
+  <arg name="mpc_weight_steering_input" default="1.0" doc="steering error weight in matrix R"/>
+  <arg name="mpc_weight_steering_input_squared_vel_coeff" default="0.25" doc="steering error * velocity weight in matrix R"/>
+  <arg name="mpc_weight_lat_jerk" default="0.0" doc="lateral jerk weight in matrix R"/>
+  <arg name="mpc_weight_terminal_lat_error" default="1.0" doc="terminal lateral error weight in matrix Q to improve mpc stability"/>
+  <arg name="mpc_weight_terminal_heading_error" default="0.1" doc="terminal heading error weight in matrix Q to improve mpc stability"/>
+  <arg name="mpc_zero_ff_steer_deg" default="2.0" doc="threshold that feed-forward angle becomes zero"/>
+  <arg name="delay_compensation_time" default="0.0" doc="steering input delay time for delay compensation"/>
+
+  <arg name="vehicle_model_steer_tau" default="0.3" doc="steering dynamics time constant (1d approzimation) [s]"/>
+  <arg name="vehicle_model_wheelbase" default="2.9" doc="wheel base length [m]"/>
+  <arg name="steer_lim_deg" default="40.0" doc="steering angle limit [deg]"/>
+  <arg name="steering_gear_ratio" default="20.0" doc="steering gear ratio"/>
+
+  <group ns="$(arg ns)">
+
+    <node pkg="mpc_follower" type="mpc_waypoints_converter" name="mpc_waypoints_converter" output="screen"/>
+
+    <node pkg="mpc_follower" type="mpc_follower" name="mpc_follower" output="screen">
+
+      <!-- system -->
+      <param name="ctrl_cmd_interface" value="all"/>
+      <param name="in_waypoints_name" value="/mpc_waypoints"/>
+      <param name="in_vehicle_status_name" value="/vehicle_status"/>
+      <param name="in_selfpose_name" value="/current_pose"/>
+      <param name="out_twist_name" value="/twist_raw"/>
+      <param name="out_vehicle_cmd_name" value="/ctrl_raw"/>
+
+      <!-- paramters -->
+      <param name="ctrl_period" value="$(arg ctrl_period)"/>
+      <param name="traj_resample_dist" value="$(arg traj_resample_dist)"/>
+      <param name="admisible_position_error" value="$(arg admisible_position_error)"/>
+      <param name="admisible_yaw_error_deg" value="$(arg admisible_yaw_error_deg)"/>
+      <param name="path_smoothing_times" value="$(arg path_smoothing_times)"/>
+      <param name="show_debug_info" value="$(arg show_debug_info)"/>
+      <param name="enable_yaw_recalculation" value="$(arg enable_yaw_recalculation)"/>
+      <param name="enable_path_smoothing" value="$(arg enable_path_smoothing)"/>
+      <param name="path_filter_moving_ave_num" value="$(arg path_filter_moving_ave_num)"/>
+      <param name="curvature_smoothing_num" value="$(arg curvature_smoothing_num)"/>
+      <param name="steering_lpf_cutoff_hz" value="$(arg steering_lpf_cutoff_hz)"/>
+      <param name="qp_solver_type" value="$(arg qp_solver_type)"/>
+      <param name="qpoases_max_iter" value="$(arg qpoases_max_iter)"/>
+      <param name="vehicle_model_type" value="$(arg vehicle_model_type)"/>
+      <param name="mpc_prediction_horizon" value="$(arg mpc_prediction_horizon)"/>
+      <param name="mpc_prediction_sampling_time" value="$(arg mpc_prediction_sampling_time)"/>
+      <param name="mpc_weight_lat_error" value="$(arg mpc_weight_lat_error)"/>
+      <param name="mpc_weight_heading_error" value="$(arg mpc_weight_heading_error)"/>
+      <param name="mpc_weight_heading_error_squared_vel_coeff" value="$(arg mpc_weight_heading_error_squared_vel_coeff)"/>
+      <param name="mpc_weight_steering_input" value="$(arg mpc_weight_steering_input)"/>
+      <param name="mpc_weight_steering_input_squared_vel_coeff" value="$(arg mpc_weight_steering_input_squared_vel_coeff)"/>
+      <param name="mpc_weight_lat_jerk" value="$(arg mpc_weight_lat_jerk)"/>
+      <param name="mpc_weight_terminal_lat_error" value="$(arg mpc_weight_terminal_lat_error)"/>
+      <param name="mpc_weight_terminal_heading_error" value="$(arg mpc_weight_terminal_heading_error)"/>
+      <param name="mpc_zero_ff_steer_deg" value="$(arg mpc_zero_ff_steer_deg)"/>
+      <param name="delay_compensation_time" value="$(arg delay_compensation_time)"/>
+      <param name="vehicle_model_steer_tau" value="$(arg vehicle_model_steer_tau)"/>
+      <param name="vehicle_model_wheelbase" value="$(arg vehicle_model_wheelbase)"/>
+      <param name="steer_lim_deg" value="$(arg steer_lim_deg)"/>
+      <param name="steering_gear_ratio" value="$(arg steering_gear_ratio)"/>
+    </node>
+
+  </group>
+
+</launch>

src/ros/catkin/src/mpc_follower/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>mpc_follower</name>
+  <version>1.12.0</version>
+  <description>The mpc_follower package</description>
+  <maintainer email="horibe.takamasa@gmail.com">Takamasa Horibe</maintainer>
+  <author email="horibe.takamasa@gmail.com">Takamasa Horibe</author>
+  <license>Apache 2</license>
+
+  <buildtool_depend>autoware_build_flags</buildtool_depend>
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>amathutils_lib</depend>
+  <depend>autoware_health_checker</depend>
+  <depend>autoware_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>roscpp</depend>
+  <depend>rostest</depend>
+  <depend>rosunit</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf</depend>
+  <depend>qpoases_vendor</depend>
+</package>

src/ros/catkin/src/mpc_follower/src/lowpass_filter.cpp

@@ -0,0 +1,141 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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 "mpc_follower/lowpass_filter.h"
+
+Butterworth2dFilter::Butterworth2dFilter(double dt, double f_cutoff_hz)
+{
+  initialize(dt, f_cutoff_hz);
+};
+
+Butterworth2dFilter::~Butterworth2dFilter(){};
+
+void Butterworth2dFilter::initialize(const double &dt, const double &f_cutoff_hz)
+{
+  y1_ = 0.0;
+  y2_ = 0.0;
+  u2_ = 0.0;
+  u1_ = 0.0;
+
+  /* 2d butterworth lowpass filter with bi-linear transformation */
+  double wc = 2.0 * M_PI * f_cutoff_hz;
+  double n = 2 / dt;
+  a0_ = n * n + sqrt(2) * wc * n + wc * wc;
+  a1_ = 2 * wc * wc - 2 * n * n;
+  a2_ = n * n - sqrt(2) * wc * n + wc * wc;
+  b0_ = wc * wc;
+  b1_ = 2 * b0_;
+  b2_ = b0_;
+}
+
+double Butterworth2dFilter::filter(const double &u0)
+{
+  double y0 = (b2_ * u2_ + b1_ * u1_ + b0_ * u0 - a2_ * y2_ - a1_ * y1_) / a0_;
+  y2_ = y1_;
+  y1_ = y0;
+  u2_ = u1_;
+  u1_ = u0;
+  return y0;
+}
+
+void Butterworth2dFilter::filt_vector(const std::vector<double> &t, std::vector<double> &u)
+{
+  double y1 = u.at(0);
+  double y2 = u.at(0);
+  double u2 = u.at(0);
+  double u1 = u.at(0);
+  double y0 = 0.0;
+  double u0 = 0.0;
+  for (unsigned int i = 0; i < u.size(); ++i)
+  {
+    u0 = u.at(i);
+    y0 = (b2_ * u2 + b1_ * u1 + b0_ * u0 - a2_ * y2 - a1_ * y1) / a0_;
+    y2 = y1;
+    y1 = y0;
+    u2 = u1;
+    u1 = u0;
+    u.at(i) = y0;
+  }
+}
+
+// filtering forward and backward direction
+void Butterworth2dFilter::filtfilt_vector(const std::vector<double> &t, std::vector<double> &u)
+{
+  std::vector<double> u_rev(u);
+
+  // forward filtering
+  filt_vector(t, u);
+
+  // backward filtering
+  std::reverse(u_rev.begin(), u_rev.end());
+  filt_vector(t, u_rev);
+  std::reverse(u_rev.begin(), u_rev.end());
+
+  // merge
+  for (unsigned int i = 0; i < u.size(); ++i)
+  {
+    u[i] = (u[i] + u_rev[i]) * 0.5;
+  }
+}
+
+void Butterworth2dFilter::getCoefficients(std::vector<double> &coeffs)
+{
+  coeffs.clear();
+  coeffs.push_back(a0_);
+  coeffs.push_back(a1_);
+  coeffs.push_back(a2_);
+  coeffs.push_back(b0_);
+  coeffs.push_back(b1_);
+  coeffs.push_back(b2_);
+  return;
+}
+
+bool MoveAverageFilter::filt_vector(const int num, std::vector<double> &u)
+{
+  if ((int)u.size() < num)
+  {
+    std::cout << "[MovingAverageFilter] vector size is lower than moving average number" << std::endl;
+    return false;
+  }
+  std::vector<double> filtered_u(u);
+  for (int i = 0; i < (int)u.size(); ++i)
+  {
+    double tmp = 0.0;
+    int num_tmp = 0;
+    int count = 0;
+    if (i - num < 0)
+    {
+      num_tmp = i;
+    }
+    else if (i + num > (int)u.size() - 1)
+    {
+      num_tmp = (int)u.size() - i - 1;
+    }
+    else
+    {
+      num_tmp = num;
+    }
+
+    for (int j = -num_tmp; j <= num_tmp; ++j)
+    {
+      tmp += u[i + j];
+      ++count;
+    }
+    filtered_u[i] = tmp / count;
+  }
+  u = filtered_u;
+  return true;
+}

src/ros/catkin/src/mpc_follower/src/mpc_follower_core.cpp

@@ -0,0 +1,708 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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 "mpc_follower/mpc_follower_core.h"
+
+#define DEBUG_INFO(...) { if (show_debug_info_) { ROS_INFO(__VA_ARGS__); }}
+
+MPCFollower::MPCFollower()
+    : nh_(""), pnh_("~"), my_position_ok_(false), my_velocity_ok_(false), my_steering_ok_(false)
+{
+  pnh_.param("show_debug_info", show_debug_info_, bool(false));
+  pnh_.param("ctrl_period", ctrl_period_, double(0.03));
+  pnh_.param("enable_path_smoothing", enable_path_smoothing_, bool(true));
+  pnh_.param("enable_yaw_recalculation", enable_yaw_recalculation_, bool(false));
+  pnh_.param("path_filter_moving_ave_num", path_filter_moving_ave_num_, int(35));
+  pnh_.param("path_smoothing_times", path_smoothing_times_, int(1));
+  pnh_.param("curvature_smoothing_num", curvature_smoothing_num_, int(35));
+  pnh_.param("traj_resample_dist", traj_resample_dist_, double(0.1)); // [m]
+  pnh_.param("admisible_position_error", admisible_position_error_, double(5.0));
+  pnh_.param("admisible_yaw_error_deg", admisible_yaw_error_deg_, double(90.0));
+  pnh_.param("output_interface", output_interface_, std::string("all"));
+
+  /* mpc parameters */
+  pnh_.param("mpc_prediction_horizon", mpc_param_.prediction_horizon, int(70));
+  pnh_.param("mpc_prediction_sampling_time", mpc_param_.prediction_sampling_time, double(0.1));
+  pnh_.param("mpc_weight_lat_error", mpc_param_.weight_lat_error, double(1.0));
+  pnh_.param("mpc_weight_heading_error", mpc_param_.weight_heading_error, double(0.0));
+  pnh_.param("mpc_weight_heading_error_squared_vel_coeff", mpc_param_.weight_heading_error_squared_vel_coeff, double(0.3));
+  pnh_.param("mpc_weight_steering_input", mpc_param_.weight_steering_input, double(1.0));
+  pnh_.param("mpc_weight_steering_input_squared_vel_coeff", mpc_param_.weight_steering_input_squared_vel_coeff, double(0.25));
+  pnh_.param("mpc_weight_lat_jerk", mpc_param_.weight_lat_jerk, double(0.0));
+  pnh_.param("mpc_weight_terminal_lat_error", mpc_param_.weight_terminal_lat_error, double(1.0));
+  pnh_.param("mpc_weight_terminal_heading_error", mpc_param_.weight_terminal_heading_error, double(0.1));
+  pnh_.param("mpc_zero_ff_steer_deg", mpc_param_.zero_ff_steer_deg, double(2.0));
+  pnh_.param("delay_compensation_time", mpc_param_.delay_compensation_time, double(0.0));
+
+  pnh_.param("steer_lim_deg", steer_lim_deg_, double(35.0));
+  pnh_.param("vehicle_model_wheelbase", wheelbase_, double(2.9));
+
+  /* vehicle model setup */
+  pnh_.param("vehicle_model_type", vehicle_model_type_, std::string("kinematics"));
+  if (vehicle_model_type_ == "kinematics")
+  {
+    double steer_tau;
+    pnh_.param("vehicle_model_steer_tau", steer_tau, double(0.1));
+
+    vehicle_model_ptr_ = std::make_shared<KinematicsBicycleModel>(wheelbase_, amathutils::deg2rad(steer_lim_deg_), steer_tau);
+    ROS_INFO("[MPC] set vehicle_model = kinematics");
+  }
+  else if (vehicle_model_type_ == "kinematics_no_delay")
+  {
+    vehicle_model_ptr_ = std::make_shared<KinematicsBicycleModelNoDelay>(wheelbase_, amathutils::deg2rad(steer_lim_deg_));
+    ROS_INFO("[MPC] set vehicle_model = kinematics_no_delay");
+  }
+  else if (vehicle_model_type_ == "dynamics")
+  {
+    double mass_fl, mass_fr, mass_rl, mass_rr, cf, cr;
+    pnh_.param("mass_fl", mass_fl, double(600));
+    pnh_.param("mass_fr", mass_fr, double(600));
+    pnh_.param("mass_rl", mass_rl, double(600));
+    pnh_.param("mass_rr", mass_rr, double(600));
+    pnh_.param("cf", cf, double(155494.663));
+    pnh_.param("cr", cr, double(155494.663));
+
+    vehicle_model_ptr_ = std::make_shared<DynamicsBicycleModel>(wheelbase_, mass_fl, mass_fr, mass_rl, mass_rr, cf, cr);
+    ROS_INFO("[MPC] set vehicle_model = dynamics");
+  }
+  else
+  {
+    ROS_ERROR("[MPC] vehicle_model_type is undefined");
+  }
+
+  /* QP solver setup */
+  std::string qp_solver_type_;
+  pnh_.param("qp_solver_type", qp_solver_type_, std::string("unconstraint_fast"));
+  if (qp_solver_type_ == "unconstraint")
+  {
+    qpsolver_ptr_ = std::make_shared<QPSolverEigenLeastSquare>();
+    ROS_INFO("[MPC] set qp solver = unconstraint");
+  }
+  else if (qp_solver_type_ == "unconstraint_fast")
+  {
+    qpsolver_ptr_ = std::make_shared<QPSolverEigenLeastSquareLLT>();
+    ROS_INFO("[MPC] set qp solver = unconstraint_fast");
+  }
+  else if (qp_solver_type_ == "qpoases_hotstart")
+  {
+    int max_iter;
+    pnh_.param("qpoases_max_iter", max_iter, int(500));
+    qpsolver_ptr_ = std::make_shared<QPSolverQpoasesHotstart>(max_iter);
+    ROS_INFO("[MPC] set qp solver = qpoases_hotstart");
+  }
+  else
+  {
+    ROS_ERROR("[MPC] qp_solver_type is undefined");
+  }
+
+  steer_cmd_prev_ = 0.0;
+  lateral_error_prev_ = 0.0;
+  yaw_error_prev_ = 0.0;
+
+  /* delay compensation */
+  const int delay_step = std::round(mpc_param_.delay_compensation_time / ctrl_period_);
+  std::deque<double> tmp_deque(delay_step, 0.0);
+  input_buffer_ = tmp_deque;
+
+  /* initialize lowpass filter */
+  double steering_lpf_cutoff_hz, error_deriv_lpf_curoff_hz;
+  pnh_.param("steering_lpf_cutoff_hz", steering_lpf_cutoff_hz, double(3.0));
+  pnh_.param("error_deriv_lpf_curoff_hz", error_deriv_lpf_curoff_hz, double(5.0));
+  lpf_steering_cmd_.initialize(ctrl_period_, steering_lpf_cutoff_hz);
+  lpf_lateral_error_.initialize(ctrl_period_, error_deriv_lpf_curoff_hz);
+  lpf_yaw_error_.initialize(ctrl_period_, error_deriv_lpf_curoff_hz);
+
+  /* set up ros system */
+  timer_control_ = nh_.createTimer(ros::Duration(ctrl_period_), &MPCFollower::timerCallback, this);
+  std::string out_twist, out_vehicle_cmd, in_vehicle_status, in_waypoints, in_selfpose;
+  pnh_.param("out_twist_name", out_twist, std::string("twist_raw"));
+  pnh_.param("out_vehicle_cmd_name", out_vehicle_cmd, std::string("ctrl_raw"));
+  pnh_.param("in_waypoints_name", in_waypoints, std::string("base_waypoints"));
+  pnh_.param("in_selfpose_name", in_selfpose, std::string("current_pose"));
+  pnh_.param("in_vehicle_status_name", in_vehicle_status, std::string("vehicle_status"));
+  pub_twist_cmd_ = nh_.advertise<geometry_msgs::TwistStamped>(out_twist, 1);
+  pub_steer_vel_ctrl_cmd_ = nh_.advertise<autoware_msgs::ControlCommandStamped>(out_vehicle_cmd, 1);
+  sub_ref_path_ = nh_.subscribe(in_waypoints, 1, &MPCFollower::callbackRefPath, this);
+  sub_pose_ = nh_.subscribe(in_selfpose, 1, &MPCFollower::callbackPose, this);
+  sub_vehicle_status_ = nh_.subscribe(in_vehicle_status, 1, &MPCFollower::callbackVehicleStatus, this);
+
+  /* for debug */
+  pub_debug_filtered_traj_ = pnh_.advertise<visualization_msgs::Marker>("debug/filtered_traj", 1);
+  pub_debug_predicted_traj_ = pnh_.advertise<visualization_msgs::Marker>("debug/predicted_traj", 1);
+  pub_debug_mpc_calc_time_ = pnh_.advertise<std_msgs::Float32>("debug/mpc_calc_time", 1);
+
+  pub_debug_values_ = pnh_.advertise<std_msgs::Float64MultiArray>("debug/debug_values", 1);
+  sub_estimate_twist_ = nh_.subscribe("estimate_twist", 1, &MPCFollower::callbackEstimateTwist, this);
+};
+
+void MPCFollower::timerCallback(const ros::TimerEvent &te)
+{
+
+  /* guard */
+  if (vehicle_model_ptr_ == nullptr || qpsolver_ptr_ == nullptr)
+  {
+    DEBUG_INFO("[MPC] vehicle_model = %d, qp_solver = %d", !(vehicle_model_ptr_ == nullptr), !(qpsolver_ptr_ == nullptr));
+    publishControlCommands(0.0, 0.0, steer_cmd_prev_, 0.0); // publish brake
+    return;
+  }
+
+  if (ref_traj_.size() == 0 || !my_position_ok_ || !my_velocity_ok_ || !my_steering_ok_)
+  {
+    DEBUG_INFO("[MPC] MPC is not solved. ref_traj_.size() = %d, my_position_ok_ = %d,  my_velocity_ok_ = %d,  my_steering_ok_ = %d",
+               ref_traj_.size(), my_position_ok_, my_velocity_ok_, my_steering_ok_);
+    publishControlCommands(0.0, 0.0, steer_cmd_prev_, 0.0); // publish brake
+    return;
+  }
+
+  /* control command */
+  double vel_cmd = 0.0;
+  double acc_cmd = 0.0;
+  double steer_cmd = 0.0;
+  double steer_vel_cmd = 0.0;
+
+  /* solve MPC */
+  auto start = std::chrono::system_clock::now();
+  const bool mpc_solved = calculateMPC(vel_cmd, acc_cmd, steer_cmd, steer_vel_cmd);
+  double elapsed_ms = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start).count() * 1.0e-6;
+  DEBUG_INFO("[MPC] timerCallback: MPC calculating time = %f [ms]\n", elapsed_ms);
+
+  /* publish computing time */
+  std_msgs::Float32 mpc_calc_time_msg;
+  mpc_calc_time_msg.data = elapsed_ms;
+  pub_debug_mpc_calc_time_.publish(mpc_calc_time_msg);
+
+  if (!mpc_solved)
+  {
+    ROS_WARN("[MPC] MPC is not solved. publish 0 velocity.");
+    vel_cmd = 0.0;
+    acc_cmd = 0.0;
+    steer_cmd = steer_cmd_prev_;
+    steer_vel_cmd = 0.0;
+  }
+
+  publishControlCommands(vel_cmd, acc_cmd, steer_cmd, steer_vel_cmd);
+};
+
+bool MPCFollower::calculateMPC(double &vel_cmd, double &acc_cmd, double &steer_cmd, double &steer_vel_cmd)
+{
+  const int N = mpc_param_.prediction_horizon;
+  const double DT = mpc_param_.prediction_sampling_time;
+  const int DIM_X = vehicle_model_ptr_->getDimX();
+  const int DIM_U = vehicle_model_ptr_->getDimU();
+  const int DIM_Y = vehicle_model_ptr_->getDimY();
+
+  const double current_yaw = tf2::getYaw(vehicle_status_.pose.orientation);
+
+  /* calculate nearest point on reference trajectory (used as initial state) */
+  unsigned int nearest_index = 0;
+  double yaw_err, dist_err, nearest_traj_time;
+  geometry_msgs::Pose nearest_pose;
+  if (!MPCUtils::calcNearestPoseInterp(ref_traj_, vehicle_status_.pose, nearest_pose, nearest_index, dist_err, yaw_err, nearest_traj_time))
+  {
+    ROS_WARN("[MPC] calculateMPC: error in calculating nearest pose. stop mpc.");
+    return false;
+  };
+
+  /* check if lateral error is not too large */
+  if (dist_err > admisible_position_error_ || std::fabs(yaw_err) > amathutils::deg2rad(admisible_yaw_error_deg_ ))
+  {
+    ROS_WARN("[MPC] error is over limit, stop mpc. (pos: error = %f[m], limit: %f[m], yaw: error = %f[deg], limit %f[deg])",
+             dist_err, admisible_position_error_, amathutils::rad2deg(yaw_err), admisible_yaw_error_deg_);
+    return false;
+  }
+
+  /* set mpc initial time */
+  const double mpc_start_time = nearest_traj_time;
+
+  /* check trajectory length */
+  const double mpc_end_time = mpc_start_time + (N - 1) * DT + mpc_param_.delay_compensation_time + ctrl_period_;
+  if (mpc_end_time > ref_traj_.relative_time.back())
+  {
+    ROS_WARN("[MPC] path is too short for prediction. path end: %f[s], mpc end time: %f[s]", ref_traj_.relative_time.back(), mpc_end_time);
+    return false;
+  }
+
+  /* convert tracking x,y error to lat error */
+  const double err_x = vehicle_status_.pose.position.x - nearest_pose.position.x;
+  const double err_y = vehicle_status_.pose.position.y - nearest_pose.position.y;
+  const double sp_yaw = tf2::getYaw(nearest_pose.orientation);
+  const double err_lat = -sin(sp_yaw) * err_x + cos(sp_yaw) * err_y;
+
+  /* get steering angle */
+  const double steer = vehicle_status_.tire_angle_rad;
+
+  /* define initial state for error dynamics */
+  Eigen::VectorXd x0 = Eigen::VectorXd::Zero(DIM_X);
+  if (vehicle_model_type_ == "kinematics")
+  {
+    x0 << err_lat, yaw_err, steer;
+  }
+  else if (vehicle_model_type_ == "kinematics_no_delay")
+  {
+    x0 << err_lat, yaw_err;
+  }
+  else if (vehicle_model_type_ == "dynamics")
+  {
+    double dot_err_lat = (err_lat - lateral_error_prev_) / ctrl_period_;
+    double dot_err_yaw = (yaw_err - yaw_error_prev_) / ctrl_period_;
+    DEBUG_INFO("[MPC] (before lpf) dot_err_lat = %f, dot_err_yaw = %f", dot_err_lat, dot_err_yaw);
+    lateral_error_prev_ = err_lat;
+    yaw_error_prev_ = yaw_err;
+    dot_err_lat = lpf_lateral_error_.filter(dot_err_lat);
+    dot_err_yaw = lpf_yaw_error_.filter(dot_err_yaw);
+    DEBUG_INFO("[MPC] (after lpf) dot_err_lat = %f, dot_err_yaw = %f", dot_err_lat, dot_err_yaw);
+    x0 << err_lat, dot_err_lat, yaw_err, dot_err_yaw;
+  }
+  else
+  {
+    ROS_ERROR("vehicle_model_type is undefined");
+    return false;
+  }
+  DEBUG_INFO("[MPC] selfpose.x = %f, y = %f, yaw = %f", vehicle_status_.pose.position.x, vehicle_status_.pose.position.y, current_yaw);
+  DEBUG_INFO("[MPC] nearpose.x = %f, y = %f, yaw = %f", nearest_pose.position.x, nearest_pose.position.y, tf2::getYaw(nearest_pose.orientation));
+  DEBUG_INFO("[MPC] nearest_index = %d, nearest_traj_time = %f", nearest_index, nearest_traj_time);
+  DEBUG_INFO("[MPC] lat error = %f, yaw error = %f, steer = %f, sp_yaw = %f, my_yaw = %f", err_lat, yaw_err, steer, sp_yaw, current_yaw);
+
+
+  /////////////// delay compensation  ///////////////
+  Eigen::MatrixXd Ad(DIM_X, DIM_X);
+  Eigen::MatrixXd Bd(DIM_X, DIM_U);
+  Eigen::MatrixXd Wd(DIM_X, 1);
+  Eigen::MatrixXd Cd(DIM_Y, DIM_X);
+  Eigen::MatrixXd Uref(DIM_U, 1);
+
+  Eigen::MatrixXd x_curr = x0;
+  double mpc_curr_time = mpc_start_time;
+  for (unsigned int i = 0; i < input_buffer_.size(); ++i)
+  {
+    double k = 0.0;
+    double v = 0.0;
+    if (!MPCUtils::interp1d(ref_traj_.relative_time, ref_traj_.k, mpc_curr_time, k) ||
+        !MPCUtils::interp1d(ref_traj_.relative_time, ref_traj_.vx, mpc_curr_time, v))
+    {
+      ROS_WARN("[MPC] calculateMPC: mpc resample error at delay compensation, stop mpc calculation. check code!");
+      return false;
+    }
+
+    /* get discrete state matrix A, B, C, W */
+    vehicle_model_ptr_->setVelocity(v);
+    vehicle_model_ptr_->setCurvature(k);
+    vehicle_model_ptr_->calculateDiscreteMatrix(Ad, Bd, Cd, Wd, ctrl_period_);
+    Eigen::MatrixXd ud = Eigen::MatrixXd::Zero(DIM_U, 1);
+    ud(0, 0) = input_buffer_.at(i); // for steering input delay
+    x_curr = Ad * x_curr + Bd * ud + Wd;
+    mpc_curr_time += ctrl_period_;
+  }
+  x0 = x_curr; // set delay compensated initial state
+
+
+  /////////////// generate mpc matrix  ///////////////
+  /*
+   * predict equation: Xec = Aex * x0 + Bex * Uex + Wex
+   * cost function: J = Xex' * Qex * Xex + (Uex - Uref)' * Rex * (Uex - Urefex)
+   * Qex = diag([Q,Q,...]), Rex = diag([R,R,...])
+   */
+
+  Eigen::MatrixXd Aex = Eigen::MatrixXd::Zero(DIM_X * N, DIM_X);
+  Eigen::MatrixXd Bex = Eigen::MatrixXd::Zero(DIM_X * N, DIM_U * N);
+  Eigen::MatrixXd Wex = Eigen::MatrixXd::Zero(DIM_X * N, 1);
+  Eigen::MatrixXd Cex = Eigen::MatrixXd::Zero(DIM_Y * N, DIM_X * N);
+  Eigen::MatrixXd Qex = Eigen::MatrixXd::Zero(DIM_Y * N, DIM_Y * N);
+  Eigen::MatrixXd Rex = Eigen::MatrixXd::Zero(DIM_U * N, DIM_U * N);
+  Eigen::MatrixXd Urefex = Eigen::MatrixXd::Zero(DIM_U * N, 1);
+
+  /* weight matrix depends on the vehicle model */
+  Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y);
+  Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_U, DIM_U);
+  Eigen::MatrixXd Q_adaptive = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y);
+  Eigen::MatrixXd R_adaptive = Eigen::MatrixXd::Zero(DIM_U, DIM_U);
+  Q(0, 0) = mpc_param_.weight_lat_error;
+  Q(1, 1) = mpc_param_.weight_heading_error;
+  R(0, 0) = mpc_param_.weight_steering_input;
+
+  /* resample ref_traj with mpc sampling time */
+  std::vector<double> mpc_time_v;
+  for (int i = 0; i < N; ++i)
+  {
+    mpc_time_v.push_back(mpc_curr_time + i * DT);
+  }
+  MPCTrajectory mpc_resampled_ref_traj;
+  if (!MPCUtils::interp1dMPCTraj(ref_traj_.relative_time, ref_traj_, mpc_time_v, mpc_resampled_ref_traj))
+  {
+    ROS_WARN("[MPC] calculateMPC: mpc resample error, stop mpc calculation. check code!");
+    return false;
+  }
+
+  /* predict dynamics for N times */
+  for (int i = 0; i < N; ++i)
+  {
+    const double ref_k = mpc_resampled_ref_traj.k[i];
+    const double ref_vx = mpc_resampled_ref_traj.vx[i];
+    const double ref_vx_squared = ref_vx * ref_vx;
+
+    /* get discrete state matrix A, B, C, W */
+    vehicle_model_ptr_->setVelocity(ref_vx);
+    vehicle_model_ptr_->setCurvature(ref_k);
+    vehicle_model_ptr_->calculateDiscreteMatrix(Ad, Bd, Cd, Wd, DT);
+
+    Q_adaptive = Q;
+    R_adaptive = R;
+    if (i == N - 1)
+    {
+      Q_adaptive(0, 0) = mpc_param_.weight_terminal_lat_error;
+      Q_adaptive(1, 1) = mpc_param_.weight_terminal_heading_error;
+    }
+    Q_adaptive(1, 1) += ref_vx_squared * mpc_param_.weight_heading_error_squared_vel_coeff;
+    R_adaptive(0, 0) += ref_vx_squared * mpc_param_.weight_steering_input_squared_vel_coeff;
+
+    /* update mpc matrix */
+    int idx_x_i = i * DIM_X;
+    int idx_x_i_prev = (i - 1) * DIM_X;
+    int idx_u_i = i * DIM_U;
+    int idx_y_i = i * DIM_Y;
+    if (i == 0)
+    {
+      Aex.block(0, 0, DIM_X, DIM_X) = Ad;
+      Bex.block(0, 0, DIM_X, DIM_U) = Bd;
+      Wex.block(0, 0, DIM_X, 1) = Wd;
+    }
+    else
+    {
+      Aex.block(idx_x_i, 0, DIM_X, DIM_X) = Ad * Aex.block(idx_x_i_prev, 0, DIM_X, DIM_X);
+      for (int j = 0; j < i; ++j)
+      {
+        int idx_u_j = j * DIM_U;
+        Bex.block(idx_x_i, idx_u_j, DIM_X, DIM_U) = Ad * Bex.block(idx_x_i_prev, idx_u_j, DIM_X, DIM_U);
+      }
+      Wex.block(idx_x_i, 0, DIM_X, 1) = Ad * Wex.block(idx_x_i_prev, 0, DIM_X, 1) + Wd;
+    }
+    Bex.block(idx_x_i, idx_u_i, DIM_X, DIM_U) = Bd;
+    Cex.block(idx_y_i, idx_x_i, DIM_Y, DIM_X) = Cd;
+    Qex.block(idx_y_i, idx_y_i, DIM_Y, DIM_Y) = Q_adaptive;
+    Rex.block(idx_u_i, idx_u_i, DIM_U, DIM_U) = R_adaptive;
+
+    /* get reference input (feed-forward) */
+    vehicle_model_ptr_->calculateReferenceInput(Uref);
+    if (std::fabs(Uref(0, 0)) < amathutils::deg2rad(mpc_param_.zero_ff_steer_deg))
+    {
+      Uref(0, 0) = 0.0; // ignore curvature noise
+    }
+
+    Urefex.block(i * DIM_U, 0, DIM_U, 1) = Uref;
+
+    mpc_curr_time += DT;
+  }
+
+  /* add lateral jerk : weight for (v * {u(i) - u(i-1)} )^2 */
+  for (int i = 0; i < N - 1; ++i)
+  {
+    const double v = mpc_resampled_ref_traj.vx[i];
+    const double lateral_jerk_weight = v * v * mpc_param_.weight_lat_jerk;
+    Rex(i, i) += lateral_jerk_weight;
+    Rex(i + 1, i) -= lateral_jerk_weight;
+    Rex(i, i + 1) -= lateral_jerk_weight;
+    Rex(i + 1, i + 1) += lateral_jerk_weight;
+  }
+
+  if (Aex.array().isNaN().any() || Bex.array().isNaN().any() ||
+      Cex.array().isNaN().any() || Wex.array().isNaN().any())
+  {
+    ROS_WARN("[MPC] calculateMPC: model matrix includes NaN, stop MPC.");
+    return false;
+  }
+
+  /////////////// optimization ///////////////
+  /*
+   * solve quadratic optimization.
+   * cost function: 1/2 * Uex' * H * Uex + f' * Uex
+   */
+  const Eigen::MatrixXd CB = Cex * Bex;
+  const Eigen::MatrixXd QCB = Qex * CB;
+  Eigen::MatrixXd H = Eigen::MatrixXd::Zero(DIM_U * N, DIM_U * N);
+  H.triangularView<Eigen::Upper>() = CB.transpose() * QCB; // NOTE: This calculation is very heavy. searching for a good way...
+  H.triangularView<Eigen::Upper>() += Rex;
+  H.triangularView<Eigen::Lower>() = H.transpose();
+  Eigen::MatrixXd f = (Cex * (Aex * x0 + Wex)).transpose() * QCB - Urefex.transpose() * Rex;
+
+  /* constraint matrix : lb < U < ub, lbA < A*U < ubA */
+  const double u_lim = amathutils::deg2rad(steer_lim_deg_);
+  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(DIM_U * N, DIM_U * N);
+  Eigen::MatrixXd lbA = Eigen::MatrixXd::Zero(DIM_U * N, 1);
+  Eigen::MatrixXd ubA = Eigen::MatrixXd::Zero(DIM_U * N, 1);
+  Eigen::VectorXd lb = Eigen::VectorXd::Constant(DIM_U * N, -u_lim); // min steering angle
+  Eigen::VectorXd ub = Eigen::VectorXd::Constant(DIM_U * N, u_lim);  // max steering angle
+
+  auto start = std::chrono::system_clock::now();
+  Eigen::VectorXd Uex;
+  if (!qpsolver_ptr_->solve(H, f.transpose(), A, lb, ub, lbA, ubA, Uex))
+  {
+    ROS_WARN("[MPC] qp solver error");
+    return false;
+  }
+  double elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start).count() * 1.0e-6;
+  DEBUG_INFO("[MPC] calculateMPC: qp solver calculation time = %f [ms]", elapsed);
+
+  if (Uex.array().isNaN().any())
+  {
+    ROS_WARN("[MPC] calculateMPC: model Uex includes NaN, stop MPC. ");
+    return false;
+  }
+
+  /* saturation */
+  const double u_sat = std::max(std::min(Uex(0), u_lim), -u_lim);
+
+  /* filtering */
+  const double u_filtered = lpf_steering_cmd_.filter(u_sat);
+
+  /* set steering command */
+  steer_cmd = u_filtered;
+  steer_vel_cmd = (Uex(1) - Uex(0)) / DT;
+
+  /* Velocity control: for simplicity, now we calculate steer and speed separately */
+  vel_cmd = ref_traj_.vx[0];
+  acc_cmd = (ref_traj_.vx[1] - ref_traj_.vx[0]) / DT;
+
+  steer_cmd_prev_ = steer_cmd;
+
+  /* save to buffer */
+  input_buffer_.push_back(steer_cmd);
+  input_buffer_.pop_front();
+
+  DEBUG_INFO("[MPC] calculateMPC: mpc steer command raw = %f, filtered = %f, steer_vel_cmd = %f", Uex(0, 0), u_filtered, steer_vel_cmd);
+  DEBUG_INFO("[MPC] calculateMPC: mpc vel command = %f, acc_cmd = %f", vel_cmd, acc_cmd);
+
+  ////////////////// DEBUG ///////////////////
+
+  /* calculate predicted trajectory */
+  Eigen::VectorXd Xex = Aex * x0 + Bex * Uex + Wex;
+  MPCTrajectory debug_mpc_predicted_traj;
+  for (int i = 0; i < N; ++i)
+  {
+    const double lat_error = Xex(i * DIM_X);
+    const double yaw_error = Xex(i * DIM_X + 1);
+    const double x = mpc_resampled_ref_traj.x[i] - std::sin(mpc_resampled_ref_traj.yaw[i]) * lat_error;
+    const double y = mpc_resampled_ref_traj.y[i] + std::cos(mpc_resampled_ref_traj.yaw[i]) * lat_error;
+    const double z = mpc_resampled_ref_traj.z[i];
+    debug_mpc_predicted_traj.push_back(x, y, z, mpc_resampled_ref_traj.yaw[i] + yaw_error, 0, 0, 0);
+  }
+
+  /* publish for visualization */
+  visualization_msgs::Marker marker;
+  convertTrajToMarker(debug_mpc_predicted_traj, marker, "predicted_traj", 0.99, 0.99, 0.99, 0.2);
+  pub_debug_predicted_traj_.publish(marker);
+
+  /* publish debug values */
+  {
+    double curr_v = vehicle_status_.twist.linear.x;
+    double nearest_k = 0.0;
+    MPCUtils::interp1d(ref_traj_.relative_time, ref_traj_.k, nearest_traj_time, nearest_k);
+
+    std_msgs::Float64MultiArray debug_values;
+    debug_values.data.push_back(steer_cmd);                                      // [0] final steering command (MPC + LPF)
+    debug_values.data.push_back(u_sat);                                          // [1] mpc calculation result
+    debug_values.data.push_back(Urefex(0));                                      // [2] feedforward steering value
+    debug_values.data.push_back(std::atan(nearest_k * wheelbase_));              // [3] feedforward steering value raw
+    debug_values.data.push_back(steer);                                          // [4] current steering angle
+    debug_values.data.push_back(err_lat);                                        // [5] lateral error
+    debug_values.data.push_back(tf2::getYaw(vehicle_status_.pose.orientation));  // [6] current_pose yaw
+    debug_values.data.push_back(tf2::getYaw(nearest_pose.orientation));          // [7] nearest_pose yaw
+    debug_values.data.push_back(yaw_err);                                        // [8] yaw error
+    debug_values.data.push_back(vel_cmd);                                        // [9] command velocitys
+    debug_values.data.push_back(vehicle_status_.twist.linear.x);                 // [10] measured velocity
+    debug_values.data.push_back(curr_v * tan(steer_cmd) / wheelbase_);  // [11] angvel from steer comand (MPC assumes)
+    debug_values.data.push_back(curr_v * tan(steer) / wheelbase_);      // [12] angvel from measured steer
+    debug_values.data.push_back(curr_v * nearest_k);                    // [13] angvel from path curvature (Path angvel)
+    debug_values.data.push_back(nearest_k);                             // [14] nearest path curvature
+    debug_values.data.push_back(estimate_twist_.twist.linear.x);        // [15] current velocity
+    debug_values.data.push_back(estimate_twist_.twist.angular.z);       // [16] estimate twist angular velocity (real angvel)
+    pub_debug_values_.publish(debug_values);
+  }
+
+  return true;
+};
+
+void MPCFollower::callbackRefPath(const autoware_msgs::Lane::ConstPtr &msg)
+{
+  current_waypoints_ = *msg;
+  DEBUG_INFO("[MPC] path callback: received path size = %lu", current_waypoints_.waypoints.size());
+
+  MPCTrajectory traj;
+
+  /* calculate relative time */
+  std::vector<double> relative_time;
+  MPCUtils::calcPathRelativeTime(current_waypoints_, relative_time);
+  DEBUG_INFO("[MPC] path callback: relative_time.size() = %lu, front() = %f, back() = %f",
+             relative_time.size(), relative_time.front(), relative_time.back());
+
+  /* resampling */
+  MPCUtils::convertWaypointsToMPCTrajWithDistanceResample(current_waypoints_, relative_time, traj_resample_dist_, traj);
+  MPCUtils::convertEulerAngleToMonotonic(traj.yaw);
+  DEBUG_INFO("[MPC] path callback: resampled traj size() = %lu", traj.relative_time.size());
+
+  /* path smoothing */
+  if (enable_path_smoothing_)
+  {
+    for (int i = 0; i < path_smoothing_times_; ++i)
+    {
+      if (!MoveAverageFilter::filt_vector(path_filter_moving_ave_num_, traj.x) ||
+          !MoveAverageFilter::filt_vector(path_filter_moving_ave_num_, traj.y) ||
+          !MoveAverageFilter::filt_vector(path_filter_moving_ave_num_, traj.yaw) ||
+          !MoveAverageFilter::filt_vector(path_filter_moving_ave_num_, traj.vx))
+      {
+        ROS_WARN("[MPC] path callback: filtering error. stop filtering");
+        return;
+      }
+    }
+  }
+
+  /* calculate yaw angle */
+  if (enable_yaw_recalculation_)
+  {
+    MPCUtils::calcTrajectoryYawFromXY(traj);
+    MPCUtils::convertEulerAngleToMonotonic(traj.yaw);
+  }
+
+  /* calculate curvature */
+  MPCUtils::calcTrajectoryCurvature(traj, curvature_smoothing_num_);
+  const double max_k = *max_element(traj.k.begin(), traj.k.end());
+  const double min_k = *min_element(traj.k.begin(), traj.k.end());
+  DEBUG_INFO("[MPC] path callback: trajectory curvature : max_k = %f, min_k = %f", max_k, min_k);
+
+  /* add end point with vel=0 on traj for mpc prediction */
+  const double mpc_predict_time_length = (mpc_param_.prediction_horizon + 1) * mpc_param_.prediction_sampling_time + mpc_param_.delay_compensation_time + ctrl_period_;
+  const double end_velocity = 0.0;
+  traj.vx.back() = end_velocity; // also for end point
+  traj.push_back(traj.x.back(), traj.y.back(), traj.z.back(), traj.yaw.back(),
+                 end_velocity, traj.k.back(), traj.relative_time.back() + mpc_predict_time_length);
+
+  if (!traj.size())
+  {
+    ROS_ERROR("[MPC] path callback: trajectory size is undesired.");
+    DEBUG_INFO("size: x=%lu, y=%lu, z=%lu, yaw=%lu, v=%lu,k=%lu,t=%lu", traj.x.size(), traj.y.size(),
+               traj.z.size(), traj.yaw.size(), traj.vx.size(), traj.k.size(), traj.relative_time.size());
+    return;
+  }
+
+  ref_traj_ = traj;
+
+  /* publish trajectory for visualize */
+  visualization_msgs::Marker markers;
+  convertTrajToMarker(ref_traj_, markers, "ref_traj", 0.0, 0.5, 1.0, 0.05);
+  pub_debug_filtered_traj_.publish(markers);
+};
+
+void MPCFollower::convertTrajToMarker(const MPCTrajectory &traj, visualization_msgs::Marker &marker,
+                                      std::string ns, double r, double g, double b, double z)
+{
+  marker.points.clear();
+  marker.header.frame_id = current_waypoints_.header.frame_id;
+  marker.header.stamp = ros::Time();
+  marker.ns = ns;
+  marker.id = 0;
+  marker.type = visualization_msgs::Marker::LINE_STRIP;
+  marker.action = visualization_msgs::Marker::ADD;
+  marker.scale.x = 0.15;
+  marker.scale.y = 0.3;
+  marker.scale.z = 0.3;
+  marker.color.a = 0.9;
+  marker.color.r = r;
+  marker.color.g = g;
+  marker.color.b = b;
+  for (unsigned int i = 0; i < traj.x.size(); ++i)
+  {
+    geometry_msgs::Point p;
+    p.x = traj.x.at(i);
+    p.y = traj.y.at(i);
+    p.z = traj.z.at(i) + z;
+    marker.points.push_back(p);
+  }
+}
+
+void MPCFollower::callbackPose(const geometry_msgs::PoseStamped::ConstPtr &msg)
+{
+  vehicle_status_.header = msg->header;
+  vehicle_status_.pose = msg->pose;
+  my_position_ok_ = true;
+};
+
+void MPCFollower::callbackVehicleStatus(const autoware_msgs::VehicleStatus &msg)
+{
+  vehicle_status_.tire_angle_rad = msg.angle;
+  vehicle_status_.twist.linear.x = amathutils::kmph2mps(msg.speed);
+  my_steering_ok_ = true;
+  my_velocity_ok_ = true;
+};
+
+void MPCFollower::publishControlCommands(const double &vel_cmd, const double &acc_cmd,
+                                         const double &steer_cmd, const double &steer_vel_cmd)
+{
+  const double omega_cmd = vehicle_status_.twist.linear.x * std::tan(steer_cmd) / wheelbase_;
+  if (output_interface_ == "twist")
+  {
+    publishTwist(vel_cmd, omega_cmd);
+  }
+  else if (output_interface_ == "ctrl_raw")
+  {
+    publishCtrlCmd(vel_cmd, acc_cmd, steer_cmd);
+  }
+  else if (output_interface_ == "all")
+  {
+    publishTwist(vel_cmd, omega_cmd);
+    publishCtrlCmd(vel_cmd, acc_cmd, steer_cmd);
+  }
+  else
+  {
+    ROS_WARN("[MPC] control command interface is not appropriate");
+  }
+}
+
+void MPCFollower::publishTwist(const double &vel_cmd, const double &omega_cmd)
+{
+  /* convert steering to twist */
+  geometry_msgs::TwistStamped twist;
+  twist.header.frame_id = "/base_link";
+  twist.header.stamp = ros::Time::now();
+  twist.twist.linear.x = vel_cmd;
+  twist.twist.linear.y = 0.0;
+  twist.twist.linear.z = 0.0;
+  twist.twist.angular.x = 0.0;
+  twist.twist.angular.y = 0.0;
+  twist.twist.angular.z = omega_cmd;
+  pub_twist_cmd_.publish(twist);
+}
+
+void MPCFollower::publishCtrlCmd(const double &vel_cmd, const double &acc_cmd, const double &steer_cmd)
+{
+  autoware_msgs::ControlCommandStamped cmd;
+  cmd.header.frame_id = "/base_link";
+  cmd.header.stamp = ros::Time::now();
+  cmd.cmd.linear_velocity = vel_cmd;
+  cmd.cmd.linear_acceleration = acc_cmd;
+  cmd.cmd.steering_angle = steer_cmd;
+  pub_steer_vel_ctrl_cmd_.publish(cmd);
+}
+
+MPCFollower::~MPCFollower()
+{
+  ROS_INFO("Publish 0 twist before I died.");
+  double vel_cmd = 0.0;
+  double acc_cmd = 0.0;
+  double steer_cmd = 0.0;
+  double steer_vel_cmd = 0.0;
+  if (my_steering_ok_)
+    steer_cmd = vehicle_status_.tire_angle_rad;
+  publishControlCommands(vel_cmd, acc_cmd, steer_cmd, steer_vel_cmd);
+};

src/ros/catkin/src/mpc_follower/src/mpc_follower_node.cpp

@@ -0,0 +1,25 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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 "mpc_follower/mpc_follower_core.h"
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "mpc_follower");
+  MPCFollower obj;
+  ros::spin();
+  return 0;
+};

src/ros/catkin/src/mpc_follower/src/mpc_trajectory.cpp

@@ -0,0 +1,55 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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 "mpc_follower/mpc_trajectory.h"
+
+void MPCTrajectory::push_back(const double &xp, const double &yp, const double &zp,
+                              const double &yawp, const double &vxp, const double &kp,
+                              const double &tp)
+{
+  x.push_back(xp);
+  y.push_back(yp);
+  z.push_back(zp);
+  yaw.push_back(yawp);
+  vx.push_back(vxp);
+  k.push_back(kp);
+  relative_time.push_back(tp);
+};
+
+void MPCTrajectory::clear()
+{
+  x.clear();
+  y.clear();
+  z.clear();
+  yaw.clear();
+  vx.clear();
+  k.clear();
+  relative_time.clear();
+};
+
+unsigned int MPCTrajectory::size() const
+{
+  if (x.size() == y.size() && x.size() == z.size() && x.size() == yaw.size() &&
+      x.size() == vx.size() && x.size() == k.size() && x.size() == relative_time.size())
+  {
+    return x.size();
+  }
+  else
+  {
+    std::cerr << "[MPC trajectory] trajectory size is inappropriate" << std::endl;
+    return 0;
+  }
+}

src/ros/catkin/src/mpc_follower/src/mpc_utils.cpp

@@ -0,0 +1,488 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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 "mpc_follower/mpc_utils.h"
+
+void MPCUtils::convertEulerAngleToMonotonic(std::vector<double> &a)
+{
+  for (unsigned int i = 1; i < a.size(); ++i)
+  {
+    const double da = a[i] - a[i - 1];
+    a[i] = a[i - 1] + amathutils::normalizeRadian(da);
+  }
+}
+
+template <typename T1, typename T2>
+bool MPCUtils::interp1d(const T1 &index, const T2 &values, const double &ref, double &ret)
+{
+  ret = 0.0;
+  if (!((int)index.size() == (int)values.size()))
+  {
+    printf("index and values must have same size, return false. size : idx = %d, values = %d\n", (int)index.size(), (int)values.size());
+    return false;
+  }
+  if (index.size() == 1)
+  {
+    printf("index size is 1, too short. return false.\n");
+    return false;
+  }
+  unsigned int end = index.size() - 1;
+  if (ref < index[0])
+  {
+    ret = values[0];
+    // printf("ref point is out of index (low), return false.\n");
+    return true;
+  }
+  if (index[end] < ref)
+  {
+    ret = values[end];
+    // printf("ref point is out of index (high), return false.\n");
+    return true;
+  }
+
+  for (unsigned int i = 1; i < index.size(); ++i)
+  {
+    if (!(index[i] > index[i - 1]))
+    {
+      printf("index must be monotonically increasing, return false. index[%d] = %f, but index[%d] = %f\n", i, index[i], i-1, index[i - 1]);
+      return false;
+    }
+  }
+  unsigned int i = 1;
+  while (ref > index[i])
+  {
+    ++i;
+  }
+  const double a = ref - index[i - 1];
+  const double d_index = index[i] - index[i - 1];
+  ret = ((d_index - a) * values[i - 1] + a * values[i]) / d_index;
+  return true;
+}
+template bool MPCUtils::interp1d<std::vector<double>, std::vector<double>>(const std::vector<double> &, const std::vector<double> &, const double &, double &);
+template bool MPCUtils::interp1d<std::vector<double>, Eigen::VectorXd>(const std::vector<double> &, const Eigen::VectorXd &, const double &, double &);
+template bool MPCUtils::interp1d<Eigen::VectorXd, std::vector<double>>(const Eigen::VectorXd &, const std::vector<double> &, const double &, double &);
+template bool MPCUtils::interp1d<Eigen::VectorXd, Eigen::VectorXd>(const Eigen::VectorXd &, const Eigen::VectorXd &, const double &, double &);
+
+// 1D interpolation
+bool MPCUtils::interp1dMPCTraj(const std::vector<double> &index, const MPCTrajectory &values,
+                               const std::vector<double> &ref_time, MPCTrajectory &ret)
+{
+  if (!(index.size() == values.size()))
+  {
+    printf("index and values must have same size, return false.\n");
+    return false;
+  }
+  if (index.size() == 1)
+  {
+    printf("index size is 1, too short. return false.\n");
+    return false;
+  }
+
+  for (unsigned int i = 1; i < index.size(); ++i)
+  {
+    if (!(index[i] > index[i - 1]))
+    {
+      printf("index must be monotonically increasing, return false. index[%d] = %f, but index[%d] = %f\n", i, index[i], i-1, index[i - 1]);
+      return false;
+    }
+  }
+
+  for (unsigned int i = 1; i < ref_time.size(); ++i)
+  {
+    if (!(ref_time[i] > ref_time[i - 1]))
+    {
+      printf("reference point must be monotonically increasing, return false. ref_time[%d] = %f, but ref_time[%d] = %f\n", i, ref_time[i], i-1, ref_time[i - 1]);
+      return false;
+    }
+  }
+
+  ret.clear();
+  unsigned int i = 1;
+  for (unsigned int j = 0; j < ref_time.size(); ++j)
+  {
+    double a, d_index;
+    if (ref_time[j] > index.back())
+    {
+      a = 1.0;
+      d_index = 1.0;
+      i = index.size() - 1;
+    }
+    else if (ref_time[j] < index.front())
+    {
+      a = 0.0;
+      d_index = 1.0;
+      i = 1;
+    }
+    else
+    {
+      while (ref_time[j] > index[i])
+      {
+        ++i;
+      }
+      a = ref_time[j] - index[i - 1];
+      d_index = index[i] - index[i - 1];
+    }
+    const double x = ((d_index - a) * values.x[i - 1] + a * values.x[i]) / d_index;
+    const double y = ((d_index - a) * values.y[i - 1] + a * values.y[i]) / d_index;
+    const double z = ((d_index - a) * values.z[i - 1] + a * values.z[i]) / d_index;
+    const double yaw = ((d_index - a) * values.yaw[i - 1] + a * values.yaw[i]) / d_index;
+    const double vx = ((d_index - a) * values.vx[i - 1] + a * values.vx[i]) / d_index;
+    const double k = ((d_index - a) * values.k[i - 1] + a * values.k[i]) / d_index;
+    const double t = ref_time[j];
+    ret.push_back(x, y, z, yaw, vx, k, t);
+  }
+  return true;
+}
+
+void MPCUtils::calcTrajectoryYawFromXY(MPCTrajectory &traj)
+{
+  if (traj.yaw.size() == 0)
+    return;
+
+  for (unsigned int i = 1; i < traj.yaw.size() - 1; ++i)
+  {
+    const double dx = traj.x[i + 1] - traj.x[i - 1];
+    const double dy = traj.y[i + 1] - traj.y[i - 1];
+    traj.yaw[i] = std::atan2(dy, dx);
+  }
+  if (traj.yaw.size() > 1)
+  {
+    traj.yaw[0] = traj.yaw[1];
+    traj.yaw.back() = traj.yaw[traj.yaw.size() - 2];
+  }
+}
+
+void MPCUtils::calcTrajectoryCurvature(MPCTrajectory &traj, int curvature_smoothing_num)
+{
+  unsigned int traj_k_size = traj.x.size();
+  traj.k.clear();
+
+  /* calculate curvature by circle fitting from three points */
+  geometry_msgs::Point p1, p2, p3;
+  for (unsigned int i = curvature_smoothing_num; i < traj_k_size - curvature_smoothing_num; ++i)
+  {
+    p1.x = traj.x[i - curvature_smoothing_num];
+    p2.x = traj.x[i];
+    p3.x = traj.x[i + curvature_smoothing_num];
+    p1.y = traj.y[i - curvature_smoothing_num];
+    p2.y = traj.y[i];
+    p3.y = traj.y[i + curvature_smoothing_num];
+    double den = std::max(amathutils::find_distance(p1, p2) * amathutils::find_distance(p2, p3) * amathutils::find_distance(p3, p1), 0.0001);
+    const double curvature = 2.0 * ((p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x)) / den;
+    traj.k.push_back(curvature);
+  }
+
+  /* first and last curvature is copied from next value */
+  for (int i = 0; i < curvature_smoothing_num; ++i)
+  {
+    traj.k.insert(traj.k.begin(), traj.k.front());
+    traj.k.push_back(traj.k.back());
+  }
+}
+
+void MPCUtils::convertWaypointsToMPCTraj(const autoware_msgs::Lane &lane, MPCTrajectory &mpc_traj)
+{
+  mpc_traj.clear();
+  const double k_tmp = 0.0;
+  const double t_tmp = 0.0;
+  for (const auto &wp : lane.waypoints)
+  {
+    const double x = wp.pose.pose.position.x;
+    const double y = wp.pose.pose.position.y;
+    const double z = wp.pose.pose.position.z;
+    const double yaw = tf2::getYaw(wp.pose.pose.orientation);
+    const double vx = wp.twist.twist.linear.x;
+    mpc_traj.push_back(x, y, z, yaw, vx, k_tmp, t_tmp);
+  }
+}
+
+void MPCUtils::convertWaypointsToMPCTrajWithDistanceResample(const autoware_msgs::Lane &path, const std::vector<double> &path_time,
+                                                             const double &dl, MPCTrajectory &ref_traj)
+{
+  ref_traj.clear();
+  double dist = 0.0;
+  std::vector<double> dists;
+  dists.push_back(0.0);
+
+  for (int i = 1; i < (int)path_time.size(); ++i)
+  {
+    double dx = path.waypoints.at(i).pose.pose.position.x - path.waypoints.at(i - 1).pose.pose.position.x;
+    double dy = path.waypoints.at(i).pose.pose.position.y - path.waypoints.at(i - 1).pose.pose.position.y;
+    dist += sqrt(dx * dx + dy * dy);
+    dists.push_back(dist);
+  }
+
+  convertWaypointsToMPCTrajWithResample(path, path_time, dists, dl, ref_traj);
+}
+
+
+void MPCUtils::convertWaypointsToMPCTrajWithTimeResample(const autoware_msgs::Lane &path, const std::vector<double> &path_time,
+                                                         const double &dt, MPCTrajectory &ref_traj)
+{
+  ref_traj.clear();
+  convertWaypointsToMPCTrajWithResample(path, path_time, path_time, dt, ref_traj);
+}
+
+void MPCUtils::convertWaypointsToMPCTrajWithResample(const autoware_msgs::Lane &path, const std::vector<double> &path_time,
+                                                     const std::vector<double> &ref_index, const double &d_ref_index, MPCTrajectory &ref_traj)
+{
+  if (ref_index.size() == 0) {
+    return;
+  }
+
+  for (unsigned int i = 1; i < ref_index.size(); ++i)
+  {
+    if (ref_index[i] < ref_index[i - 1])
+    {
+      ROS_ERROR("[convertWaypointsToMPCTrajWithResample] resampling index must be monotonically increasing. idx[%d] = %f, idx[%d+1] = %f",
+                i, ref_index[i], i, ref_index[i + 1]);
+      return;
+    }
+  }
+
+  double point = ref_index[0];
+  while (point < ref_index.back())
+  {
+    unsigned int j = 1;
+    while (point > ref_index.at(j))
+    {
+      ++j;
+    }
+
+    const double a = point - ref_index.at(j - 1);
+    const double ref_index_dist = ref_index.at(j) - ref_index.at(j - 1);
+    const geometry_msgs::Pose pos0 = path.waypoints.at(j - 1).pose.pose;
+    const geometry_msgs::Pose pos1 = path.waypoints.at(j).pose.pose;
+    const geometry_msgs::Twist twist0 = path.waypoints.at(j - 1).twist.twist;
+    const geometry_msgs::Twist twist1 = path.waypoints.at(j).twist.twist;
+    const double x = ((ref_index_dist - a) * pos0.position.x + a * pos1.position.x) / ref_index_dist;
+    const double y = ((ref_index_dist - a) * pos0.position.y + a * pos1.position.y) / ref_index_dist;
+    const double z = ((ref_index_dist - a) * pos0.position.z + a * pos1.position.z) / ref_index_dist;
+
+    /* for singular point of euler angle */
+    const double yaw0 = tf2::getYaw(pos0.orientation);
+    const double dyaw = amathutils::normalizeRadian(tf2::getYaw(pos1.orientation) - yaw0);
+    const double yaw1 = yaw0 + dyaw;
+    const double yaw = ((ref_index_dist - a) * yaw0 + a * yaw1) / ref_index_dist;
+    const double vx = ((ref_index_dist - a) * twist0.linear.x + a * twist1.linear.x) / ref_index_dist;
+    const double curvature_tmp = 0.0;
+    const double t = ((ref_index_dist - a) * path_time.at(j - 1) + a * path_time.at(j)) / ref_index_dist;
+    ref_traj.push_back(x, y, z, yaw, vx, curvature_tmp, t);
+    point += d_ref_index;
+  }
+}
+
+void MPCUtils::calcPathRelativeTime(const autoware_msgs::Lane &path, std::vector<double> &path_time)
+{
+  double t = 0.0;
+  path_time.clear();
+  path_time.push_back(t);
+  for (int i = 0; i < (int)path.waypoints.size() - 1; ++i)
+  {
+    const double x0 = path.waypoints.at(i).pose.pose.position.x;
+    const double y0 = path.waypoints.at(i).pose.pose.position.y;
+    const double z0 = path.waypoints.at(i).pose.pose.position.z;
+    const double x1 = path.waypoints.at(i + 1).pose.pose.position.x;
+    const double y1 = path.waypoints.at(i + 1).pose.pose.position.y;
+    const double z1 = path.waypoints.at(i + 1).pose.pose.position.z;
+    const double dx = x1 - x0;
+    const double dy = y1 - y0;
+    const double dz = z1 - z0;
+    const double dist = sqrt(dx * dx + dy * dy + dz * dz);
+    double v = std::max(std::fabs(path.waypoints.at(i).twist.twist.linear.x), 1.0);
+    t += (dist / v);
+    path_time.push_back(t);
+  }
+}
+
+bool MPCUtils::calcNearestPose(const MPCTrajectory &traj, const geometry_msgs::Pose &self_pose, geometry_msgs::Pose &nearest_pose,
+                               unsigned int &nearest_index, double &min_dist_error, double &nearest_yaw_error, double &nearest_time)
+{
+  int nearest_index_tmp = -1;
+  double min_dist_squared = std::numeric_limits<double>::max();
+  nearest_yaw_error = std::numeric_limits<double>::max();
+  for (uint i = 0; i < traj.size(); ++i)
+  {
+    const double dx = self_pose.position.x - traj.x[i];
+    const double dy = self_pose.position.y - traj.y[i];
+    const double dist_squared = dx * dx + dy * dy;
+
+    /* ignore when yaw error is large, for crossing path */
+    const double err_yaw = amathutils::normalizeRadian(tf2::getYaw(self_pose.orientation) - traj.yaw[i]);
+    if (fabs(err_yaw) < (M_PI / 3.0))
+    {
+      if (dist_squared < min_dist_squared)
+      {
+        /* save nearest index */
+        min_dist_squared = dist_squared;
+        nearest_yaw_error = err_yaw;
+        nearest_index_tmp = i;
+      }
+    }
+  }
+  if (nearest_index_tmp == -1)
+  {
+    ROS_WARN("[calcNearestPose] yaw error is over PI/3 for all waypoints. no closest waypoint found.");
+    return false;
+  }
+
+  nearest_index = nearest_index_tmp;
+
+  min_dist_error = std::sqrt(min_dist_squared);
+  nearest_time = traj.relative_time[nearest_index];
+  nearest_pose.position.x = traj.x[nearest_index];
+  nearest_pose.position.y = traj.y[nearest_index];
+  nearest_pose.orientation = amathutils::getQuaternionFromYaw(traj.yaw[nearest_index]);
+  return true;
+};
+
+bool MPCUtils::calcNearestPoseInterp(const MPCTrajectory &traj, const geometry_msgs::Pose &self_pose, geometry_msgs::Pose &nearest_pose,
+                                     unsigned int &nearest_index, double &min_dist_error, double &nearest_yaw_error, double &nearest_time)
+{
+
+  if (traj.size() == 0)
+  {
+    ROS_WARN("[calcNearestPoseInterp] trajectory size is zero");
+    return false;
+  }
+  const double my_x = self_pose.position.x;
+  const double my_y = self_pose.position.y;
+  const double my_yaw = tf2::getYaw(self_pose.orientation);
+
+  int nearest_index_tmp = -1;
+  double min_dist_squared = std::numeric_limits<double>::max();
+  for (uint i = 0; i < traj.size(); ++i)
+  {
+    const double dx = my_x - traj.x[i];
+    const double dy = my_y - traj.y[i];
+    const double dist_squared = dx * dx + dy * dy;
+
+    /* ignore when yaw error is large, for crossing path */
+    const double err_yaw = amathutils::normalizeRadian(my_yaw - traj.yaw[i]);
+    if (fabs(err_yaw) < (M_PI / 3.0))
+    {
+      if (dist_squared < min_dist_squared)
+      {
+        /* save nearest index */
+        min_dist_squared = dist_squared;
+        nearest_index_tmp = i;
+      }
+    }
+  }
+  if (nearest_index_tmp == -1)
+  {
+    ROS_WARN("[calcNearestPoseInterp] yaw error is over PI/3 for all waypoints. no closest waypoint found.");
+    return false;
+  }
+
+  nearest_index = nearest_index_tmp;
+
+  if (traj.size() == 1)
+  {
+    nearest_pose.position.x = traj.x[nearest_index];
+    nearest_pose.position.y = traj.y[nearest_index];
+    tf2::Quaternion q;
+    q.setRPY(0, 0, traj.yaw[nearest_index]);
+    nearest_pose.orientation = tf2::toMsg(q);
+    nearest_time = traj.relative_time[nearest_index];
+    min_dist_error = std::sqrt(min_dist_squared);
+    nearest_yaw_error = amathutils::normalizeRadian(my_yaw - traj.yaw[nearest_index]);
+    return true;
+  }
+
+  /* get second nearest index = next to nearest_index */
+  int second_nearest_index = 0;
+  if (nearest_index == traj.size() - 1)
+    second_nearest_index = nearest_index - 1;
+  else if (nearest_index == 0)
+    second_nearest_index = 1;
+  else
+  {
+    double dx1, dy1, dist_squared1, dx2, dy2, dist_squared2;
+    dx1 = my_x - traj.x[nearest_index + 1];
+    dy1 = my_y - traj.y[nearest_index + 1];
+    dist_squared1 = dx1 * dx1 + dy1 * dy1;
+    dx2 = my_x - traj.x[nearest_index - 1];
+    dy2 = my_y - traj.y[nearest_index - 1];
+    dist_squared2 = dx2 * dx2 + dy2 * dy2;
+    if (dist_squared1 < dist_squared2)
+      second_nearest_index = nearest_index + 1;
+    else
+      second_nearest_index = nearest_index - 1;
+  }
+
+  const double a_sq = min_dist_squared;
+
+  /* distance between my position and second nearest position */
+  const double dx2 = my_x - traj.x[second_nearest_index];
+  const double dy2 = my_y - traj.y[second_nearest_index];
+  const double b_sq = dx2 * dx2 + dy2 * dy2;
+
+  /* distance between first and second nearest position */
+  const double dx3 = traj.x[nearest_index] - traj.x[second_nearest_index];
+  const double dy3 = traj.y[nearest_index] - traj.y[second_nearest_index];
+  const double c_sq = dx3 * dx3 + dy3 * dy3;
+
+  /* if distance between two points are too close */
+  if (c_sq < 1.0E-5)
+  {
+    nearest_pose.position.x = traj.x[nearest_index];
+    nearest_pose.position.y = traj.y[nearest_index];
+    tf2::Quaternion q;
+    q.setRPY(0, 0, traj.yaw[nearest_index]);
+    nearest_pose.orientation = tf2::toMsg(q);
+    nearest_time = traj.relative_time[nearest_index];
+    min_dist_error = std::sqrt(min_dist_squared);
+    nearest_yaw_error = amathutils::normalizeRadian(my_yaw - traj.yaw[nearest_index]);
+    return true;
+  }
+
+  /* linear interpolation */
+  const double alpha = 0.5 * (c_sq - a_sq + b_sq) / c_sq;
+  nearest_pose.position.x = alpha * traj.x[nearest_index] + (1 - alpha) * traj.x[second_nearest_index];
+  nearest_pose.position.y = alpha * traj.y[nearest_index] + (1 - alpha) * traj.y[second_nearest_index];
+  double tmp_yaw_err = traj.yaw[nearest_index] - traj.yaw[second_nearest_index];
+  if (tmp_yaw_err > M_PI)
+  {
+    tmp_yaw_err -= 2.0 * M_PI;
+  }
+  else if (tmp_yaw_err < -M_PI)
+  {
+    tmp_yaw_err += 2.0 * M_PI;
+  }
+  const double nearest_yaw = traj.yaw[second_nearest_index] + alpha * tmp_yaw_err;
+  tf2::Quaternion q;
+  q.setRPY(0, 0, nearest_yaw);
+  nearest_pose.orientation = tf2::toMsg(q);
+  nearest_time = alpha * traj.relative_time[nearest_index] + (1 - alpha) * traj.relative_time[second_nearest_index];
+
+  /* calcuate the perpendicular distance from ego position to the line joining
+     2 nearest way points. */
+  auto min_dist_err_sq = b_sq - c_sq * alpha * alpha;
+
+  /* If ego vehicle is very close to or on the line, min_dist_err_sq would be
+     very close to 0, any rounding error in the floating point arithmetic
+     could cause it to become negative. Hence its value is limited to 0
+     in order to perform sqrt. */
+  if (min_dist_err_sq < 0) {
+    min_dist_err_sq = 0;
+  }
+
+  min_dist_error = std::sqrt(min_dist_err_sq);
+
+  nearest_yaw_error = amathutils::normalizeRadian(my_yaw - nearest_yaw);
+  return true;
+}

src/ros/catkin/src/mpc_follower/src/mpc_waypoints_converter.cpp

@@ -0,0 +1,121 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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 <iostream>
+#include <ros/ros.h>
+#include <std_msgs/Int32.h>
+#include <autoware_msgs/Lane.h>
+
+class MPCWaypointsConverter
+{
+public:
+  MPCWaypointsConverter()
+  {
+    pub_waypoints_ = nh_.advertise<autoware_msgs::Lane>("/mpc_waypoints", 1);
+    sub_closest_waypoint_ = nh_.subscribe("/closest_waypoint", 1, &MPCWaypointsConverter::callbackClosestWaypoints, this);
+    sub_base_waypoints_ = nh_.subscribe("/base_waypoints", 1, &MPCWaypointsConverter::callbackBaseWaypoints, this);
+    sub_final_waypoints_ = nh_.subscribe("/final_waypoints", 1, &MPCWaypointsConverter::callbackFinalWaypoints, this);
+
+    closest_idx_ = 0;
+    back_waypoints_num_ = 10;
+    front_waypoints_num_ = 50;
+  };
+  ~MPCWaypointsConverter(){};
+
+private:
+  ros::NodeHandle nh_, pnh_;
+  ros::Publisher pub_waypoints_;
+  ros::Subscriber sub_closest_waypoint_, sub_base_waypoints_, sub_final_waypoints_, sub_current_velocity_;
+
+  autoware_msgs::Lane base_waypoints_;
+  int closest_idx_;
+  int back_waypoints_num_;
+  int front_waypoints_num_;
+
+  void callbackClosestWaypoints(const std_msgs::Int32 msg)
+  {
+    closest_idx_ = msg.data;
+  }
+
+  void callbackBaseWaypoints(const autoware_msgs::Lane &msg)
+  {
+    base_waypoints_ = msg;
+  }
+
+  void callbackFinalWaypoints(const autoware_msgs::Lane &final_waypoints)
+  {
+    if (base_waypoints_.waypoints.size() == 0 || final_waypoints.waypoints.size() == 0)
+      return;
+
+    if ((int)base_waypoints_.waypoints.size() - 1 < closest_idx_)
+    {
+      ROS_WARN("base_waypoints_.waypoints.size() - 1 = %d, closest_idx_ = %d", (int)base_waypoints_.waypoints.size(), closest_idx_);
+      return;
+    }
+
+    auto sq_dist = [](const geometry_msgs::Point &a, const geometry_msgs::Point &b) {
+      const double dx = a.x - b.x;
+      const double dy = a.y - b.y;
+      return dx * dx + dy * dy;
+    };
+
+    autoware_msgs::Lane mpc_waypoints;
+    mpc_waypoints.header = final_waypoints.header;
+    mpc_waypoints.increment = final_waypoints.increment;
+    mpc_waypoints.lane_id = final_waypoints.lane_id;
+    mpc_waypoints.lane_index = final_waypoints.lane_index;
+    mpc_waypoints.cost = final_waypoints.cost;
+    mpc_waypoints.closest_object_distance = final_waypoints.closest_object_distance;
+    mpc_waypoints.closest_object_velocity = final_waypoints.closest_object_velocity;
+    mpc_waypoints.is_blocked = final_waypoints.is_blocked;
+
+    // find closest point index in base_waypoints (topic /closest_waypoints has no consistency with /final_waypoints due to delay)
+    int closest_idx = -1;
+    for (int i = 0; i < (int)base_waypoints_.waypoints.size(); ++i) {
+      const double d = sq_dist(final_waypoints.waypoints[1].pose.pose.position, base_waypoints_.waypoints[i].pose.pose.position);
+      if (d < 0.01) {
+        closest_idx = i;
+        break;
+      }
+    }
+    if (closest_idx == -1) {
+      ROS_ERROR("cannot find closest base_waypoints' waypoint to final_waypoints.waypoint[1] !!");
+    }
+
+    int base_start = std::max(closest_idx - back_waypoints_num_, 0);
+    for (int i = base_start; i < closest_idx; ++i)
+    {
+      mpc_waypoints.waypoints.push_back(base_waypoints_.waypoints.at(i));
+      mpc_waypoints.waypoints.back().twist = final_waypoints.waypoints[1].twist;
+    }
+
+    int final_end = std::min(front_waypoints_num_ + 1, (int)final_waypoints.waypoints.size());
+    for (int i = 1; i < final_end; ++i)
+    {
+      mpc_waypoints.waypoints.push_back(final_waypoints.waypoints.at(i));
+    }
+
+    pub_waypoints_.publish(mpc_waypoints);
+  }
+};
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "mpc_waypoints_converter");
+  MPCWaypointsConverter obj;
+  ros::spin();
+  return 0;
+};

src/ros/catkin/src/mpc_follower/src/qp_solver/qp_solver_qpoases.cpp

@@ -0,0 +1,96 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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 "mpc_follower/qp_solver/qp_solver_qpoases.h"
+
+QPSolverQpoasesHotstart::QPSolverQpoasesHotstart(const int max_iter)
+  : is_solver_initialized_(false), max_iter_(max_iter){};
+
+bool QPSolverQpoasesHotstart::solve(const Eigen::MatrixXd& Hmat, const Eigen::MatrixXd& fvec, const Eigen::MatrixXd& A,
+                                    const Eigen::VectorXd& lb, const Eigen::VectorXd& ub, const Eigen::MatrixXd& lbA,
+                                    const Eigen::MatrixXd& ubA, Eigen::VectorXd& U)
+{
+  int max_iter = max_iter_; // redeclaration to give a non-const value to solver
+
+  const int kNumOfMatrixElements = Hmat.rows() * Hmat.cols();
+  double h_matrix[kNumOfMatrixElements];
+
+  const int kNumOfOffsetRows = fvec.rows();
+  double g_matrix[kNumOfOffsetRows];
+
+  double lower_bound[kNumOfOffsetRows];
+  double upper_bound[kNumOfOffsetRows];
+
+  double result[kNumOfOffsetRows];
+  U = Eigen::VectorXd::Zero(kNumOfOffsetRows);
+
+  Eigen::MatrixXd Aconstraint = Eigen::MatrixXd::Identity(kNumOfOffsetRows, kNumOfOffsetRows);
+  double a_constraint_matirx[kNumOfMatrixElements];
+
+  int index = 0;
+
+  for (int r = 0; r < Hmat.rows(); ++r)
+  {
+    g_matrix[r] = fvec(r, 0);
+    for (int c = 0; c < Hmat.cols(); ++c)
+    {
+      h_matrix[index] = Hmat(r, c);
+      a_constraint_matirx[index] = Aconstraint(r, c);
+      index++;
+    }
+  }
+
+  for (int i = 0; i < kNumOfOffsetRows; ++i)
+  {
+    lower_bound[i] = lb[i];
+    upper_bound[i] = ub[i];
+  }
+
+  solver_.setPrintLevel(qpOASES::PL_NONE); // options: PL_DEBUG_ITER, PL_TABULAR, PL_NONE, PL_LOW, PL_MEDIUM, PL_HIGH
+
+  if (!is_solver_initialized_)
+  {
+    solver_ = qpOASES::SQProblem(kNumOfOffsetRows, kNumOfOffsetRows);
+    auto ret = solver_.init(h_matrix, g_matrix, a_constraint_matirx, lower_bound, upper_bound, lower_bound, upper_bound,
+                            max_iter);
+    if (ret != qpOASES::SUCCESSFUL_RETURN)
+    {
+      std::cerr << "[QPOASES] not successfully solved in init()" << std::endl;
+      return false;
+    }
+
+    is_solver_initialized_ = true;
+  }
+  else
+  {
+    auto ret = solver_.hotstart(h_matrix, g_matrix, a_constraint_matirx, lower_bound, upper_bound, lower_bound,
+                                upper_bound, max_iter);
+    if (ret != qpOASES::SUCCESSFUL_RETURN)
+    {
+      std::cerr << "[QPOASES] not successfully solved in hotstart()" << std::endl;
+      return false;
+    }
+  }
+
+  solver_.getPrimalSolution(result);
+
+  for (int i = 0; i < kNumOfOffsetRows; ++i)
+  {
+    U(i) = result[i];
+  }
+
+  return true;
+};

src/ros/catkin/src/mpc_follower/src/qp_solver/qp_solver_unconstr.cpp

@@ -0,0 +1,30 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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 "mpc_follower/qp_solver/qp_solver_unconstr.h"
+
+QPSolverEigenLeastSquare::QPSolverEigenLeastSquare(){};
+bool QPSolverEigenLeastSquare::solve(const Eigen::MatrixXd &Hmat, const Eigen::MatrixXd &fvec, const Eigen::MatrixXd &A,
+                                     const Eigen::VectorXd &lb, const Eigen::VectorXd &ub, const Eigen::MatrixXd &lbA,
+                                     const Eigen::MatrixXd &ubA, Eigen::VectorXd &U)
+{
+     if (std::fabs(Hmat.determinant()) < 1.0E-9)
+          return false;
+
+     U = -Hmat.inverse() * fvec;
+
+     return true;
+};

src/ros/catkin/src/mpc_follower/src/qp_solver/qp_solver_unconstr_fast.cpp

@@ -0,0 +1,30 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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 "mpc_follower/qp_solver/qp_solver_unconstr_fast.h"
+
+QPSolverEigenLeastSquareLLT::QPSolverEigenLeastSquareLLT(){};
+bool QPSolverEigenLeastSquareLLT::solve(const Eigen::MatrixXd &Hmat, const Eigen::MatrixXd &fvec, const Eigen::MatrixXd &A,
+                                        const Eigen::VectorXd &lb, const Eigen::VectorXd &ub, const Eigen::MatrixXd &lbA,
+                                        const Eigen::MatrixXd &ubA, Eigen::VectorXd &U)
+{
+     if (std::fabs(Hmat.determinant()) < 1.0E-9)
+          return false;
+
+     U = -Hmat.llt().solve(fvec);
+
+     return true;
+};

src/ros/catkin/src/mpc_follower/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp

@@ -0,0 +1,88 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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 "mpc_follower/vehicle_model/vehicle_model_bicycle_dynamics.h"
+
+DynamicsBicycleModel::DynamicsBicycleModel(double &wheelbase, double &mass_fl, double &mass_fr,
+                                           double &mass_rl, double &mass_rr, double &cf, double &cr)
+    : VehicleModelInterface(/* dim_x */ 4, /* dim_u */ 1, /* dim_y */ 2)
+{
+    wheelbase_ = wheelbase;
+
+    const double mass_front = mass_fl + mass_fr;
+    const double mass_rear = mass_rl + mass_rr;
+
+    mass_ = mass_front + mass_rear;
+    lf_ = wheelbase_ * (1.0 - mass_front / mass_);
+    lr_ = wheelbase_ * (1.0 - mass_rear / mass_);
+    iz_ = lf_ * lf_ * mass_front + lr_ * lr_ * mass_rear;
+    cf_ = cf;
+    cr_ = cr;
+};
+
+void DynamicsBicycleModel::calculateDiscreteMatrix(Eigen::MatrixXd &Ad,
+                                                   Eigen::MatrixXd &Bd,
+                                                   Eigen::MatrixXd &Cd,
+                                                   Eigen::MatrixXd &Wd,
+                                                   const double &dt)
+{
+    /*
+     * x[k+1] = Ad*x[k] + Bd*u + Wd
+     */
+
+    const double vel = std::max(velocity_, 0.01);
+
+    Ad = Eigen::MatrixXd::Zero(dim_x_, dim_x_);
+    Ad(0, 1) = 1.0;
+    Ad(1, 1) = -(cf_ + cr_) / (mass_ * vel);
+    Ad(1, 2) = (cf_ + cr_) / mass_;
+    Ad(1, 3) = (lr_ * cr_ - lf_ * cf_) / (mass_ * vel);
+    Ad(2, 3) = 1.0;
+    Ad(3, 1) = (lr_ * cr_ - lf_ * cf_) / (iz_ * vel);
+    Ad(3, 2) = (lf_ * cf_ - lr_ * cr_) / iz_;
+    Ad(3, 3) = -(lf_ * lf_ * cf_ + lr_ * lr_ * cr_) / (iz_ * vel);
+
+    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(dim_x_, dim_x_);
+    Eigen::MatrixXd Ad_inverse = (I - dt * 0.5 * Ad).inverse();
+
+    Ad = Ad_inverse * (I + dt * 0.5 * Ad); // bilinear discretization
+
+    Bd = Eigen::MatrixXd::Zero(dim_x_, dim_u_);
+    Bd(0, 0) = 0.0;
+    Bd(1, 0) = cf_ / mass_;
+    Bd(2, 0) = 0.0;
+    Bd(3, 0) = lf_ * cf_ / iz_;
+
+    Wd = Eigen::MatrixXd::Zero(dim_x_, 1);
+    Wd(0, 0) = 0.0;
+    Wd(1, 0) = (lr_ * cr_ - lf_ * cf_) / (mass_ * vel) - vel;
+    Wd(2, 0) = 0.0;
+    Wd(3, 0) = -(lf_ * lf_ * cf_ + lr_ * lr_ * cr_) / (iz_ * vel);
+
+    Bd = (Ad_inverse * dt) * Bd;
+    Wd = (Ad_inverse * dt * curvature_ * vel) * Wd;
+
+    Cd = Eigen::MatrixXd::Zero(dim_y_, dim_x_);
+    Cd(0, 0) = 1.0;
+    Cd(1, 2) = 1.0;
+}
+
+void DynamicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd &Uref)
+{
+    const double vel = std::max(velocity_, 0.01);
+    const double Kv = lr_ * mass_ / (2 * cf_ * wheelbase_) - lf_ * mass_ / (2 * cr_ * wheelbase_);
+    Uref(0, 0) = wheelbase_ * curvature_ + Kv * vel * vel * curvature_;
+}

src/ros/catkin/src/mpc_follower/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp

@@ -0,0 +1,59 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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 "mpc_follower/vehicle_model/vehicle_model_bicycle_kinematics.h"
+
+KinematicsBicycleModel::KinematicsBicycleModel(const double &wheelbase, const double &steer_lim, const double &steer_tau)
+    : VehicleModelInterface(/* dim_x */ 3, /* dim_u */ 1, /* dim_y */ 2)
+{
+    wheelbase_ = wheelbase;
+    steer_lim_ = steer_lim;
+    steer_tau_ = steer_tau;
+};
+
+void KinematicsBicycleModel::calculateDiscreteMatrix(Eigen::MatrixXd &Ad, Eigen::MatrixXd &Bd,
+                                                     Eigen::MatrixXd &Cd, Eigen::MatrixXd &Wd, const double &dt)
+{
+    auto sign = [](double x) { return (x > 0.0) - (x < 0.0); };
+
+    /* Linearize delta around delta_r (referece delta) */
+    double delta_r = atan(wheelbase_ * curvature_);
+    if (abs(delta_r) >= steer_lim_)
+        delta_r = steer_lim_ * (double)sign(delta_r);
+    double cos_delta_r_squared_inv = 1 / (cos(delta_r) * cos(delta_r));
+
+    Ad << 0.0, velocity_, 0.0,
+        0.0, 0.0, velocity_ / wheelbase_ * cos_delta_r_squared_inv,
+        0.0, 0.0, -1.0 / steer_tau_;
+    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(dim_x_, dim_x_);
+    Ad = (I - dt * 0.5 * Ad).inverse() * (I + dt * 0.5 * Ad); // bilinear discretization
+
+    Bd << 0.0, 0.0, 1.0 / steer_tau_;
+    Bd *= dt;
+
+    Cd << 1.0, 0.0, 0.0,
+        0.0, 1.0, 0.0;
+
+    Wd << 0.0,
+        -velocity_ * curvature_ + velocity_ / wheelbase_ * (tan(delta_r) - delta_r * cos_delta_r_squared_inv),
+        0.0;
+    Wd *= dt;
+}
+
+void KinematicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd &Uref)
+{
+    Uref(0, 0) = std::atan(wheelbase_ * curvature_);
+}

src/ros/catkin/src/mpc_follower/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp

@@ -0,0 +1,56 @@
+
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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 "mpc_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.h"
+
+KinematicsBicycleModelNoDelay::KinematicsBicycleModelNoDelay(const double &wheelbase, const double &steer_lim)
+    : VehicleModelInterface(/* dim_x */ 2, /* dim_u */ 1, /* dim_y */ 2)
+{
+    wheelbase_ = wheelbase;
+    steer_lim_ = steer_lim;
+};
+void KinematicsBicycleModelNoDelay::calculateDiscreteMatrix(Eigen::MatrixXd &Ad, Eigen::MatrixXd &Bd,
+                                                     Eigen::MatrixXd &Cd, Eigen::MatrixXd &Wd, const double &dt)
+{
+    auto sign = [](double x) { return (x > 0.0) - (x < 0.0); };
+
+    /* Linearize delta around delta_r (referece delta) */
+    double delta_r = atan(wheelbase_ * curvature_);
+    if (abs(delta_r) >= steer_lim_)
+        delta_r = steer_lim_ * (double)sign(delta_r);
+    double cos_delta_r_squared_inv = 1 / (cos(delta_r) * cos(delta_r));
+
+    Ad << 0.0, velocity_,
+          0.0, 0.0;
+    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(dim_x_, dim_x_);
+    Ad = I + Ad * dt;
+
+    Bd << 0.0, velocity_ / wheelbase_ * cos_delta_r_squared_inv;
+    Bd *= dt;
+
+    Cd << 1.0, 0.0,
+          0.0, 1.0;
+
+    Wd << 0.0,
+        -velocity_ / wheelbase_ * delta_r * cos_delta_r_squared_inv;
+    Wd *= dt;
+}
+
+void KinematicsBicycleModelNoDelay::calculateReferenceInput(Eigen::MatrixXd &Uref)
+{
+    Uref(0, 0) = std::atan(wheelbase_ * curvature_);
+}

src/ros/catkin/src/mpc_follower/src/vehicle_model/vehicle_model_interface.cpp

@@ -0,0 +1,24 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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 "mpc_follower/vehicle_model/vehicle_model_interface.h"
+
+VehicleModelInterface::VehicleModelInterface(int dim_x, int dim_u, int dim_y) : dim_x_(dim_x), dim_u_(dim_u), dim_y_(dim_y) {};
+int VehicleModelInterface::getDimX() { return dim_x_; };
+int VehicleModelInterface::getDimU() { return dim_u_; };
+int VehicleModelInterface::getDimY() { return dim_y_; };
+void VehicleModelInterface::setVelocity(const double &velocity) { velocity_ = velocity; };
+void VehicleModelInterface::setCurvature(const double &curvature) { curvature_ = curvature; };

src/ros/catkin/src/mpc_follower/test/src/test_mpc_follower.cpp

@@ -0,0 +1,327 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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 <ros/ros.h>
+#include <cmath>
+#include <gtest/gtest.h>
+#include <iostream>
+
+#include <autoware_msgs/Lane.h>
+#include <autoware_msgs/VehicleStatus.h>
+#include <autoware_msgs/ControlCommand.h>
+#include <amathutils_lib/amathutils.hpp>
+#include "amathutils_lib/amathutils.hpp"
+#include "mpc_follower/mpc_utils.h"
+#include "mpc_follower/mpc_follower_core.h"
+
+class TestSuite : public ::testing::Test
+{
+public:
+    TestSuite() : nh_(""), pnh_("~")
+    {
+        pub_pose_ = nh_.advertise<geometry_msgs::PoseStamped>("current_pose", 1);
+        pub_vs_ = nh_.advertise<autoware_msgs::VehicleStatus>("vehicle_status", 1);
+        pub_lane_ = nh_.advertise<autoware_msgs::Lane>("base_waypoints", 1);
+        pub_estimate_twist_ = nh_.advertise<geometry_msgs::TwistStamped>("estimate_twist", 1);
+        sub_twist_ = nh_.subscribe("twist_raw", 1, &TestSuite::callbackTwistRaw, this);
+        sub_ctrl_cmd_ = nh_.subscribe("ctrl_raw", 1, &TestSuite::callbackCtrlCmd, this);
+        spin_duration_ = 0.05;
+        spin_loopnum_ = 10;
+    }
+    ~TestSuite() {}
+
+    ros::NodeHandle nh_, pnh_;
+    ros::Subscriber sub_twist_, sub_ctrl_cmd_;
+    ros::Publisher pub_pose_, pub_vs_, pub_lane_, pub_estimate_twist_;
+    geometry_msgs::TwistStamped twist_raw_;
+    autoware_msgs::ControlCommandStamped ctrl_cmd_;
+    double spin_duration_;
+    int spin_loopnum_;
+
+    void callbackTwistRaw(const geometry_msgs::TwistStamped &twist)
+    {
+        twist_raw_ = twist;
+    }
+    void callbackCtrlCmd(const autoware_msgs::ControlCommandStamped &cmd)
+    {
+        ctrl_cmd_ = cmd;
+    }
+
+    void publishEstimateTwist()
+    {
+        geometry_msgs::TwistStamped twist;
+        twist.header.frame_id = "base_link";
+        for (int i = 0; i < spin_loopnum_; ++i)
+        {
+            twist.header.stamp = ros::Time::now();
+            pub_estimate_twist_.publish(twist);
+            ros::spinOnce();
+            ros::Duration(spin_duration_).sleep();
+        }
+    }
+
+    void publishMsgs(geometry_msgs::PoseStamped &pose, autoware_msgs::VehicleStatus &vs, double wp_vx,
+                     double wp_wz, double wp_dt, double wp_x_ini, double wp_y_ini, double wp_yaw_ini)
+    {
+        double x = wp_x_ini;
+        double y = wp_y_ini;
+        double yaw = wp_yaw_ini;
+        autoware_msgs::Lane lane;
+        autoware_msgs::Waypoint wp;
+        for (int i = 0; i < 50; ++i)
+        {
+            wp.pose.pose.position.x = x;
+            wp.pose.pose.position.y = y;
+            wp.pose.pose.orientation = amathutils::getQuaternionFromYaw(yaw);
+            wp.twist.twist.linear.x = wp_vx;
+            wp.twist.twist.angular.z = wp_wz;
+            lane.waypoints.push_back(wp);
+            x += wp_vx * std::cos(yaw) * wp_dt;
+            y += wp_vx * std::sin(yaw) * wp_dt;
+            yaw += wp_wz * wp_dt;
+        }
+
+        for (int i = 0; i < spin_loopnum_; ++i)
+        {
+            ros::Time current_time = ros::Time::now();
+            pose.header.stamp = current_time;
+            vs.header.stamp = current_time;
+            lane.header.stamp = current_time;
+            pub_pose_.publish(pose);
+            pub_vs_.publish(vs);
+            pub_lane_.publish(lane);
+            ros::spinOnce();
+            ros::Duration(spin_duration_).sleep();
+        }
+    }
+
+    void testTurningLeft()
+    {
+        MPCFollower mpc_follower;
+
+        geometry_msgs::PoseStamped current_pose;
+        current_pose.pose.position.y = 1.0; // vehicle position is on the RIGHT side of the path -> turning LEFT
+        current_pose.pose.orientation.w = 1.0;
+
+        autoware_msgs::VehicleStatus vs;
+        vs.speed = 0.0;
+        vs.angle = 0.0;
+
+        // autoware_msgs::Lane lane;
+        const double vx = 1.0;
+        const double wz = 0.1;
+        const double dt = 1.0;
+        double x(0.0), y(0.0), yaw(0.0);
+
+        publishMsgs(current_pose, vs, vx, wz, dt, x, y, yaw);
+
+        ASSERT_EQ(true, twist_raw_.twist.linear.x > 0.1) << "going forward";
+        ASSERT_EQ(true, ctrl_cmd_.cmd.linear_velocity > 0.1) << "going forward";
+        ASSERT_LT(ctrl_cmd_.cmd.steering_angle, 0.0) << "vehicle is turning left, negative steering is expected";
+    }
+
+    void testTurningRight()
+    {
+        MPCFollower mpc_follower;
+
+        geometry_msgs::PoseStamped current_pose;
+        current_pose.pose.position.y = -1.0; // vehicle position is on the LEFT side of the path -> turning RIGHT
+        current_pose.pose.orientation.w = 1.0;
+
+        autoware_msgs::VehicleStatus vs;
+        vs.speed = 0.0;
+        vs.angle = 0.0;
+
+        // autoware_msgs::Lane lane;
+        const double vx = 1.0;
+        const double wz = -0.1;
+        const double dt = 1.0;
+        double x(0.0), y(0.0), yaw(0.0);
+
+        publishMsgs(current_pose, vs, vx, wz, dt, x, y, yaw);
+
+        ASSERT_GT(twist_raw_.twist.linear.x, 0.1) << "going forward";
+        ASSERT_GT(ctrl_cmd_.cmd.linear_velocity, 0.1) << "going forward";
+        ASSERT_GT(ctrl_cmd_.cmd.steering_angle, 0.0) << "vehicle is turning right, positive steering is expected";
+    }
+};
+
+
+TEST_F(TestSuite, TestMPCFollower)
+{
+    /* TestMPCFollowerInvalidPath */
+    {
+        MPCFollower mpc_follower;
+
+        publishEstimateTwist();
+
+        geometry_msgs::PoseStamped current_pose;
+        current_pose.pose.position.y = 1.0;
+        current_pose.pose.orientation.w = 1.0;
+
+        autoware_msgs::VehicleStatus vs;
+        vs.speed = 0.0;
+        vs.angle = 0.0;
+
+        // autoware_msgs::Lane lane;
+        const double vx = 1.0;
+        const double wz = 0.1;
+        const double dt = 1.0;
+        double x(0.0), y(0.0), yaw(0.0);
+
+        // first publish valid values
+        publishMsgs(current_pose, vs, vx, wz, dt, x, y, yaw);
+
+        // then, publish invalid path
+        autoware_msgs::Lane empty_lane;
+        for (int i = 0; i < spin_loopnum_; ++i)
+        {
+            pub_lane_.publish(empty_lane);
+            ros::spinOnce();
+            ros::Duration(spin_duration_).sleep();
+        }
+
+        ASSERT_TRUE(std::isfinite(twist_raw_.twist.linear.x)) << "expected keepping old path and publish finite values";
+        ASSERT_TRUE(std::isfinite(twist_raw_.twist.angular.z)) << "expected keepping old path and publish finite values";
+        ASSERT_TRUE(std::isfinite(ctrl_cmd_.cmd.linear_velocity)) << "expected keepping old path and publish finite values";
+        ASSERT_TRUE(std::isfinite(ctrl_cmd_.cmd.steering_angle)) << "expected keepping old path and publish finite values";
+    }
+
+
+    /*  == TestMPCFollowerInvalidPose == */
+    {
+        MPCFollower mpc_follower;
+
+        geometry_msgs::PoseStamped current_pose;
+        current_pose.pose.position.y = 1.0; // first publish valid value
+        current_pose.pose.orientation.w = 1.0;
+
+        autoware_msgs::VehicleStatus vs;
+        vs.speed = 0.0;
+        vs.speed = 0.0;
+        vs.speed = 0.0;
+        vs.angle = 0.0;
+
+        // autoware_msgs::Lane lane;
+        const double vx = 1.0;
+        const double wz = 0.1;
+        const double dt = 1.0;
+        double x(0.0), y(0.0), yaw(0.0);
+
+        // first publish valid values
+        twist_raw_.twist.linear.x = -9.99;
+        publishMsgs(current_pose, vs, vx, wz, dt, x, y, yaw);
+
+        ASSERT_GT(twist_raw_.twist.linear.x, 0.1);
+
+
+        // then publish invalid pose
+        current_pose.pose.position.y = NAN;
+        vs.speed = 0.0;
+        publishMsgs(current_pose, vs, vx, wz, dt, x, y, yaw);
+        ASSERT_DOUBLE_EQ(0.0, twist_raw_.twist.linear.x) << "emergency stop, zero speed command is expected";
+        ASSERT_DOUBLE_EQ(0.0, ctrl_cmd_.cmd.linear_velocity) << "emergency stop, zero speed command is expected";
+    }
+
+    /* == TestMPCFollowerInvalidVehicleStatus == */
+    {
+        pnh_.setParam("vehicle_model_type", "kinematics"); // set as default. kinematics_no_delay & dynamics does not use vehicle status.
+        pnh_.setParam("qp_solver_type", "unconstraint_fast");
+
+        MPCFollower mpc_follower;
+
+        geometry_msgs::PoseStamped current_pose;
+        current_pose.pose.position.y = 1.0;
+        current_pose.pose.position.y = 1.0;
+        current_pose.pose.position.y = 1.0;
+        current_pose.pose.orientation.w = 1.0;
+
+        autoware_msgs::VehicleStatus vs;
+        vs.speed = 0.0;
+        vs.speed = 0.0;
+        vs.speed = 0.0;
+        vs.angle = 0.0; // first publish valid value
+
+        // autoware_msgs::Lane lane;
+        const double vx = 3.28;
+        const double wz = 0.1;
+        const double dt = 1.0;
+        double x(0.0), y(0.0), yaw(0.0);
+
+        // first publish valid values
+        publishMsgs(current_pose, vs, vx, wz, dt, x, y, yaw);
+        ASSERT_GT(twist_raw_.twist.linear.x, 0.1);
+
+        // then publish invalid vehicle status
+        current_pose.pose.position.y = 1.0;
+        vs.angle = NAN;
+        publishMsgs(current_pose, vs, vx, wz, dt, x, y, yaw);
+        ASSERT_DOUBLE_EQ(0.0, twist_raw_.twist.linear.x) << "emergency stop, zero speed command is expected";
+        ASSERT_DOUBLE_EQ(0.0, ctrl_cmd_.cmd.linear_velocity) << "emergency stop, zero speed command is expected";
+    }
+
+    /* == TestMPCFollowerNoMessageCase == */
+    {
+        MPCFollower mpc_follower;
+        for (int i = 0; i < spin_loopnum_; ++i)
+        {
+            ros::spinOnce();
+            ros::Duration(spin_duration_).sleep();
+        }
+        ASSERT_DOUBLE_EQ(0.0, twist_raw_.twist.linear.x) << "no messages published, zero speed command is expected";
+        ASSERT_DOUBLE_EQ(0.0, twist_raw_.twist.angular.z) << "no messages published, zero speed command is expected";
+        ASSERT_DOUBLE_EQ(0.0, ctrl_cmd_.cmd.linear_velocity) << "no messages published, zero speed command is expected";
+        ASSERT_DOUBLE_EQ(0.0, ctrl_cmd_.cmd.steering_angle) << "no messages published, zero speed command is expected";
+    }
+
+    /* == TestMPCFollowerAlgorithmOptions == */
+    {
+        std::string vehicle_mode_type_array[] = {"kinematics", "kinematics_no_delay", "dynamics"};
+        std::string qp_solver_type_array[] = {"unconstraint", "unconstraint_fast", "qpoases_hotstart"};
+
+        for (const auto vehicle_model_type : vehicle_mode_type_array)
+        {
+            for (const auto qp_solver_type : qp_solver_type_array)
+            {
+                // ROS_ERROR("%s, %s", vehicle_model_type.c_str(), qp_solver_type.c_str());
+                pnh_.setParam("vehicle_model_type", vehicle_model_type);
+                pnh_.setParam("qp_solver_type", qp_solver_type);
+                testTurningRight();
+                testTurningLeft();
+            }
+        }
+    }
+
+    /* == TestMPCFollowerDebugOptions == */
+    {
+        pnh_.setParam("show_debug_info", true);
+        testTurningRight();
+
+        pnh_.setParam("show_debug_info", false);
+        testTurningRight();
+    }
+
+
+}
+
+
+
+int main(int argc, char **argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    ros::init(argc, argv, "TestNode");
+    return RUN_ALL_TESTS();
+}

src/ros/catkin/src/mpc_follower/test/src/test_mpc_lowpass_filter.cpp

@@ -0,0 +1,73 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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 <ros/ros.h>
+#include <cmath>
+#include <gtest/gtest.h>
+#include <iostream>
+
+#include "mpc_follower/lowpass_filter.h"
+
+class TestSuite : public ::testing::Test
+{
+public:
+    TestSuite() {}
+    ~TestSuite() {}
+};
+
+TEST_F(TestSuite, TestButterworthFilter)
+{
+    double dt = 0.01; // sampling time [s]
+    double fc = 5.0; // cutoff [hz]
+    Butterworth2dFilter butter(dt, fc);
+
+    std::vector<double> coeffs;
+    butter.getCoefficients(coeffs);
+
+    const double a0_tmp = coeffs.at(0);
+    ASSERT_GT(std::fabs(a0_tmp), 1.0E-5) << "non-zero value is expected";
+
+    // const double a0 = 1.0;
+    const double a1 = coeffs.at(1) / a0_tmp;
+    const double a2 = coeffs.at(2) / a0_tmp;
+    const double b0 = coeffs.at(3) / a0_tmp;
+    const double b1 = coeffs.at(4) / a0_tmp;
+    const double b2 = coeffs.at(5) / a0_tmp;
+
+    // calculated by butter(2, f_cutoff_hz/(1/dt/2))
+    // const double a0_matlab = 1.0;
+    const double a1_matlab = -1.5610;
+    const double a2_matlab = 0.6414;
+    const double b0_matlab = 0.0201;
+    const double b1_matlab = 0.0402;
+    const double b2_matlab = 0.0201;
+
+    // these values does not match perfectly due to discretization algorithm.
+    ASSERT_NEAR(a1, a1_matlab, std::fabs(a1_matlab * 0.1)/* error limit*/) << "a1 value differs more than 10% with matlab, check equation";
+    ASSERT_NEAR(a2, a2_matlab, std::fabs(a2_matlab * 0.1)/* error limit*/) << "a2 value differs more than 10% with matlab, check equation";
+    ASSERT_NEAR(b0, b0_matlab, std::fabs(b0_matlab * 0.1)/* error limit*/) << "b0 value differs more than 10% with matlab, check equation";
+    ASSERT_NEAR(b1, b1_matlab, std::fabs(b1_matlab * 0.1)/* error limit*/) << "b1 value differs more than 10% with matlab, check equation";
+    ASSERT_NEAR(b2, b2_matlab, std::fabs(b2_matlab * 0.1)/* error limit*/) << "b2 value differs more than 10% with matlab, check equation";
+}
+
+
+
+int main(int argc, char **argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    ros::init(argc, argv, "TestNode");
+    return RUN_ALL_TESTS();
+}

src/ros/catkin/src/mpc_follower/test/src/test_mpc_utils.cpp

@@ -0,0 +1,292 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * 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 <ros/ros.h>
+#include <cmath>
+#include <gtest/gtest.h>
+#include <amathutils_lib/amathutils.hpp>
+#include "mpc_follower/mpc_utils.h"
+
+class TestSuite: public ::testing::Test {
+public:
+  TestSuite(){}
+  ~TestSuite(){}
+};
+
+
+TEST(TestSuite, TestConvertEulerAngleToMonotonic){
+    std::vector<double> yaw;
+    for (int i = -5; i < 5; ++i) {
+        double tmp = amathutils::normalizeRadian((double)i * M_PI);
+        yaw.push_back(tmp);
+    }
+    ASSERT_DOUBLE_EQ(-M_PI, yaw.front());
+    ASSERT_DOUBLE_EQ(0.0, yaw.back());
+    double diff = yaw.back() - yaw.front();
+
+    MPCUtils::convertEulerAngleToMonotonic(yaw);
+    ASSERT_DOUBLE_EQ(-M_PI, yaw.front());
+    ASSERT_DOUBLE_EQ(-M_PI + diff, yaw.back());
+}
+
+
+
+TEST(TestSuite, InterpolationTest){
+
+    std::vector<double> idx = {0.0, 1.0, 2.0, 3.0};
+    
+    std::vector<double> value = {-2.0, 0.0, 2.0, 4.0};
+    double ref = 1.0;
+    double ret = 0.0;
+    
+    std::vector<double> idx_bad = {0.0, 1.0, 0.0, 3.0};
+    bool res = MPCUtils::interp1d<std::vector<double>, std::vector<double>>(idx_bad, value, ref, ret);
+    ASSERT_EQ(false, res);
+
+    ref = -10.0;
+    MPCUtils::interp1d<std::vector<double>, std::vector<double>>(idx, value, ref, ret);
+    ASSERT_DOUBLE_EQ(-2.0, ret);
+
+    ref = 10.0;
+    MPCUtils::interp1d<std::vector<double>, std::vector<double>>(idx, value, ref, ret);
+    ASSERT_DOUBLE_EQ(4.0, ret);
+
+    ref = 0.3;
+    MPCUtils::interp1d<std::vector<double>, std::vector<double>>(idx, value, ref, ret);
+    ASSERT_DOUBLE_EQ(-1.4, ret);
+}
+
+
+TEST(TestSuite, TestCalcTrajectoryYawFromXY) {
+
+    MPCTrajectory traj;
+    /*              x    y    z   yaw   vx   k  time */
+    traj.push_back(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+    traj.push_back(1.0, 1.0, 0.0, 0.0, 1.0, 0.0, 1.0);
+    traj.push_back(2.0, 2.0, 0.0, 0.0, 1.0, 0.0, 2.0);
+    MPCUtils::calcTrajectoryYawFromXY(traj);
+    ASSERT_DOUBLE_EQ(M_PI / 4.0, traj.yaw[0]);
+    ASSERT_DOUBLE_EQ(M_PI / 4.0, traj.yaw[1]);
+    ASSERT_DOUBLE_EQ(M_PI / 4.0, traj.yaw[2]);
+
+    traj.clear();
+    /*              x    y    z   yaw   vx   k  time */
+    traj.push_back(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+    traj.push_back(0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 1.0);
+    traj.push_back(0.0, 2.0, 0.0, 0.0, 1.0, 0.0, 2.0);
+    MPCUtils::calcTrajectoryYawFromXY(traj);
+    ASSERT_DOUBLE_EQ(M_PI / 2.0, traj.yaw[0]);
+    ASSERT_DOUBLE_EQ(M_PI / 2.0, traj.yaw[1]);
+    ASSERT_DOUBLE_EQ(M_PI / 2.0, traj.yaw[2]);
+
+    traj.clear();
+    /*              x    y    z   yaw   vx   k  time */
+    traj.push_back(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+    traj.push_back(0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0);
+    traj.push_back(0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 2.0);
+    MPCUtils::calcTrajectoryYawFromXY(traj);
+    ASSERT_DOUBLE_EQ(0.0, traj.yaw[0]);
+    ASSERT_DOUBLE_EQ(0.0, traj.yaw[1]);
+    ASSERT_DOUBLE_EQ(0.0, traj.yaw[2]);
+}
+
+TEST(TestSuite, TestCalcTrajectoryCurvature)
+{
+    MPCTrajectory traj;
+    double radius = 0.5;
+    for (double theta = 0; theta < M_PI; theta += 0.1)
+    {
+        double x = radius * std::cos(theta);
+        double y = radius * std::sin(theta);
+        traj.push_back(x, y, 0, 0, 0, 0, theta);
+    }
+    MPCUtils::calcTrajectoryCurvature(traj, 1);
+    ASSERT_NEAR(1 / radius, traj.k[0], 1.0E-5);
+    ASSERT_NEAR(1 / radius, traj.k[5], 1.0E-5);
+    ASSERT_NEAR(1 / radius, traj.k.back(), 1.0E-5);
+
+    MPCUtils::calcTrajectoryCurvature(traj, 3);
+    ASSERT_NEAR(1 / radius, traj.k[0], 1.0E-5);
+    ASSERT_NEAR(1 / radius, traj.k[5], 1.0E-5);
+    ASSERT_NEAR(1 / radius, traj.k.back(), 1.0E-5);
+
+    traj.clear();
+    radius = 10.0;
+    for (double theta = 0; theta < M_PI; theta += 0.1)
+    {
+        double x = radius * std::cos(theta);
+        double y = radius * std::sin(theta);
+        traj.push_back(x, y, 0, 0, 0, 0, theta);
+    }
+
+    MPCUtils::calcTrajectoryCurvature(traj, 1);
+    ASSERT_NEAR(1 / radius, traj.k[0], 1.0E-5);
+    ASSERT_NEAR(1 / radius, traj.k[5], 1.0E-5);
+    ASSERT_NEAR(1 / radius, traj.k.back(), 1.0E-5);
+
+    MPCUtils::calcTrajectoryCurvature(traj, 3);
+    ASSERT_NEAR(1 / radius, traj.k[0], 1.0E-5);
+    ASSERT_NEAR(1 / radius, traj.k[5], 1.0E-5);
+    ASSERT_NEAR(1 / radius, traj.k.back(), 1.0E-5);
+}
+
+TEST(TestSuite, TestCalcNearestPose){
+
+    MPCTrajectory traj;
+    /*              x    y    z     yaw        vx   k  time */
+    traj.push_back(0.0, 0.0, 0.0, M_PI / 4.0, 0.0, 0.0, 0.0);
+    traj.push_back(1.0, 1.0, 0.0, M_PI / 4.0, 1.0, 0.0, 1.0);
+    traj.push_back(2.0, 2.0, 0.0, M_PI / 4.0, 1.0, 0.0, 2.0);
+
+    geometry_msgs::Pose self_pose;
+    geometry_msgs::Pose nearest_pose;
+    unsigned int nearest_index;
+    double min_dist_error, nearest_yaw_error, nearest_time;
+
+    self_pose.position.x = 0.3;
+    self_pose.position.y = 0.3;
+    self_pose.position.z = 0.0;
+    self_pose.orientation = amathutils::getQuaternionFromYaw(M_PI / 3.0);
+    MPCUtils::calcNearestPose(traj, self_pose, nearest_pose, nearest_index, min_dist_error, nearest_yaw_error, nearest_time);
+    ASSERT_EQ(0, nearest_index);
+    ASSERT_DOUBLE_EQ(std::sqrt(0.3 * 0.3 + 0.3 * 0.3), min_dist_error);
+    ASSERT_DOUBLE_EQ(M_PI / 3.0 - M_PI / 4.0, nearest_yaw_error);
+    ASSERT_DOUBLE_EQ(0.0, nearest_time);
+
+    self_pose.position.x = 0.0;
+    self_pose.position.y = 0.0;
+    self_pose.position.z = 0.1;
+    self_pose.orientation = amathutils::getQuaternionFromYaw(M_PI / 4.0);
+    MPCUtils::calcNearestPose(traj, self_pose, nearest_pose, nearest_index, min_dist_error, nearest_yaw_error, nearest_time);
+    ASSERT_EQ(0, nearest_index);
+    ASSERT_DOUBLE_EQ(0.0, min_dist_error);
+    ASSERT_EQ(true, std::fabs(nearest_yaw_error) < 1.0E-5);
+    ASSERT_DOUBLE_EQ(0.0, nearest_time);
+
+
+    self_pose.position.x = 0.3;
+    self_pose.position.y = 0.3;
+    self_pose.position.z = 0.0;
+    self_pose.orientation = amathutils::getQuaternionFromYaw(M_PI / 4.0);
+    MPCUtils::calcNearestPoseInterp(traj, self_pose, nearest_pose, nearest_index, min_dist_error, nearest_yaw_error, nearest_time);
+    ASSERT_EQ(0, nearest_index);
+    ASSERT_DOUBLE_EQ(0.0, min_dist_error);
+    ASSERT_EQ(true, std::fabs(nearest_yaw_error) < 1.0E-5);
+    ASSERT_EQ(true, std::fabs(nearest_time - 0.3) < 1.0E-5);
+
+    self_pose.position.x = 0.3;
+    self_pose.position.y = 0.3;
+    self_pose.position.z = 0.0;
+    self_pose.orientation = amathutils::getQuaternionFromYaw(M_PI / 4.0);
+    MPCUtils::calcNearestPoseInterp(traj, self_pose, nearest_pose, nearest_index, min_dist_error, nearest_yaw_error, nearest_time);
+    ASSERT_EQ(0, nearest_index);
+    ASSERT_DOUBLE_EQ(0.0, min_dist_error) << "min_dist_error = " << min_dist_error;
+    ASSERT_EQ(true, std::fabs(nearest_yaw_error) < 1.0E-5) << "nearest_yaw_error = " << nearest_yaw_error;
+    ASSERT_EQ(true, std::fabs(nearest_time - 0.3) < 1.0E-5) << "nearest_time = " << nearest_time;
+
+    self_pose.position.x = -1.0;
+    self_pose.position.y = 0.0;
+    self_pose.position.z = 0.0;
+    self_pose.orientation = amathutils::getQuaternionFromYaw(M_PI / 4.0);
+    MPCUtils::calcNearestPoseInterp(traj, self_pose, nearest_pose, nearest_index, min_dist_error, nearest_yaw_error, nearest_time);
+    ASSERT_EQ(0, nearest_index);
+    ASSERT_EQ(true, std::fabs(min_dist_error - sqrt(2.0)/2.0) < 1.0E-5) << "min_dist_error = " << min_dist_error;
+    ASSERT_EQ(true, std::fabs(nearest_yaw_error) < 1.0E-5) << "nearest_yaw_error = " << nearest_yaw_error;
+    ASSERT_EQ(true, std::fabs(nearest_time - (-0.5)) < 1.0E-5) << "nearest_time = " << nearest_time;
+
+}
+
+TEST(TestSuite, TestInterp1dMPCTraj){
+
+    MPCTrajectory traj, traj_result;
+    /*              x    y    z    yaw   vx   k  time */
+    traj.push_back(0.0, 0.0, 0.0,  0.2, 0.0, 0.0, 0.0);
+    traj.push_back(1.0, 2.0, 0.0,  0.5, 1.0, 0.0, 1.0);
+    traj.push_back(2.0, 3.0, 0.0, -0.2, 1.0, 0.0, 2.0);
+
+    /*                               0     1    2    3    4    5    6  */
+    std::vector<double> index_time{-0.1, 0.0, 0.7, 1.0, 1.5, 2.0, 2.2};
+    bool res = MPCUtils::interp1dMPCTraj(traj.relative_time, traj, index_time, traj_result);
+    ASSERT_EQ(true, res);
+    ASSERT_DOUBLE_EQ(index_time[0], traj_result.relative_time[0]);
+    ASSERT_DOUBLE_EQ(traj.x[0], traj_result.x[0]);
+    ASSERT_DOUBLE_EQ(traj.y[0], traj_result.y[0]);
+    ASSERT_DOUBLE_EQ(traj.z[0], traj_result.z[0]);
+    ASSERT_DOUBLE_EQ(traj.yaw[0], traj_result.yaw[0]);
+    ASSERT_DOUBLE_EQ(traj.k[0], traj_result.k[0]);
+    ASSERT_DOUBLE_EQ(traj.vx[0], traj_result.vx[0]);
+
+    ASSERT_DOUBLE_EQ(0.0, traj_result.relative_time[1]);
+    ASSERT_DOUBLE_EQ(traj.x[0], traj_result.x[1]);
+    ASSERT_DOUBLE_EQ(traj.y[0], traj_result.y[1]);
+    ASSERT_DOUBLE_EQ(traj.z[0], traj_result.z[1]);
+    ASSERT_DOUBLE_EQ(traj.yaw[0], traj_result.yaw[1]);
+    ASSERT_DOUBLE_EQ(traj.k[0], traj_result.k[1]);
+    ASSERT_DOUBLE_EQ(traj.vx[0], traj_result.vx[1]);
+
+    ASSERT_DOUBLE_EQ(0.7, traj_result.relative_time[2]);
+    ASSERT_DOUBLE_EQ(0.7, traj_result.x[2]);
+    ASSERT_DOUBLE_EQ(1.4, traj_result.y[2]);
+    ASSERT_DOUBLE_EQ(0.0, traj_result.z[2]);
+    ASSERT_DOUBLE_EQ(0.5*0.7+0.2*0.3, traj_result.yaw[2]);
+    ASSERT_DOUBLE_EQ(0.0, traj_result.k[2]);
+    ASSERT_DOUBLE_EQ(0.7, traj_result.vx[2]);
+
+    ASSERT_DOUBLE_EQ(index_time[5], traj_result.relative_time[5]);
+    ASSERT_DOUBLE_EQ(traj.x[2], traj_result.x[5]);
+    ASSERT_DOUBLE_EQ(traj.y[2], traj_result.y[5]);
+    ASSERT_DOUBLE_EQ(traj.z[2], traj_result.z[5]);
+    ASSERT_DOUBLE_EQ(traj.yaw[2], traj_result.yaw[5]);
+    ASSERT_DOUBLE_EQ(traj.k[2], traj_result.k[5]);
+    ASSERT_DOUBLE_EQ(traj.vx[2], traj_result.vx[5]);
+
+    ASSERT_DOUBLE_EQ(index_time[6], traj_result.relative_time[6]);
+    ASSERT_DOUBLE_EQ(traj.x[2], traj_result.x[6]);
+    ASSERT_DOUBLE_EQ(traj.y[2], traj_result.y[6]);
+    ASSERT_DOUBLE_EQ(traj.z[2], traj_result.z[6]);
+    ASSERT_DOUBLE_EQ(traj.yaw[2], traj_result.yaw[6]);
+    ASSERT_DOUBLE_EQ(traj.k[2], traj_result.k[6]);
+    ASSERT_DOUBLE_EQ(traj.vx[2], traj_result.vx[6]);
+
+    MPCTrajectory bad_traj;
+    /*                 x    y    z    yaw   k    vx  time */
+    bad_traj.push_back(0.0, 0.0, 0.0,  0.2, 0.0, 0.0, 0.0);
+    bad_traj.push_back(1.0, 2.0, 0.0,  0.5, 1.0, 0.0, -1.0);
+    bad_traj.push_back(2.0, 3.0, 0.0, -0.2, 1.0, 0.0, 2.0);
+
+    /*                                      0     1    2    3    4    5    6  */
+    std::vector<double> bad_index_time = {-0.1, 0.0, -0.7, 1.0, 1,5, 2.0, 2.2};
+    ASSERT_EQ(false, MPCUtils::interp1dMPCTraj(bad_traj.relative_time, traj, index_time, traj_result));
+    ASSERT_EQ(false, MPCUtils::interp1dMPCTraj(traj.relative_time, traj, bad_index_time, traj_result));
+    ASSERT_EQ(false, MPCUtils::interp1dMPCTraj(bad_traj.relative_time, traj, bad_index_time, traj_result));
+}
+
+TEST(TestSuite, TestMPCTrajSize)
+{
+    MPCTrajectory traj;
+    traj.push_back(0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0);
+    ASSERT_EQ(1, traj.size()) << "invalid size, zero expected";
+
+    traj.z.push_back(0.0);
+    ASSERT_EQ(0, traj.size()) << "invalid size, zero expected";
+}
+
+int main(int argc, char **argv) {
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "TestNode");
+  return RUN_ALL_TESTS();
+}

src/ros/catkin/src/mpc_follower/test/test_mpc_follower.test

@@ -0,0 +1,5 @@
+<launch>
+
+  <test test-name="test-mpc_follower" pkg="mpc_follower" type="test-mpc_follower" name="test"/>
+
+</launch>

src/ros/catkin/src/mpc_follower/test/test_mpc_lowpass_filter.test

@@ -0,0 +1,5 @@
+<launch>
+
+  <test test-name="test-mpc_lowpass_filter" pkg="mpc_follower" type="test-mpc_lowpass_filter" name="test"/>
+
+</launch>

src/ros/catkin/src/mpc_follower/test/test_mpc_utils.test

@@ -0,0 +1,5 @@
+<launch>
+
+  <test test-name="test-mpc_utils" pkg="mpc_follower" type="test-mpc_utils" name="test"/>
+
+</launch>

src/ros/catkin/src/pilottoros/src/main.cpp

@@ -43,6 +43,9 @@ static std::string _waypoints_msgname = "waypoints";
 static std::string _pose_topic = "/current_pose";
 static std::string _twist_topic =  "/current_velocity";
 
+static bool _use_rtk_odom = true; //if use rtk , use_rtk_odom is true;
+static bool  _use_pilot_waypoints = true; //if use pilot to calculate waypoints, this is true
+
 ros::Publisher  point_cloud_pub;
 image_transport::Publisher  image_pub;
 ros::Publisher odom_pub;
@@ -167,11 +170,17 @@ void Listenodom(const char * strdata,const unsigned int nSize,const unsigned int
     geometry_msgs::TwistStamped xtwist;
     xtwist.twist = msg.twist.twist;
 
+if(_use_pilot_waypoints)
+{
     pose_pub.publish(xPose);
     twist_pub.publish(xtwist);
+}
 
 
+if(_use_rtk_odom)
+{
   odom_pub.publish(msg);
+}
 
 }
 
@@ -267,7 +276,10 @@ void ListenWayPointsMap(const char * strdata,const unsigned int nSize,const unsi
 
     }
 
-    waypoints_pub.publish(xlane);
+    if(_use_pilot_waypoints)
+    {
+        waypoints_pub.publish(xlane);
+    }
 
 
 /*
@@ -323,12 +335,18 @@ int main(int argc, char *argv[])
     std::cout<<"_waypoints_topic is "<<_waypoints_topic<<std::endl;
 	std::cout<<"_waypoints_msgname : "<<_waypoints_msgname<<std::endl;
 
+    private_nh.getParam("use_rtk_odom",_use_rtk_odom);
+    private_nh.getParam("use_pilot_waypoints",_use_pilot_waypoints);
+
     point_cloud_pub = private_nh.advertise<sensor_msgs::PointCloud2>(
                 _point_topic, 10);
 
-    waypoints_pub = private_nh.advertise<autoware_msgs::Lane>(_waypoints_topic,10);
-    pose_pub = private_nh.advertise<geometry_msgs::PoseStamped>(_pose_topic,10);
-    twist_pub = private_nh.advertise<geometry_msgs::TwistStamped>(_twist_topic,10);
+    if(_use_pilot_waypoints)
+    {
+        waypoints_pub = private_nh.advertise<autoware_msgs::Lane>(_waypoints_topic,10);
+        pose_pub = private_nh.advertise<geometry_msgs::PoseStamped>(_pose_topic,10);
+        twist_pub = private_nh.advertise<geometry_msgs::TwistStamped>(_twist_topic,10);
+    }
 	
 
 
@@ -336,7 +354,11 @@ int main(int argc, char *argv[])
     image_transport::ImageTransport it(private_nh);
     image_pub  = it.advertise("image_raw", 1);
 
-    odom_pub =  private_nh.advertise<nav_msgs::Odometry>(_odom_topic,10);
+   if(_use_rtk_odom)
+   {
+        odom_pub =  private_nh.advertise<nav_msgs::Odometry>(_odom_topic,10);
+        std::cout<<" use rtk odom "<<std::endl;
+   }
 
     void * pimage = iv::modulecomm::RegisterRecv(_image_msgname.data(),Listenpic);
     void * pliar = iv::modulecomm::RegisterRecv(_point_msgname.data(),ListenPointCloud);

src/ros/catkin/src/pure_pursuit/src/pure_pursuit.cpp

@@ -40,6 +40,9 @@ double PurePursuit::calcCurvature(geometry_msgs::Point target) const
   double kappa;
   double denominator = pow(getPlaneDistance(target, current_pose_.position), 2);
   double numerator = 2 * calcRelativeCoordinate(target, current_pose_).y;
+  //ROS_WARN(" T:%lf %lf %lf %lf  C:%lf %lf %lf %lf  ",target.)
+
+  //ROS_WARN(" dis : %lf  y:%lf",denominator,numerator);
 
   if (denominator != 0)
   {
@@ -64,6 +67,7 @@ double PurePursuit::calcCurvature(geometry_msgs::Point target) const
 bool PurePursuit::interpolateNextTarget(
   int next_waypoint, geometry_msgs::Point* next_target) const
 {
+  
   constexpr double ERROR = pow(10, -5);  // 0.00001
 
   int path_size = static_cast<int>(current_waypoints_.size());
@@ -102,6 +106,7 @@ bool PurePursuit::interpolateNextTarget(
   // ROS_INFO("b : %lf ", b);
   // ROS_INFO("c : %lf ", c);
   // ROS_INFO("distance : %lf ", d);
+  //ROS_WARN(" distance   %lf",d);
 
   if (d > search_radius)
     return false;
@@ -176,6 +181,7 @@ bool PurePursuit::interpolateNextTarget(
 
     // check intersection is between end and start
     double interval = getPlaneDistance(end, start);
+    
     if (getPlaneDistance(target1, end) < interval)
     {
       // ROS_INFO("result : target1");
@@ -223,11 +229,13 @@ void PurePursuit::getNextWaypoint()
       current_waypoints_.at(i).pose.pose.position, current_pose_.position)
       > lookahead_distance_)
     {
+      //ROS_WARN("next_waypoint_number_: %d %lf ",next_waypoint_number_,lookahead_distance_);
       next_waypoint_number_ = i;
       return;
     }
   }
 
+  
   // if this program reaches here , it means we lost the waypoint!
   next_waypoint_number_ = -1;
   return;
@@ -272,6 +280,7 @@ bool PurePursuit::canGetCurvature(double* output_kappa)
     return true;
   }
 
+
   // linear interpolation and calculate angular velocity
   bool interpolation =
     interpolateNextTarget(next_waypoint_number_, &next_target_position_);
@@ -282,8 +291,11 @@ bool PurePursuit::canGetCurvature(double* output_kappa)
     return false;
   }
 
-  // ROS_INFO("next_target : ( %lf , %lf , %lf)",
-  //  next_target.x, next_target.y,next_target.z);
+  /*
+   ROS_WARN("next_target : ( %lf , %lf , %lf)  NOW : ( %lf , %lf , %lf)",
+      next_target_position_.x, next_target_position_.y,next_target_position_.z,
+      current_pose_.position.x ,current_pose_.position.y,current_pose_.position.z);
+      */
 
   *output_kappa = calcCurvature(next_target_position_);
     ROS_WARN("get kappa %lf... ",*output_kappa);

src/ros/catkin/src/pure_pursuit/src/pure_pursuit_core.cpp

@@ -211,7 +211,7 @@ double PurePursuitNode::computeCommandVelocity() const
     return getSgn() * kmph2mps(const_velocity_);
   }
 
-  ROS_WARN("get velocity  %lf... ",command_linear_velocity_);
+  //ROS_WARN("get velocity  %lf... ",command_linear_velocity_);
   return command_linear_velocity_;
 }
 
@@ -289,7 +289,7 @@ void PurePursuitNode::callbackFromCurrentVelocity(
 void PurePursuitNode::callbackFromWayPoints(
   const autoware_msgs::LaneConstPtr& msg)
 {
-  ROS_WARN(" TWIST X %lf",msg->waypoints.at(0).twist.twist.linear.x );
+ // ROS_WARN(" TWIST X %lf",msg->waypoints.at(0).twist.twist.linear.x );
   command_linear_velocity_ =
     (!msg->waypoints.empty()) ? msg->waypoints.at(0).twist.twist.linear.x : 0;
   if (add_virtual_end_waypoints_)