change for test ok.

src/apollo/change3/sim_perfect_control.cc

@@ -94,6 +94,8 @@ void SimPerfectControl::InitTimerAndIO() {
       [this](const std::shared_ptr<PredictionObstacles> &obstacles) {
         this->OnPredictionObstacles(obstacles);
       });
+      
+
 
   localization_writer_ =
       node_->CreateWriter<LocalizationEstimate>(FLAGS_localization_topic);
@@ -107,6 +109,18 @@ void SimPerfectControl::InitTimerAndIO() {
       [this](const std::shared_ptr<LocalizationEstimate> &xloc) {
         this->onADCLoc(xloc);
       });
+      
+ control_command_reader_ = node_->CreateReader<apollo::control::ControlCommand>(
+      "/apollo/control",
+      [this](const std::shared_ptr<apollo::control::ControlCommand> &xctrlcmd) {
+        this->onControl(xctrlcmd);
+      });
+      
+ chassis_reader_ = node_->CreateReader<apollo::canbus::Chassis>(
+      "/apollo/adchassis",
+      [this](const std::shared_ptr<apollo::canbus::Chassis> &xchassis) {
+        this->onADCChassis(xchassis);
+      });
   //*********完成本段
 
   // Start timer to publish localization and chassis messages.
@@ -293,6 +307,20 @@ void SimPerfectControl::OnPlanningCommand(
   return;
 }
 
+
+
+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;
+  
+  fcmdbrake_ = xCmd->brake();
+  fcmdacc_ = xCmd->throttle();
+}
+
 void SimPerfectControl::OnPredictionObstacles(
     const std::shared_ptr<PredictionObstacles> &obstacles) {
   std::lock_guard<std::mutex> lock(mutex_);
@@ -454,6 +482,10 @@ void SimPerfectControl::PublishChassis(double cur_speed,
 
   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(fchassissteer);
 
   chassis_writer_->Write(chassis);
 }
@@ -470,7 +502,7 @@ void SimPerfectControl::PublishLocalization(const TrajectoryPoint &point) {
 	  
 	  auto *pose = localization->mutable_pose();
 	  
-	  fNowVel = sqrt(pow(pose->mutable_linear_velocity()->x(),2)+pow(pose->mutable_linear_velocity()->y(),2));
+//	  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());
@@ -565,6 +597,17 @@ void SimPerfectControl::onADCLoc(const std::shared_ptr<apollo::localization::Loc
 	adcloc_ = xloc;
 	receiveadcloc = true;
 }
+
+void SimPerfectControl::onADCChassis(
+      const std::shared_ptr<apollo::canbus::Chassis   >
+          &adchassis)
+{
+	std::lock_guard<std::mutex> lock(mutex_);
+	
+	fchassissteer = adchassis->steering_percentage();
+	
+	fNowVel = adchassis->speed_mps();
+}
 //*********完成增加
 
 }  // namespace dreamview

src/apollo/change3/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"
@@ -99,6 +100,14 @@ class SimPerfectControl final : public SimControlBase {
   void OnPredictionObstacles(
       const std::shared_ptr<apollo::prediction::PredictionObstacles>
           &obstacles);
+          
+  void onControl(
+    const std::shared_ptr<apollo::control::ControlCommand> &xCmd); 
+       
+       
+  void onADCChassis(
+      const std::shared_ptr<apollo::canbus::Chassis   >
+          &adchassis);
 
   /**
    * @brief Predict the next trajectory point using perfect control model
@@ -156,8 +165,12 @@ class SimPerfectControl final : public SimControlBase {
   std::shared_ptr<cyber::Writer<apollo::localization::LocalizationEstimate>>
       localization_writer_;
   std::shared_ptr<cyber::Writer<apollo::canbus::Chassis>> chassis_writer_;
+  std::shared_ptr<cyber::Reader<apollo::canbus::Chassis>> chassis_reader_;
   std::shared_ptr<cyber::Writer<apollo::prediction::PredictionObstacles>>
       prediction_writer_;
+      
+    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_;
@@ -207,6 +220,10 @@ class SimPerfectControl final : public SimControlBase {
   std::mutex mutex_;
   // Locks related to timer.
   std::mutex timer_mutex_;
+  
+  double fcmdbrake_ ;
+  double fcmdacc_ ;
+  double fchassissteer;
 
   FRIEND_TEST(SimControlTest, Test);
   FRIEND_TEST(SimControlTest, TestDummyPrediction);

src/apollo/code/apollocontroller_shenlanfd/main.cpp

@@ -61,7 +61,7 @@ static void ExecSend()
     xmsg.set_index(gnIndex);
 
     unsigned char byte_24E[64];
-    gpShenlanfd->Get1C4(byte_24E);
+    gpShenlanfd->Get24E(byte_24E);
     xraw.set_id(0x24E);
     xraw.set_data(byte_24E,64);
     xraw.set_bext(false);
@@ -76,7 +76,7 @@ static void ExecSend()
     xmsg.set_index(gnIndex);
 
     unsigned char byte_25E[32];
-    gpShenlanfd->Get1C4(byte_25E);
+    gpShenlanfd->Get25E(byte_25E);
     xraw.set_id(0x25E);
     xraw.set_data(byte_25E,32);
     xraw.set_bext(false);