change some apollo code, now sim ok.

src/apollo/change/sim_perfect_control.cc

@@ -164,6 +164,12 @@ void SimPerfectControl::InitTimerAndIO() {
       [this](const std::shared_ptr<PredictionObstacles> &obstacles) {
         this->OnPredictionObstacles(obstacles);
       });
+  
+ control_command_reader_ = node_->CreateReader<apollo::control::ControlCommand>(
+      "/apollo/control",
+      [this](const std::shared_ptr<apollo::control::ControlCommand> &xctrlcmd) {
+        this->onControl(xctrlcmd);
+      });
 
   localization_writer_ =
       node_->CreateWriter<LocalizationEstimate>(FLAGS_localization_topic);
@@ -361,6 +367,19 @@ void SimPerfectControl::OnPlanningCommand(
   }
 }
 
+void SimPerfectControl::onControl(
+    const std::shared_ptr<apollo::control::ControlCommand> &xCmd) {
+//  std::lock_guard<std::mutex> lock(mutex_);
+
+  std::lock_guard<std::mutex> lock(mutex_);
+
+ // std::cout<<"steer: "<<xCmd->steering_target()<<" acc: "<<xCmd->acceleration()<<" brake:"<<xCmd->brake()<<std::endl;
+  
+  fcmdsteer_ = xCmd->steering_target();
+  fcmdbrake_ = xCmd->brake();
+  fcmdacc_ = xCmd->throttle();
+}
+
 void SimPerfectControl::OnPredictionObstacles(
     const std::shared_ptr<PredictionObstacles> &obstacles) {
   std::lock_guard<std::mutex> lock(mutex_);
@@ -522,8 +541,9 @@ void SimPerfectControl::PublishChassis(double cur_speed,
 	chassis->set_speed_mps(static_cast<float>(fNowVel));
   }
 
-  chassis->set_throttle_percentage(0.0);
-  chassis->set_brake_percentage(0.0);
+  chassis->set_throttle_percentage(fcmdacc_);
+  chassis->set_brake_percentage(fcmdbrake_);
+  chassis->set_steering_percentage(fcmdsteer_);
 
   chassis_writer_->Write(chassis);
 }

src/apollo/change/sim_perfect_control.h

@@ -30,6 +30,7 @@
 #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 "modules/common_msgs/control_msgs/control_cmd.pb.h"
 
 #include "cyber/cyber.h"
 #include "modules/dreamview/backend/common/dreamview_gflags.h"
@@ -101,6 +102,9 @@ class SimPerfectControl final : public SimControlBase {
           &obstacles);
           
   void onADCLoc(const std::shared_ptr<apollo::localization::LocalizationEstimate> &xloc);
+  
+  void onControl(
+    const std::shared_ptr<apollo::control::ControlCommand> &xCmd); 
 
   /**
    * @brief Predict the next trajectory point using perfect control model
@@ -163,6 +167,9 @@ class SimPerfectControl final : public SimControlBase {
       
    std::shared_ptr<cyber::Reader<apollo::localization::LocalizationEstimate>>
       localization_adc_reader_;
+      
+  std::shared_ptr<apollo::cyber::Reader<apollo::control::ControlCommand>>
+      control_command_reader_;
 
   // The timer to publish simulated localization and chassis messages.
   std::unique_ptr<cyber::Timer> sim_control_timer_;
@@ -205,6 +212,10 @@ class SimPerfectControl final : public SimControlBase {
   std::mutex mutex_;
   // Locks related to timer.
   std::mutex timer_mutex_;
+  
+  double fcmdsteer_ = 0.0;
+  double fcmdbrake_ = 0.0;
+  double fcmdacc_  = 0.0;
 
   FRIEND_TEST(SimControlTest, Test);
   FRIEND_TEST(SimControlTest, TestDummyPrediction);

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

@@ -321,7 +321,7 @@ void ListenGPSIMU(const char * strdata,const unsigned int nSize,const unsigned i
     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);
+    PublishLocalization(utm_x,utm_y,xgpsimu.height(),(90-xgpsimu.heading())*M_PI/180.0,xgpsimu.speed(),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;