Control: Add more smooth and script (#1055)

modules/control/controller/lat_controller.cc

@@ -168,6 +168,8 @@ void LatController::InitializeFilters(const ControlConf *control_conf) {
   **/
   lateral_error_filter_ =
       MeanFilter(control_conf->lat_controller_conf().mean_filter_window_size());
+  heading_error_filter_ =
+      MeanFilter(control_conf->lat_controller_conf().mean_filter_window_size());
 }
 
 Status LatController::Init(const ControlConf *control_conf) {
@@ -521,10 +523,15 @@ void LatController::ComputeLateralErrors(
       common::math::NormalizeAngle(theta - target_point.path_point().theta());
   double sin_delta_theta = std::sin(delta_theta);
   // d_error_dot = linear_v * sin_delta_theta;
+  // theta_error = delta_theta
+  // TODO(QiL): Code reformat after test
   debug->set_lateral_error_rate(linear_v * sin_delta_theta);
+  if (FLAGS_use_relative_position) {
+    debug->set_heading_error(heading_error_filter_.Update(delta_theta));
+  } else {
+    debug->set_heading_error(delta_theta);
+  }
 
-  // theta_error = delta_theta;
-  debug->set_heading_error(delta_theta);
   // theta_error_dot = angular_v - target_point.path_point().kappa() *
   // target_point.v();
   debug->set_heading_error_rate(angular_v - target_point.path_point().kappa() *

modules/control/controller/lat_controller.h

@@ -220,6 +220,7 @@ class LatController : public Controller {
 
   // MeanFilter heading_rate_filter_;
   MeanFilter lateral_error_filter_;
+  MeanFilter heading_error_filter_;
 
   // for logging purpose
   std::ofstream steer_log_file_;

scripts/planning_lite.sh

+#!/usr/bin/env bash
+
+###############################################################################
+# Copyright 2017 The Apollo Authors. 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.
+###############################################################################
+
+
+DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
+
+source "${DIR}/apollo_base.sh"
+
+# run function from apollo_base.sh
+# run command_name module_name
+function start() {
+	echo "start"
+	python modules/tools/planning_lite/planning_lite.py &
+	python modules/tools/mobileye_viewer/plot_mobileye.py
+}
+
+function stop() {
+    echo "stop"
+    pkill -f planning_lite.py 
+    pkill -f plot_mobileye.py
+}
+
+case $1 in                                                                                          
+start)                                                                                              
+    start                                                                                           
+    ;;                                                                                              
+help)                                                                                               
+    echo "$0 start|stop|help"                                                             
+    ;;                                                                                              
+stop)                                                                                               
+    stop                                                                                       
+    ;;                                                                                              
+*)                                                                                                  
+    stop                                                                                           
+    start                                                                                           
+    ;;
+esac