change pilot_autoware_bridge. add ctrlcmd to pilot.

src/decition/decition_fromautoware/ctrlcmd2decition.cpp

@@ -101,12 +101,14 @@ void ctrlcmd2decition::ListenCtrlCmd(const char *strdata, const unsigned int nSi
             facc = -1.0;
         }
     }
+    facc = xctrlcmd.linear_acceleration();
     if(mpLT->IsINterpolationOK())
     {
         mpLT->GetTorqueBrake(xctrlcmd.linear_velocity()*3.6,facc,ftorque,fbrake);
     }
 //    facc = xctrlcmd.linear_acceleration();
     fwheelangle = xctrlcmd.steering_angle() * (180.0/M_PI) * 15.0;
+    std::cout<<" acc: "<<facc<<"  wheel: "<<fwheelangle<<std::endl;
     xdecition.set_accelerator(facc);
     xdecition.set_brake(fbrake);
     xdecition.set_leftlamp(false);

src/ros2/src/pilot_autoware_bridge/CMakeLists.txt

@@ -49,6 +49,7 @@ add_executable(pilot_autoware_bridge
   src/pilot_autoware_bridge_node.cpp
   src/pilot_autoware_bridge_core.cpp
   ./../include/msgtype/gpsimu.pb.cc
+  ./../include/msgtype/autowarectrlcmd.pb.cc
 )
 
 target_link_libraries(pilot_autoware_bridge  ${Protobuf_LIBRARIES}  Geographic  modulecomm )
@@ -59,7 +60,6 @@ ament_target_dependencies(pilot_autoware_bridge rclcpp std_msgs geometry_msgs
   tier4_autoware_utils)
 
 
-
 install(TARGETS
 pilot_autoware_bridge
   DESTINATION lib/${PROJECT_NAME})

src/ros2/src/pilot_autoware_bridge/include/pilot_autoware_bridge/pilot_autoware_bridge_core.hpp

@@ -69,6 +69,7 @@
 #include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
 
 #include "gpsimu.pb.h"
+#include "autowarectrlcmd.pb.h"
 
 #include "modulecomm.h"
 
@@ -165,7 +166,10 @@ private:
 
   AckermannControlCommand::SharedPtr mpcmd_ptr;
 
+  bool mbcmdupdate = false;
+
   void * mpagpsimu;
+  void * mpactrlcmd;
 
 
 };

src/ros2/src/pilot_autoware_bridge/src/pilot_autoware_bridge_core.cpp

@@ -67,13 +67,17 @@ pub_steer_ = create_publisher<SteeringReport>("/vehicle/status/steering_status",
 pub_acc_ = create_publisher<AccelWithCovarianceStamped>("/localization/acceleration", durable_qos);
 
 sub_cmd_ = create_subscription<AckermannControlCommand>(
-    "/vehicle/command/manual_control_cmd", 1, [this](const AckermannControlCommand::SharedPtr msg) { mpcmd_ptr = msg; std::cout<<"new cmd. "<<std::endl;});
+    "/control/command/control_cmd", 1, [this](const AckermannControlCommand::SharedPtr msg) { mpcmd_ptr = msg;mbcmdupdate = true; std::cout<<"new cmd. "<<std::endl;});
 
   mptimer= create_wall_timer(100ms,std::bind(&pilot_autoware_bridge::callbackTimer, this));
 
+  
+
  ModuleFun fungpsimu =std::bind(&pilot_autoware_bridge::UpdateGPSIMU,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
  mpagpsimu = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",fungpsimu);
 
+ mpactrlcmd = iv::modulecomm::RegisterSend("ctrlcmd",1000,1);
+
 
 }
 
@@ -209,6 +213,31 @@ void pilot_autoware_bridge::callbackTimer()
 {
   std::cout<<" testpose. "<<std::endl;
 
+
+  if(mbcmdupdate == true)
+  {
+  const auto steer = mpcmd_ptr->lateral.steering_tire_angle;
+  const auto vel = mpcmd_ptr->longitudinal.speed;
+  const auto accel = mpcmd_ptr->longitudinal.acceleration;
+  iv::autoware::autowarectrlcmd xcmd;
+  xcmd.set_linear_acceleration(accel);
+  xcmd.set_linear_velocity(vel);
+  xcmd.set_steering_angle(steer);
+  int ndatasize = xcmd.ByteSizeLong();
+  std::shared_ptr<char> pstr_ptr = std::shared_ptr<char>(new char[ndatasize]);
+  if(xcmd.SerializeToArray(pstr_ptr.get(),ndatasize))
+  {
+    iv::modulecomm::ModuleSendMsg(mpactrlcmd ,pstr_ptr.get(),ndatasize);
+  }
+  else
+  {
+    std::cout<<" cmd serialize fail."<<std::endl;
+  }
+  std::cout<<" stear: "<<steer<<" vel: "<<vel<<" accel: "<<accel<<std::endl;
+  mbcmdupdate = false;
+  }
+  return;
+
    double pitch,roll,yaw;
     pitch = 0;
     roll = 0;