change for apollo. now first run ok.

src/apollo/change/sim_perfect_control.cc

@@ -0,0 +1,625 @@
+/******************************************************************************
+ * 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.
+ *****************************************************************************/
+
+#include "modules/dreamview/backend/common/sim_control_manager/dynamic_model/perfect_control/sim_perfect_control.h"
+
+#include "cyber/common/file.h"
+#include "cyber/time/clock.h"
+#include "modules/common/adapters/adapter_gflags.h"
+#include "modules/common/math/linear_interpolation.h"
+#include "modules/common/math/math_utils.h"
+#include "modules/common/math/quaternion.h"
+#include "modules/common/util/message_util.h"
+#include "modules/common/util/util.h"
+
+#include <math.h>
+
+namespace apollo {
+namespace dreamview {
+
+using apollo::canbus::Chassis;
+using apollo::common::Header;
+using apollo::common::Point3D;
+using apollo::common::Quaternion;
+using apollo::common::TrajectoryPoint;
+using apollo::common::math::HeadingToQuaternion;
+using apollo::common::math::InterpolateUsingLinearApproximation;
+using apollo::common::math::InverseQuaternionRotate;
+using apollo::common::util::FillHeader;
+using apollo::cyber::Clock;
+using apollo::localization::LocalizationEstimate;
+using apollo::planning::ADCTrajectory;
+using apollo::planning::PlanningCommand;
+using apollo::prediction::PredictionObstacles;
+using apollo::relative_map::NavigationInfo;
+using Json = nlohmann::json;
+
+#include <iostream>
+#include <fstream>
+#include <chrono>
+#include <iomanip>
+void  adclog1(const char * strlog){
+    std::ofstream myFile;
+ //   myFile.open("/home/yuchuli/adclog.txt", std::ios_base::app);
+    myFile.open("/apollo/adclog.txt", std::ios_base::app);
+    if(myFile.is_open())
+    {       
+        auto now = std::chrono::system_clock::now();
+//        std::time_t now_c = std::chrono::system_clock::to_time_t(now - std::chrono::hours(24));
+        std::time_t now_c = std::chrono::system_clock::to_time_t(now);
+        myFile<<std::put_time(std::localtime(&now_c), "%Y-%m-%d %X  ") <<strlog<<std::endl;
+        myFile.close();
+    }
+    else
+    {
+        std::cout<<"open fail"<<std::endl;
+    }
+}
+
+// gflags定义类型	描述
+DEFINE_bool(use_realveh, false, "set use real vehicle");   // bool位整型
+ 
+DECLARE_bool(use_realveh);
+
+double fNowVel = 0.0;
+
+void LoadConfigFile(const std::string& file_path) {
+    std::ifstream config_file(file_path);
+    if (!config_file.is_open()) {
+        std::cout << "Failed to open configuration file: " << file_path << std::endl;
+        return;
+    }
+    std::string line;
+    while (std::getline(config_file, line)) {
+        std::istringstream iss(line);
+        std::string key, value;
+        if (std::getline(iss, key, '=') && std::getline(iss, value)) {
+            // 去除前后的空白字符
+            std::size_t first = value.find_first_not_of(" \t");
+            std::size_t last = value.find_last_not_of(" \t");
+            if (first != std::string::npos && last != std::string::npos) {
+                value = value.substr(first, last - first + 1);
+                // 这里可以处理不同的 flag 类型，这里简单处理为 string 类型
+                if (key == "-use_realveh") {
+                    google::SetCommandLineOption("use_realveh", value.c_str());
+                }
+                // 可以添加更多类型的处理
+            }
+        }
+    }
+    config_file.close();
+}
+
+namespace {
+
+void TransformToVRF(const Point3D &point_mrf, const Quaternion &orientation,
+                    Point3D *point_vrf) {
+  Eigen::Vector3d v_mrf(point_mrf.x(), point_mrf.y(), point_mrf.z());
+  auto v_vrf = InverseQuaternionRotate(orientation, v_mrf);
+  point_vrf->set_x(v_vrf.x());
+  point_vrf->set_y(v_vrf.y());
+  point_vrf->set_z(v_vrf.z());
+}
+
+bool IsSameHeader(const Header &lhs, const Header &rhs) {
+  return lhs.sequence_num() == rhs.sequence_num() &&
+         lhs.timestamp_sec() == rhs.timestamp_sec();
+}
+
+}  // namespace
+
+SimPerfectControl::SimPerfectControl(const MapService *map_service)
+    : SimControlBase(),
+      map_service_(map_service),
+      node_(cyber::CreateNode("sim_perfect_control")),
+      current_trajectory_(std::make_shared<ADCTrajectory>()) {
+  InitTimerAndIO();
+  
+  LoadConfigFile("/apollo/adcconfig.txt");
+  if(FLAGS_use_realveh == true){
+	  adclog1("use real vehicle.");
+  }else{
+	  adclog1("use simulate.");
+  }
+}
+
+void SimPerfectControl::InitTimerAndIO() {
+  localization_reader_ =
+      node_->CreateReader<LocalizationEstimate>(FLAGS_localization_topic);
+  localization_adc_reader_ = node_->CreateReader<LocalizationEstimate>(
+      "/apollo/localization/adcpose",
+      [this](const std::shared_ptr<LocalizationEstimate> &xloc) {
+        this->onADCLoc(xloc);
+      });
+  planning_reader_ = node_->CreateReader<ADCTrajectory>(
+      FLAGS_planning_trajectory_topic,
+      [this](const std::shared_ptr<ADCTrajectory> &trajectory) {
+        this->OnPlanning(trajectory);
+      });
+  planning_command_reader_ = node_->CreateReader<PlanningCommand>(
+      FLAGS_planning_command,
+      [this](const std::shared_ptr<PlanningCommand> &planning_command) {
+        this->OnPlanningCommand(planning_command);
+      });
+  navigation_reader_ = node_->CreateReader<NavigationInfo>(
+      FLAGS_navigation_topic,
+      [this](const std::shared_ptr<NavigationInfo> &navigation_info) {
+        this->OnReceiveNavigationInfo(navigation_info);
+      });
+  prediction_reader_ = node_->CreateReader<PredictionObstacles>(
+      FLAGS_prediction_topic,
+      [this](const std::shared_ptr<PredictionObstacles> &obstacles) {
+        this->OnPredictionObstacles(obstacles);
+      });
+
+  localization_writer_ =
+      node_->CreateWriter<LocalizationEstimate>(FLAGS_localization_topic);
+  chassis_writer_ = node_->CreateWriter<Chassis>(FLAGS_chassis_topic);
+  prediction_writer_ =
+      node_->CreateWriter<PredictionObstacles>(FLAGS_prediction_topic);
+
+  // Start timer to publish localization and chassis messages.
+  sim_control_timer_.reset(new cyber::Timer(
+      kSimControlIntervalMs, [this]() { this->RunOnce(); }, false));
+  sim_prediction_timer_.reset(new cyber::Timer(
+      kSimPredictionIntervalMs, [this]() { this->PublishDummyPrediction(); },
+      false));
+}
+
+void SimPerfectControl::Init(bool set_start_point,
+                             nlohmann::json start_point_attr,
+                             bool use_start_point_position) {
+  if (set_start_point && !FLAGS_use_navigation_mode) {
+    InitStartPoint(start_point_attr["start_velocity"],
+                   start_point_attr["start_acceleration"]);
+  }
+}
+
+void SimPerfectControl::InitStartPoint(double x, double y,
+                                       double start_velocity,
+                                       double start_acceleration) {
+  TrajectoryPoint point;
+  // Use the scenario start point as start point,
+  start_point_from_localization_ = false;
+  point.mutable_path_point()->set_x(x);
+  point.mutable_path_point()->set_y(y);
+  // z use default 0
+  point.mutable_path_point()->set_z(0);
+  double theta = 0.0;
+  double s = 0.0;
+  map_service_->GetPoseWithRegardToLane(x, y, &theta, &s);
+  point.mutable_path_point()->set_theta(theta);
+  point.set_v(start_velocity);
+  point.set_a(start_acceleration);
+  SetStartPoint(point);
+}
+
+void SimPerfectControl::ReSetPoinstion(double x, double y, double heading) {
+  std::lock_guard<std::mutex> lock(mutex_);
+  TrajectoryPoint point;
+  point.mutable_path_point()->set_x(x);
+  point.mutable_path_point()->set_y(y);
+  // z use default 0
+  point.mutable_path_point()->set_z(0);
+  point.mutable_path_point()->set_theta(heading);
+  AINFO << "start point:" << point.DebugString();
+  SetStartPoint(point);
+  InternalReset();
+}
+
+void SimPerfectControl::InitStartPoint(double start_velocity,
+                                       double start_acceleration) {
+  TrajectoryPoint point;
+  // Use the latest localization position as start point,
+  // fall back to a dummy point from map
+  localization_reader_->Observe();
+  start_point_from_localization_ = false;
+  if (!localization_reader_->Empty()) {
+    const auto &localization = localization_reader_->GetLatestObserved();
+    const auto &pose = localization->pose();
+    if (map_service_->PointIsValid(pose.position().x(), pose.position().y())) {
+      point.mutable_path_point()->set_x(pose.position().x());
+      point.mutable_path_point()->set_y(pose.position().y());
+      point.mutable_path_point()->set_z(pose.position().z());
+      point.mutable_path_point()->set_theta(pose.heading());
+      point.set_v(
+          std::hypot(pose.linear_velocity().x(), pose.linear_velocity().y()));
+      // Calculates the dot product of acceleration and velocity. The sign
+      // of this projection indicates whether this is acceleration or
+      // deceleration.
+      double projection =
+          pose.linear_acceleration().x() * pose.linear_velocity().x() +
+          pose.linear_acceleration().y() * pose.linear_velocity().y();
+
+      // Calculates the magnitude of the acceleration. Negate the value if
+      // it is indeed a deceleration.
+      double magnitude = std::hypot(pose.linear_acceleration().x(),
+                                    pose.linear_acceleration().y());
+      point.set_a(std::signbit(projection) ? -magnitude : magnitude);
+      start_point_from_localization_ = true;
+    }
+  }
+  if (!start_point_from_localization_) {
+    apollo::common::PointENU start_point;
+    if (!map_service_->GetStartPoint(&start_point)) {
+      AWARN << "Failed to get a dummy start point from map!";
+      return;
+    }
+    point.mutable_path_point()->set_x(start_point.x());
+    point.mutable_path_point()->set_y(start_point.y());
+    point.mutable_path_point()->set_z(start_point.z());
+    double theta = 0.0;
+    double s = 0.0;
+    map_service_->GetPoseWithRegardToLane(start_point.x(), start_point.y(),
+                                          &theta, &s);
+    point.mutable_path_point()->set_theta(theta);
+    point.set_v(start_velocity);
+    point.set_a(start_acceleration);
+  }
+  SetStartPoint(point);
+}
+
+void SimPerfectControl::SetStartPoint(const TrajectoryPoint &start_point) {
+  next_point_ = start_point;
+  prev_point_index_ = next_point_index_ = 0;
+  received_planning_ = false;
+}
+
+void SimPerfectControl::Reset() {
+  std::lock_guard<std::mutex> lock(mutex_);
+  InternalReset();
+}
+
+void SimPerfectControl::Stop() {
+  if (enabled_) {
+    {
+      std::lock_guard<std::mutex> lock(timer_mutex_);
+      sim_control_timer_->Stop();
+      sim_prediction_timer_->Stop();
+    }
+    enabled_ = false;
+  }
+}
+
+void SimPerfectControl::InternalReset() {
+  current_routing_header_.Clear();
+  send_dummy_prediction_ = true;
+  ClearPlanning();
+}
+
+void SimPerfectControl::ClearPlanning() {
+  current_trajectory_->Clear();
+  received_planning_ = false;
+}
+
+void SimPerfectControl::OnReceiveNavigationInfo(
+    const std::shared_ptr<NavigationInfo> &navigation_info) {
+  std::lock_guard<std::mutex> lock(mutex_);
+  if (!enabled_) {
+    return;
+  }
+  if (navigation_info->navigation_path_size() > 0) {
+    const auto &path = navigation_info->navigation_path(0).path();
+    if (path.path_point_size() > 0) {
+      adc_position_ = path.path_point(0);
+    }
+  }
+}
+
+void SimPerfectControl::onADCLoc(const std::shared_ptr<apollo::localization::LocalizationEstimate> &xloc){
+	std::lock_guard<std::mutex> lock(mutex_);
+	adcloc_ = xloc;
+	receiveadcloc = true;
+}
+
+void SimPerfectControl::OnPlanningCommand(
+    const std::shared_ptr<PlanningCommand> &planning_command) {
+  std::lock_guard<std::mutex> lock(mutex_);
+  if (!enabled_) {
+    return;
+  }
+  // Avoid not sending prediction messages to planning after playing packets
+  // with obstacles
+  send_dummy_prediction_ = true;
+  current_routing_header_ = planning_command->header();
+  AINFO << "planning_command: " << planning_command->DebugString();
+
+  if (planning_command->lane_follow_command()
+          .routing_request()
+          .is_start_pose_set()) {
+    auto lane_follow_command = planning_command->mutable_lane_follow_command();
+    const auto &start_pose =
+        lane_follow_command->routing_request().waypoint(0).pose();
+    ClearPlanning();
+    TrajectoryPoint point;
+    point.mutable_path_point()->set_x(start_pose.x());
+    point.mutable_path_point()->set_y(start_pose.y());
+    point.set_a(next_point_.has_a() ? next_point_.a() : 0.0);
+    point.set_v(next_point_.has_v() ? next_point_.v() : 0.0);
+    double theta = 0.0;
+    double s = 0.0;
+
+    // Find the lane nearest to the start pose and get its heading as theta.
+    map_service_->GetPoseWithRegardToLane(start_pose.x(), start_pose.y(),
+                                          &theta, &s);
+
+    point.mutable_path_point()->set_theta(theta);
+    SetStartPoint(point);
+  }
+}
+
+void SimPerfectControl::OnPredictionObstacles(
+    const std::shared_ptr<PredictionObstacles> &obstacles) {
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  if (!enabled_) {
+    return;
+  }
+
+  send_dummy_prediction_ = obstacles->header().module_name() == "SimPrediction";
+}
+
+void SimPerfectControl::Start() {
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  if (!enabled_) {
+    // When there is no localization yet, Init(true) will use a
+    // dummy point from the current map as an arbitrary start.
+    // When localization is already available, we do not need to
+    // reset/override the start point.
+    localization_reader_->Observe();
+    Json start_point_attr({});
+    start_point_attr["start_velocity"] =
+        next_point_.has_v() ? next_point_.v() : 0.0;
+    start_point_attr["start_acceleration"] =
+        next_point_.has_a() ? next_point_.a() : 0.0;
+    Init(true, start_point_attr);
+    InternalReset();
+    {
+      std::lock_guard<std::mutex> lock(timer_mutex_);
+      sim_control_timer_->Start();
+      sim_prediction_timer_->Start();
+    }
+    enabled_ = true;
+  }
+}
+
+void SimPerfectControl::Start(double x, double y) {
+  std::lock_guard<std::mutex> lock(mutex_);
+  if (!enabled_) {
+    // Do not use localization info. use scenario start point to init start
+    // point.
+    InitStartPoint(x, y, 0, 0);
+    InternalReset();
+    sim_control_timer_->Start();
+    sim_prediction_timer_->Start();
+    enabled_ = true;
+  }
+}
+
+void SimPerfectControl::OnPlanning(
+    const std::shared_ptr<ADCTrajectory> &trajectory) {
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  if (!enabled_) {
+    return;
+  }
+
+  // Reset current trajectory and the indices upon receiving a new trajectory.
+  // The routing SimPerfectControl owns must match with the one Planning has.
+  if (IsSameHeader(trajectory->routing_header(), current_routing_header_)) {
+    current_trajectory_ = trajectory;
+    prev_point_index_ = 0;
+    next_point_index_ = 0;
+    received_planning_ = true;
+  } else {
+    ClearPlanning();
+  }
+}
+
+void SimPerfectControl::Freeze() {
+  next_point_.set_v(0.0);
+  next_point_.set_a(0.0);
+  prev_point_ = next_point_;
+}
+
+void SimPerfectControl::RunOnce() {
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  TrajectoryPoint trajectory_point;
+  Chassis::GearPosition gear_position;
+  if (!PerfectControlModel(&trajectory_point, &gear_position)) {
+    AERROR << "Failed to calculate next point with perfect control model";
+    return;
+  }
+
+  PublishChassis(trajectory_point.v(), gear_position);
+  PublishLocalization(trajectory_point);
+}
+
+bool SimPerfectControl::PerfectControlModel(
+    TrajectoryPoint *point, Chassis::GearPosition *gear_position) {
+  // Result of the interpolation.
+  auto current_time = Clock::NowInSeconds();
+  const auto &trajectory = current_trajectory_->trajectory_point();
+  *gear_position = current_trajectory_->gear();
+  
+//  adclog1("run perfect control model.");
+
+  if (!received_planning_) {
+    prev_point_ = next_point_;
+  } else {
+    if (current_trajectory_->estop().is_estop() ||
+        next_point_index_ >= trajectory.size()) {
+      // Freeze the car when there's an estop or the current trajectory has
+      // been exhausted.
+      Freeze();
+    } else {
+      // Determine the status of the car based on received planning message.
+      while (next_point_index_ < trajectory.size() &&
+             current_time > trajectory.Get(next_point_index_).relative_time() +
+                                current_trajectory_->header().timestamp_sec()) {
+        ++next_point_index_;
+      }
+
+      if (next_point_index_ >= trajectory.size()) {
+        next_point_index_ = trajectory.size() - 1;
+      }
+
+      if (next_point_index_ == 0) {
+        AERROR << "First trajectory point is a future point!";
+        return false;
+      }
+
+      prev_point_index_ = next_point_index_ - 1;
+
+      next_point_ = trajectory.Get(next_point_index_);
+      prev_point_ = trajectory.Get(prev_point_index_);
+    }
+  }
+
+  if (current_time > next_point_.relative_time() +
+                         current_trajectory_->header().timestamp_sec()) {
+    // Don't try to extrapolate if relative_time passes last point
+    *point = next_point_;
+  } else {
+    *point = InterpolateUsingLinearApproximation(
+        prev_point_, next_point_,
+        current_time - current_trajectory_->header().timestamp_sec());
+  }
+  
+  return true;
+}
+
+void SimPerfectControl::PublishChassis(double cur_speed,
+                                       Chassis::GearPosition gear_position) {
+  auto chassis = std::make_shared<Chassis>();
+  FillHeader("SimPerfectControl", chassis.get());
+
+  chassis->set_engine_started(true);
+  chassis->set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE);
+  chassis->set_gear_location(gear_position);
+
+  chassis->set_speed_mps(static_cast<float>(cur_speed));
+  if (gear_position == canbus::Chassis::GEAR_REVERSE) {
+    chassis->set_speed_mps(-chassis->speed_mps());
+  }
+  
+  if((FLAGS_use_realveh == true) && (receiveadcloc)){
+	chassis->set_speed_mps(static_cast<float>(fNowVel));
+  }
+
+  chassis->set_throttle_percentage(0.0);
+  chassis->set_brake_percentage(0.0);
+
+  chassis_writer_->Write(chassis);
+}
+
+void SimPerfectControl::PublishLocalization(const TrajectoryPoint &point) {
+  auto localization = std::make_shared<LocalizationEstimate>();
+  FillHeader("SimPerfectControl", localization.get());
+  
+  if((FLAGS_use_realveh == true) && (receiveadcloc))
+  {
+	  localization = adcloc_;
+	  localization_writer_->Write(localization);
+	  
+	  auto *pose = localization->mutable_pose();
+	  
+	  fNowVel = sqrt(pow(pose->mutable_linear_velocity()->x(),2)+pow(pose->mutable_linear_velocity()->y(),2));
+
+	 adc_position_.set_x(pose->position().x());
+     adc_position_.set_y(pose->position().y());
+     adc_position_.set_z(pose->position().z());
+	 return;
+  }
+  auto *pose = localization->mutable_pose();
+  auto prev = prev_point_.path_point();
+  auto next = next_point_.path_point();
+
+  // Set position  507530.95 4331929.85
+  pose->mutable_position()->set_x(point.path_point().x());
+  pose->mutable_position()->set_y(point.path_point().y());
+  pose->mutable_position()->set_z(point.path_point().z());
+  // Set orientation and heading
+  double cur_theta = point.path_point().theta();
+
+  if (FLAGS_use_navigation_mode) {
+    double flu_x = point.path_point().x();
+    double flu_y = point.path_point().y();
+
+    Eigen::Vector2d enu_coordinate =
+        common::math::RotateVector2d({flu_x, flu_y}, cur_theta);
+
+    enu_coordinate.x() += adc_position_.x();
+    enu_coordinate.y() += adc_position_.y();
+    pose->mutable_position()->set_x(enu_coordinate.x());
+    pose->mutable_position()->set_y(enu_coordinate.y());
+  }
+
+  Eigen::Quaternion<double> cur_orientation =
+      HeadingToQuaternion<double>(cur_theta);
+  pose->mutable_orientation()->set_qw(cur_orientation.w());
+  pose->mutable_orientation()->set_qx(cur_orientation.x());
+  pose->mutable_orientation()->set_qy(cur_orientation.y());
+  pose->mutable_orientation()->set_qz(cur_orientation.z());
+  pose->set_heading(cur_theta);
+
+  // Set linear_velocity
+  pose->mutable_linear_velocity()->set_x(std::cos(cur_theta) * point.v());
+  pose->mutable_linear_velocity()->set_y(std::sin(cur_theta) * point.v());
+  pose->mutable_linear_velocity()->set_z(0);
+
+  // Set angular_velocity in both map reference frame and vehicle reference
+  // frame
+  pose->mutable_angular_velocity()->set_x(0);
+  pose->mutable_angular_velocity()->set_y(0);
+  pose->mutable_angular_velocity()->set_z(point.v() *
+                                          point.path_point().kappa());
+
+  TransformToVRF(pose->angular_velocity(), pose->orientation(),
+                 pose->mutable_angular_velocity_vrf());
+
+  // Set linear_acceleration in both map reference frame and vehicle reference
+  // frame
+  auto *linear_acceleration = pose->mutable_linear_acceleration();
+  linear_acceleration->set_x(std::cos(cur_theta) * point.a());
+  linear_acceleration->set_y(std::sin(cur_theta) * point.a());
+  linear_acceleration->set_z(0);
+
+  TransformToVRF(pose->linear_acceleration(), pose->orientation(),
+                 pose->mutable_linear_acceleration_vrf());
+  localization_writer_->Write(localization);
+
+  adc_position_.set_x(pose->position().x());
+  adc_position_.set_y(pose->position().y());
+  adc_position_.set_z(pose->position().z());
+}
+
+void SimPerfectControl::PublishDummyPrediction() {
+  auto prediction = std::make_shared<PredictionObstacles>();
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (!send_dummy_prediction_) {
+      return;
+    }
+    FillHeader("SimPrediction", prediction.get());
+  }
+  prediction_writer_->Write(prediction);
+}
+
+}  // namespace dreamview
+}  // namespace apollo

src/apollo/change/sim_perfect_control.h

@@ -0,0 +1,214 @@
+/******************************************************************************
+ * 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.
+ *****************************************************************************/
+
+/**
+ * @file
+ */
+
+#pragma once
+
+#include <memory>
+
+#include "gtest/gtest_prod.h"
+
+#include "modules/common_msgs/localization_msgs/localization.pb.h"
+#include "modules/common_msgs/planning_msgs/navigation.pb.h"
+#include "modules/common_msgs/planning_msgs/planning.pb.h"
+#include "modules/common_msgs/planning_msgs/planning_command.pb.h"
+#include "modules/common_msgs/prediction_msgs/prediction_obstacle.pb.h"
+#include "modules/common_msgs/routing_msgs/routing.pb.h"
+
+#include "cyber/cyber.h"
+#include "modules/dreamview/backend/common/dreamview_gflags.h"
+#include "modules/dreamview/backend/common/map_service/map_service.h"
+#include "modules/dreamview/backend/common/sim_control_manager/core/sim_control_base.h"
+
+/**
+ * @namespace apollo::dreamview
+ * @brief apollo::dreamview
+ */
+namespace apollo {
+namespace dreamview {
+
+/**
+ * @class SimPerfectControl
+ * @brief A module that simulates a 'perfect control' algorithm, which assumes
+ * an ideal world where the car can be perfectly placed wherever the planning
+ * asks it to be, with the expected speed, acceleration, etc.
+ */
+class SimPerfectControl final : public SimControlBase {
+ public:
+  /**
+   * @brief Constructor of SimPerfectControl.
+   * @param map_service the pointer of MapService.
+   */
+  explicit SimPerfectControl(const MapService *map_service);
+
+  /**
+   * @brief setup callbacks and timer
+   * @param set_start_point initialize localization.
+   */
+  void Init(bool set_start_point, nlohmann::json start_point_attr,
+            bool use_start_point_position = false) override;
+
+  /**
+   * @brief Starts the timer to publish simulated localization and chassis
+   * messages.
+   */
+  void Start() override;
+
+  /**
+   * @brief Stops the timer.
+   */
+  void Stop() override;
+
+  /**
+   * @brief Set vehicle position.
+   */
+  void ReSetPoinstion(double x, double y, double heading) override;
+
+  /**
+   * @brief Resets the internal state.
+   */
+  void Reset() override;
+
+  void RunOnce() override;
+
+ private:
+  void OnPlanning(
+      const std::shared_ptr<apollo::planning::ADCTrajectory> &trajectory);
+  void OnPlanningCommand(
+      const std::shared_ptr<apollo::planning::PlanningCommand>
+          &planning_command);
+  void OnReceiveNavigationInfo(
+      const std::shared_ptr<apollo::relative_map::NavigationInfo>
+          &navigation_info);
+  void OnPredictionObstacles(
+      const std::shared_ptr<apollo::prediction::PredictionObstacles>
+          &obstacles);
+          
+  void onADCLoc(const std::shared_ptr<apollo::localization::LocalizationEstimate> &xloc);
+
+  /**
+   * @brief Predict the next trajectory point using perfect control model
+   */
+  bool PerfectControlModel(
+      apollo::common::TrajectoryPoint *point,
+      apollo::canbus::Chassis::GearPosition *gear_position);
+
+  void PublishChassis(double cur_speed,
+                      apollo::canbus::Chassis::GearPosition gear_position);
+
+  void PublishLocalization(const apollo::common::TrajectoryPoint &point);
+
+  void PublishDummyPrediction();
+
+  void InitTimerAndIO();
+
+  /**
+   * @brief Starts the timer to publish simulated localization and chassis
+   * messages. Designated Start point for scenario
+   */
+  void Start(double x, double y) override;
+
+  void InitStartPoint(double start_velocity, double start_acceleration);
+
+  // use scenario start point to init start point under the simulation
+  // condition.
+  void InitStartPoint(double x, double y, double start_velocity,
+                      double start_acceleration);
+
+  // Reset the start point, which can be a dummy point on the map, a current
+  // localization pose, or a start position received from the routing module.
+  void SetStartPoint(const apollo::common::TrajectoryPoint &point);
+
+  void Freeze();
+
+  void ClearPlanning();
+
+  void InternalReset();
+
+  const MapService *map_service_ = nullptr;
+
+  std::unique_ptr<cyber::Node> node_;
+
+  std::shared_ptr<cyber::Reader<apollo::localization::LocalizationEstimate>>
+      localization_reader_;
+  std::shared_ptr<cyber::Reader<apollo::planning::ADCTrajectory>>
+      planning_reader_;
+  std::shared_ptr<cyber::Reader<apollo::relative_map::NavigationInfo>>
+      navigation_reader_;
+  std::shared_ptr<cyber::Reader<apollo::prediction::PredictionObstacles>>
+      prediction_reader_;
+  std::shared_ptr<cyber::Reader<apollo::planning::PlanningCommand>>
+      planning_command_reader_;
+  std::shared_ptr<cyber::Writer<apollo::localization::LocalizationEstimate>>
+      localization_writer_;
+  std::shared_ptr<cyber::Writer<apollo::canbus::Chassis>> chassis_writer_;
+  std::shared_ptr<cyber::Writer<apollo::prediction::PredictionObstacles>>
+      prediction_writer_;
+      
+   std::shared_ptr<cyber::Reader<apollo::localization::LocalizationEstimate>>
+      localization_adc_reader_;
+
+  // The timer to publish simulated localization and chassis messages.
+  std::unique_ptr<cyber::Timer> sim_control_timer_;
+
+  // The timer to publish dummy prediction
+  std::unique_ptr<cyber::Timer> sim_prediction_timer_;
+
+  // Time interval of the timer, in milliseconds.
+  static constexpr double kSimControlIntervalMs = 10;
+  static constexpr double kSimPredictionIntervalMs = 100;
+
+  // The latest received planning trajectory.
+  std::shared_ptr<apollo::planning::ADCTrajectory> current_trajectory_;
+  // The index of the previous and next point with regard to the
+  // current_trajectory.
+  int prev_point_index_ = 0;
+  int next_point_index_ = 0;
+
+  // Whether there's a planning received after the most recent routing.
+  bool received_planning_ = false;
+  
+  std::shared_ptr<apollo::localization::LocalizationEstimate> adcloc_;
+  bool receiveadcloc = false;
+
+  // Whether start point is initialized from actual localization data
+  bool start_point_from_localization_ = false;
+
+  // Whether to send dummy predictions
+  bool send_dummy_prediction_ = true;
+
+  // The header of the routing planning is following.
+  apollo::common::Header current_routing_header_;
+
+  apollo::common::TrajectoryPoint prev_point_;
+  apollo::common::TrajectoryPoint next_point_;
+
+  common::PathPoint adc_position_;
+
+  // Linearize reader/timer callbacks and external operations.
+  std::mutex mutex_;
+  // Locks related to timer.
+  std::mutex timer_mutex_;
+
+  FRIEND_TEST(SimControlTest, Test);
+  FRIEND_TEST(SimControlTest, TestDummyPrediction);
+};
+
+}  // namespace dreamview
+}  // namespace apollo

src/apollo/modules/adc/pilot_apollo_bridge/BUILD

@@ -13,10 +13,12 @@ apollo_cc_binary(
         "//modules/common/util:util_tool",
         "//modules/common/adapters:adapter_gflags",
         "//modules/adc/pilot_apollo_bridge/proto:gpsimu_proto",
+        "//modules/adc/pilot_apollo_bridge/proto:apolloctrlcmd_proto",
         "//modules/common_msgs/localization_msgs:gps_proto",
         "//modules/common_msgs/localization_msgs:localization_proto",
         "//modules/common_msgs/sensor_msgs:ins_cc_proto",
         "//modules/common_msgs/localization_msgs:imu_cc_proto",
+        "//modules/common_msgs/control_msgs:control_cmd_proto",
         "@qt//:qt_core",
         "@eigen",
         "@proj",

src/apollo/modules/adc/pilot_apollo_bridge/pilot_apollo_bridge.cc

@@ -1,4 +1,5 @@
 #include "modules/adc/pilot_apollo_bridge/proto/gpsimu.pb.h"
+#include "modules/adc/pilot_apollo_bridge/proto/apolloctrlcmd.pb.h"
  #include "cyber/cyber.h"
  #include "cyber/time/rate.h"
  #include "cyber/time/time.h"
@@ -14,6 +15,8 @@
 
 #include "modules/common_msgs/localization_msgs/localization.pb.h"
 
+#include "modules/common_msgs/control_msgs/control_cmd.pb.h"
+
 #include <GeographicLib/MGRS.hpp>
 #include <GeographicLib/UTMUPS.hpp>
 
@@ -70,6 +73,11 @@ const char *WGS84_TEXT = "+proj=latlong +ellps=WGS84";
   std::shared_ptr<apollo::cyber::Writer<apollo::localization::LocalizationEstimate>>
       localization_writer_;
 
+  std::shared_ptr<apollo::cyber::Reader<apollo::control::ControlCommand>>
+      control_command_reader_;
+      
+void * gpadecision;
+
 //const double DEG_TO_RAD = M_PI / 180.0;
 constexpr double DEG_TO_RAD_LOCAL = M_PI / 180.0;
 constexpr float FLOAT_NAN = std::numeric_limits<float>::quiet_NaN();
@@ -231,6 +239,28 @@ void PublishLocalization(const double x,const double y,const double z,const doub
 
 }
 
+
+void onControl(
+    const std::shared_ptr<apollo::control::ControlCommand> &xCmd) {
+//  std::lock_guard<std::mutex> lock(mutex_);
+
+  std::cout<<"steer: "<<xCmd->steering_target()<<" acc: "<<xCmd->acceleration()<<" brake:"<<xCmd->brake()<<std::endl;
+  
+    iv::apollo::apolloctrlcmd xctrlcmd;
+    xctrlcmd.set_acceleration(xCmd->acceleration());
+    xctrlcmd.set_brake(xCmd->brake());
+    xctrlcmd.set_steering_target(xCmd->steering_target());
+    
+    int nbytesize = (int)xctrlcmd.ByteSize();
+    std::shared_ptr<char> pstr_ptr = std::shared_ptr<char>(new char[nbytesize]);
+    if(!xctrlcmd.SerializeToArray(pstr_ptr.get(),nbytesize))
+    {
+        std::cout<<" send ctrlcmd Serialize Fail. "<<std::endl;
+        return;
+    }
+    iv::modulecomm::ModuleSendMsg(gpadecision,pstr_ptr.get(),nbytesize);
+}
+
 template <class T, size_t N>
 constexpr size_t array_size(T (&)[N]) {
   return N;
@@ -283,6 +313,16 @@ void ListenGPSIMU(const char * strdata,const unsigned int nSize,const unsigned i
     
     std::cout<<"recv gpsimu."<<std::endl;
     
+    double flon = xgpsimu.lon();
+    double flat = xgpsimu.lat();
+    
+    double utm_x,utm_y;
+    int zone{};
+    bool is_north{};
+    GeographicLib::UTMUPS::Forward(flat, flon, zone, is_north, utm_x,utm_y);
+    
+    PublishLocalization(utm_x,utm_y,xgpsimu.height(),(90-xgpsimu.heading())*M_PI/180.0,0,0,0);
+    
     int64_t timenow = std::chrono::system_clock::now().time_since_epoch().count();
     double seconds = timenow * 1e-9;//gps_week * SECONDS_PER_WEEK + gps_millisecs * 1e-3;
     
@@ -403,6 +443,9 @@ void InitIns(){
 	insstat_writer_ = talker_node->CreateWriter<apollo::drivers::gnss::InsStat>(FLAGS_ins_stat_topic);
 	corrimu_writer_ = talker_node->CreateWriter<apollo::localization::CorrectedImu>(FLAGS_imu_topic);
 	localization_writer_ = talker_node->CreateWriter<apollo::localization::LocalizationEstimate>(stradcpose);
+	
+    control_command_reader_ = talker_node->CreateReader<apollo::control::ControlCommand>(
+      "/apollo/control",onControl);
    // 从节点创建一个Topic,来实现对车速的查看
 //   auto talker = talker_node->CreateWriter<Car>("car_speed");
 //   AINFO << "I'll start telling you the current speed of the car.";
@@ -411,6 +454,10 @@ void InitIns(){
    (void)paraw;
    
    void * pasend = iv::modulecomm::RegisterSend("b1",1000,1);
+   
+   void * pactrl = iv::modulecomm::RegisterSend("apolloctrlcmd",10000,1);
+   
+   gpadecision = pactrl;
  
    std::cout<<" enter talker."<<std::endl;
    //设置初始速度为0，然后速度每秒增加5km/h
@@ -427,6 +474,8 @@ void InitIns(){
         iv::modulecomm::ModuleSendMsg(pasend,str,30);
        sleep(1);
    }
+   
+   apollo::cyber::WaitForShutdown();
    return 0;
  }
 

src/apollo/modules/adc/pilot_apollo_bridge/proto/BUILD

@@ -9,4 +9,9 @@ proto_library(
     srcs = ["gpsimu.proto"],
 )
 
+proto_library(
+    name = "apolloctrlcmd_proto",
+    srcs = ["apolloctrlcmd.proto"],
+)
+
 apollo_package()

src/apollo/third_party/geographic/3rd-geographic.BUILD

@@ -0,0 +1,17 @@
+load("@rules_cc//cc:defs.bzl", "cc_library")
+
+package(default_visibility = ["//visibility:public"])
+
+licenses(["notice"])
+
+cc_library(
+    name = "Geographic",
+    includes = [
+        "include",
+    ],
+    hdrs = glob(["include/**/*"]),
+    linkopts = [
+        "-lGeographic",
+    ],
+    strip_include_prefix = "include",
+)

src/apollo/third_party/geographic/BUILD

@@ -0,0 +1,21 @@
+load("//tools/install:install.bzl", "install", "install_files", "install_src_files")
+
+package(
+    default_visibility = ["//visibility:public"],
+)
+
+install(
+    name = "install",
+    data_dest = "3rd-geographic",
+    data = [
+        ":cyberfile.xml",
+        ":3rd-geographic.BUILD",
+    ],
+)
+
+install_src_files(
+    name = "install_src",
+    src_dir = ["."],
+    dest = "3rd-geographic/src",
+    filter = "*",
+)

src/apollo/third_party/geographic/cyberfile.xml

@@ -0,0 +1,17 @@
+<package format="2">
+  <name>3rd-geographic</name>
+  <version>local</version>
+  <description>
+    Apollo packaged modulecomm Lib.
+  </description>
+
+  <maintainer email="apollo-support@baidu.com">Apollo</maintainer>
+  <license>Apache License 2.0</license>
+  <url type="website">https://www.apollo.auto/</url>
+  <url type="repository">https://github.com/ApolloAuto/apollo</url>
+  <url type="bugtracker">https://github.com/ApolloAuto/apollo/issues</url>
+
+  <type>third-binary</type>
+  <src_path url="https://github.com/ApolloAuto/apollo">//third_party/geographic</src_path>
+
+</package>

src/apollo/third_party/geographic/geographic.BUILD

@@ -0,0 +1,17 @@
+load("@rules_cc//cc:defs.bzl", "cc_library")
+
+package(default_visibility = ["//visibility:public"])
+
+licenses(["notice"])
+
+cc_library(
+    name = "geographic",
+    include_prefix = "",
+    includes = [
+        ".",
+    ],
+    hdrs = glob(["**/*"]),
+    linkopts = [
+        "-lGeographic",
+    ],
+)

src/apollo/third_party/geographic/workspace.bzl

@@ -0,0 +1,13 @@
+"""Loads the osqp library"""
+
+# Sanitize a dependency so that it works correctly from code that includes
+# Apollo as a submodule.
+def clean_dep(dep):
+    return str(Label(dep))
+
+def repo():
+    native.new_local_repository(
+        name = "geographic",
+        build_file = clean_dep("//third_party/geographic:geographic.BUILD"),
+        path = "/usr/include",
+    )

src/decition/decition_fromapollo/.gitignore

@@ -0,0 +1,73 @@
+# This file is used to ignore files which are generated
+# ----------------------------------------------------------------------------
+
+*~
+*.autosave
+*.a
+*.core
+*.moc
+*.o
+*.obj
+*.orig
+*.rej
+*.so
+*.so.*
+*_pch.h.cpp
+*_resource.rc
+*.qm
+.#*
+*.*#
+core
+!core/
+tags
+.DS_Store
+.directory
+*.debug
+Makefile*
+*.prl
+*.app
+moc_*.cpp
+ui_*.h
+qrc_*.cpp
+Thumbs.db
+*.res
+*.rc
+/.qmake.cache
+/.qmake.stash
+
+# qtcreator generated files
+*.pro.user*
+
+# xemacs temporary files
+*.flc
+
+# Vim temporary files
+.*.swp
+
+# Visual Studio generated files
+*.ib_pdb_index
+*.idb
+*.ilk
+*.pdb
+*.sln
+*.suo
+*.vcproj
+*vcproj.*.*.user
+*.ncb
+*.sdf
+*.opensdf
+*.vcxproj
+*vcxproj.*
+
+# MinGW generated files
+*.Debug
+*.Release
+
+# Python byte code
+*.pyc
+
+# Binaries
+# --------
+*.dll
+*.exe
+

src/decition/decition_fromapollo/ctrlcmd2decition.cpp

@@ -0,0 +1,136 @@
+#include "ctrlcmd2decition.h"
+
+#include <iostream>
+#include <math.h>
+
+ctrlcmd2decition::ctrlcmd2decition()
+{
+
+    ModuleFun xFunctrlcmd = std::bind(&ctrlcmd2decition::ListenCtrlCmd,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpactrlcmd = iv::modulecomm::RegisterRecvPlus("apolloctrlcmd",xFunctrlcmd);
+    mpadecision = iv::modulecomm::RegisterSend("deciton",1000,1);
+
+    ModuleFun xFunGPSIMU = std::bind(&ctrlcmd2decition::ListenGPSIMU,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+
+    mpagpsimu = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",xFunGPSIMU);
+
+}
+
+ctrlcmd2decition::~ctrlcmd2decition()
+{
+    iv::modulecomm::Unregister(mpagpsimu);
+    iv::modulecomm::Unregister(mpadecision);
+    iv::modulecomm::Unregister(mpactrlcmd);
+}
+
+void ctrlcmd2decition::ListenGPSIMU(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    (void)index;
+    (void)dt;
+    (void)strmemname;
+
+    iv::gps::gpsimu xgpsimu;
+    if(xgpsimu.ParseFromArray(strdata,nSize))
+    {
+        mfVelNow = sqrt(pow(xgpsimu.ve(),2) + pow(xgpsimu.vn(),2));
+    }
+    else
+    {
+        std::cout<<" ctrlcmd2decition::ListenGPSIMU Parse Fail ." <<std::endl;
+    }
+}
+
+void ctrlcmd2decition::ListenCtrlCmd(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    (void)index;
+    (void)dt;
+    (void)strmemname;
+
+    iv::apollo::apolloctrlcmd xctrlcmd;
+    // xctrlcmd.set_acceleration();
+    // xctrlcmd.set_brake();
+    // xctrlcmd.set_steering_target();
+
+    // int nbytesize = (int)xctrlcmd.ByteSize();
+    // std::shared_ptr<char> pstr_ptr = std::shared_ptr<char>(new char[nbytesize]);
+    // if(!xctrlcmd.SerializeToArray(pstr_ptr.get(),nbytesize))
+    // {
+    //     std::cout<<" send ctrlcmd Serialize Fail. "<<std::endl;
+    //     return;
+    // }
+    // iv::modulecomm::ModuleSendMsg(mpadecision,pstr_ptr.get(),nbytesize);
+
+    if(!xctrlcmd.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<" ctrlcmd2decition::ListenCtrlCmd Parse Fail"<<std::endl;
+        return;
+    }
+
+    iv::brain::decition xdecition;
+    double facc = 0;
+    double ftorque = 0;
+    double fbrake = xctrlcmd.brake();
+    double fwheelangle = 0;
+    std::cout<<" cmd acc: "<<xctrlcmd.acceleration()<<std::endl;
+
+    facc = xctrlcmd.acceleration();
+
+        double fVehWeight = 1800;
+        double fg = 9.8;
+        double fRollForce = 50;
+        const double fRatio = 2.5;
+        double fNeed = fRollForce + fVehWeight*facc;
+
+        ftorque = fNeed/fRatio;
+
+        if(fbrake>0.0001)
+        {
+            fbrake = fbrake * (-5.0) /100.0;
+            ftorque = 0;
+        }
+
+        if(ftorque<0)ftorque = 0;
+
+
+
+//    facc = xctrlcmd.linear_acceleration();
+    fwheelangle = xctrlcmd.steering_target() * 430.0/100.0;
+    if(fwheelangle>430)fwheelangle = 430;
+    if(fwheelangle<-430)fwheelangle = -430;
+    std::cout<<" acc: "<<facc<<"  wheel: "<<fwheelangle<<std::endl;
+    xdecition.set_accelerator(facc);
+    xdecition.set_brake(fbrake);
+    xdecition.set_leftlamp(false);
+    xdecition.set_rightlamp(false);
+    xdecition.set_speed(10.0);
+    xdecition.set_wheelangle(fwheelangle);
+    xdecition.set_wheelspeed(300);
+    xdecition.set_torque(ftorque);
+    xdecition.set_mode(1);
+    xdecition.set_gear(1);
+    xdecition.set_handbrake(0);
+    xdecition.set_grade(1);
+    xdecition.set_engine(0);
+    xdecition.set_brakelamp(false);
+    xdecition.set_ttc(15);
+//    xdecition.set_air_enable(decition->air_enable);
+//    xdecition.set_air_temp(decition->air_temp);
+//    xdecition.set_air_mode(decition->air_mode);
+//    xdecition.set_wind_level(decition->wind_level);
+    xdecition.set_roof_light(0);
+    xdecition.set_home_light(0);
+//    xdecition.set_air_worktime(decition->air_worktime);
+//    xdecition.set_air_offtime(decition->air_offtime);
+//    xdecition.set_air_on(decition->air_on);
+    xdecition.set_door(0);
+    xdecition.set_dippedlight(0);
+
+    int nbytesize = (int)xdecition.ByteSize();
+    std::shared_ptr<char> pstr_ptr = std::shared_ptr<char>(new char[nbytesize]);
+    if(!xdecition.SerializeToArray(pstr_ptr.get(),nbytesize))
+    {
+        std::cout<<" ctrlcmd2decition::ListenCtrlCmd Serialize Fail. "<<std::endl;
+        return;
+    }
+    iv::modulecomm::ModuleSendMsg(mpadecision,pstr_ptr.get(),nbytesize);
+}

src/decition/decition_fromapollo/ctrlcmd2decition.h

@@ -0,0 +1,32 @@
+#ifndef CTRLCMD2DECITION_H
+#define CTRLCMD2DECITION_H
+
+#include "modulecomm.h"
+
+#include "decition.pb.h"
+#include "apolloctrlcmd.pb.h"
+#include "gpsimu.pb.h"
+
+
+class ctrlcmd2decition
+{
+public:
+    ctrlcmd2decition();
+    ~ctrlcmd2decition();
+
+private:
+    void ListenCtrlCmd(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void ListenGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+
+
+private:
+    void * mpadecision;
+    void * mpactrlcmd;
+    void * mpagpsimu;
+
+private:
+
+    double mfVelNow = 0;
+};
+
+#endif // CTRLCMD2DECITION_H

src/decition/decition_fromapollo/decition_fromapollo.pro

@@ -0,0 +1,41 @@
+QT -= gui
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any Qt feature that has been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if it uses deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+        ../../include/msgtype/apolloctrlcmd.pb.cc \
+        ../../include/msgtype/decition.pb.cc \
+        ctrlcmd2decition.cpp \
+        main.cpp \
+    ../../include/msgtype/gpsimu.pb.cc
+
+# Default rules for deployment.
+qnx: target.path = /tmp/$${TARGET}/bin
+else: unix:!android: target.path = /opt/$${TARGET}/bin
+!isEmpty(target.path): INSTALLS += target
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+HEADERS += \
+    ../../include/msgtype/apolloctrlcmd.pb.h \
+    ../../include/msgtype/decition.pb.h \
+    ctrlcmd2decition.h \
+    ../../include/msgtype/gpsimu.pb.h

src/decition/decition_fromapollo/main.cpp

@@ -0,0 +1,9 @@
+#include <QCoreApplication>
+#include "ctrlcmd2decition.h"
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+    ctrlcmd2decition xc2d;
+    (void)xc2d;
+    return a.exec();
+}

src/include/proto/apolloctrlcmd.proto

@@ -0,0 +1,11 @@
+syntax = "proto2";
+
+package iv.apollo;
+
+message apolloctrlcmd
+{
+  required double steering_target = 1;
+  required double acceleration =2 ;//#m/s^2
+  required double brake = 3;
+ 
+};

src/tool/simple_planning_simulator/simmodel.cpp

@@ -39,6 +39,7 @@ simmodel::~simmodel()
     mpthreadstate->join();
 }
 
+#ifdef REAL_DYNAMICS
 void simmodel::UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
 {
     iv::chassis xchassis;
@@ -62,6 +63,8 @@ void simmodel::UpdateChassis(const char *strdata, const unsigned int nSize, cons
 
 }
 
+#endif
+
 void simmodel::SetCMD(double facc,double ftorque,double fbrake,double fwheelangle)
 {
     mfcmd_acc = facc;