change apollocontroller_shenlanfd. add chassis feedback.

src/apollo/code/apollocontroller_shenlanfd/main.cpp

@@ -12,6 +12,7 @@
 #include "adchassis.pb.h"
 #include "canmsg.pb.h"
 #include "modules/common_msgs/control_msgs/control_cmd.pb.h"
+#include "modules/common_msgs/chassis_msgs/chassis.pb.h"
 
 #include "gflags/gflags.h"
 
@@ -35,6 +36,7 @@ static std::shared_ptr<apollo::cyber::Reader<iv::chassis>>
         chassis_reader_;
 static std::shared_ptr<apollo::cyber::Reader<apollo::control::ControlCommand>>
         controller_reader_;
+std::shared_ptr<apollo::cyber::Writer<apollo::canbus::Chassis>> apollochassis_writer_;
 
 static void ExecSend()
 {
@@ -119,7 +121,15 @@ void RecvCANMsg(const std::shared_ptr<iv::can::canmsg> &xmsg)
 
 void RecvChassis(const std::shared_ptr<iv::chassis> &xchassis)
 {
+    
     gpShenlanfd->SetSpeed(xchassis->vel());
+
+    std::shared_ptr<apollo::canbus::Chassis> apollochassis_ptr;
+    apollo::canbus::Chassis * papollochassis = new apollo::canbus::Chassis();
+    papollochassis->set_steering_percentage(xchassis->angle_feedback()*100.0/430.0);
+    papollochassis->set_speed_mps(xchassis->vel()/3.6);
+    apollochassis_ptr.reset(papollochassis);
+    apollochassis_writer_->Write(apollochassis_ptr);
 }
 
 void RecvController(const std::shared_ptr<apollo::control::ControlCommand> &xcmd)
@@ -216,6 +226,8 @@ int main(int argc, char *argv[])
         }
     );
 
+    apollochassis_writer_ = pilot_node->CreateWriter<apollo::canbus::Chassis>("/apollo/adchassis");
+
     controller_reader_ = pilot_node->CreateReader<apollo::control::ControlCommand>(
         FLAGS_controller_msg_name,
         [](const std::shared_ptr<apollo::control::ControlCommand> &xcmd){