change autoware pilot code.

src/ros2/AutoWare2025/src/adc_autowaremodule_launch/CMakeLists.txt

@@ -11,7 +11,7 @@ def generate_launch_description():
             output="screen",
             emulate_tty=True,
             parameters=[
-                {"SerialPortName": "/dev/ttyUSB0"},
+                {"SerialPortName": "/dev/ttyUART_232_C"},
                 {"UseVelocity": True}
             ]
         ),
@@ -21,7 +21,10 @@ def generate_launch_description():
             executable="adc_controller_shenlan_v2",
             name="adc_controller_shenlan_v2",
             output="screen",
-            emulate_tty=True
+            emulate_tty=True,
+            parameters=[
+                {"TorqueLim": 1000.0}
+            ]
         ),
 
         Node(

src/ros2/AutoWare2025/src/adc_autowaremodule_launch/package.xml

@@ -15,6 +15,7 @@ find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(adc_autoware_msgs REQUIRED)
 find_package(autoware_control_msgs REQUIRED)
+find_package(autoware_vehicle_msgs REQUIRED)
 find_package(Protobuf REQUIRED)
 #find_package(Qt5 COMPONENTS Core Widgets REQUIRED)
 
@@ -43,7 +44,7 @@ add_executable(${PROJECT_NAME}
 target_link_libraries(${PROJECT_NAME}  ${Protobuf_LIBRARIES})  
 
 ament_target_dependencies(${PROJECT_NAME} rclcpp std_msgs 
-  adc_autoware_msgs autoware_control_msgs )
+  adc_autoware_msgs autoware_control_msgs autoware_vehicle_msgs)
 
 install(TARGETS
   ${PROJECT_NAME}

src/ros2/AutoWare2025/src/adc_controller_shenlan_v2/LICENSE

@@ -14,7 +14,10 @@
 #include "adc_autoware_msgs/msg/adc_chassis.hpp"
 #include "autoware_control_msgs/msg/control.hpp"
 #include "adc_autoware_msgs/srv/chassis_enable_ctrl.hpp"
+#include "autoware_vehicle_msgs/msg/steering_report.hpp"
 
+
+using autoware_vehicle_msgs::msg::SteeringReport;
 #include "decition.pb.h"
 
 
@@ -43,6 +46,8 @@ private:
 
     rclcpp::Service<adc_autoware_msgs::srv::ChassisEnableCtrl>::SharedPtr srv_chassisenable;
 
+    rclcpp::Publisher<SteeringReport>::SharedPtr pub_steer_;
+
 private:
     void topic_chassis_callback(const adc_autoware_msgs::msg::AdcChassis & msg);
     void topic_cmd_callback(const autoware_control_msgs::msg::Control & msg) ;
@@ -62,7 +67,12 @@ private:
     void UnAcitvate();
     void initial();
 
+    void threadtest();
+
 private:
+
+SteeringReport current_steer_;
+
     bool gbHaveVehSpd = false;
     double gfVehSpd = 0.0;
 
@@ -83,6 +93,11 @@ private:
 
     bool mbEnableChassis = true;
 
+    std::thread * mptestthread;
+
+    const double MAXTORQUE = 1000;
+    double mfTorqueLim = 600;
+
 
 
 };

src/ros2/AutoWare2025/src/adc_controller_shenlan_v2/include/adc_controller_shenlan_v2/control_status.h

@@ -14,7 +14,7 @@
   <depend>rclcpp</depend>
   <depend>adc_autoware_msgs</depend>
   <depend>autoware_control_msgs</depend>
-
+  <depend>autoware_vehicle_msgs</depend>
   <export>
     <build_type>ament_cmake</build_type>
   </export>

src/ros2/AutoWare2025/src/adc_controller_shenlan_v2/src/adc_controller_shenlan_v2_core.cpp

@@ -87,6 +87,8 @@ void service1_enablechassis(const std::shared_ptr<adc_autoware_msgs::srv::Chassi
 
 adc_controller_shenlan_v2_core::adc_controller_shenlan_v2_core() : Node("adc_controller_shenlan_v2")
 {
+    mfTorqueLim =  declare_parameter<double >("TorqueLim",600.0);
+    std::cout<<" torque limit : "<<mfTorqueLim<<std::endl;
     gdecition_def.set_brake(0);
     gdecition_def.set_rightlamp(false);
     gdecition_def.set_leftlamp(false);
@@ -102,15 +104,21 @@ adc_controller_shenlan_v2_core::adc_controller_shenlan_v2_core() : Node("adc_con
     sub_msgs_chassis = this->create_subscription<adc_autoware_msgs::msg::AdcChassis>("chassis",10,
                         std::bind(&adc_controller_shenlan_v2_core::topic_chassis_callback,this,_1));
 
+    pub_steer_ = create_publisher<SteeringReport>("/vehicle/status/steering_status", 10);
+
     pub_msgs_cansend0 = this->create_publisher<adc_autoware_msgs::msg::AdcCanMsgs>("cansend0",10);
 
-    sub_msgs_cmd = this->create_subscription<autoware_control_msgs::msg::Control>("xx",10,
+    sub_msgs_cmd = this->create_subscription<autoware_control_msgs::msg::Control>("/control/command/control_cmd",10,
                         std::bind(&adc_controller_shenlan_v2_core::topic_cmd_callback,this,_1));
 
+
+
     srv_chassisenable = this->create_service<adc_autoware_msgs::srv::ChassisEnableCtrl>("/adc_controller_shenlan_v2/chassisenable",
                         std::bind(&adc_controller_shenlan_v2_core::service_enablechassis,this,_1,_2));
 
     mpthreadsend = new std::thread(&adc_controller_shenlan_v2_core::threadsend,this);
+
+ //   mptestthread = new std::thread(&adc_controller_shenlan_v2_core::threadtest,this);
 }
 
 adc_controller_shenlan_v2_core::~adc_controller_shenlan_v2_core()
@@ -131,6 +139,11 @@ void adc_controller_shenlan_v2_core::topic_chassis_callback(const adc_autoware_m
     gfWheelAngle = msg.angle_feedback;
     gbHaveWheelAngle = true;
 
+    current_steer_.steering_tire_angle = static_cast<double>(gfWheelAngle/(1.0* (180.0/M_PI) * 15.0));
+    current_steer_.stamp = get_clock()->now();
+
+    pub_steer_->publish(current_steer_);
+
 }
 
 void adc_controller_shenlan_v2_core::threadsend()
@@ -141,6 +154,7 @@ void adc_controller_shenlan_v2_core::threadsend()
     UnAcitvate();
  //   UnAcitvate();
 
+mbEnableChassis = true;
     int nstate = 0; //0 Un 1 Activate
 //    Activate();
     while(mbtheadrun)
@@ -160,6 +174,7 @@ void adc_controller_shenlan_v2_core::threadsend()
             {
                 if(nstate == 0)
                 {
+                    std::cout<<" activate. "<<std::endl;
                     Activate();
                     nstate = 1;
                 }
@@ -168,6 +183,7 @@ void adc_controller_shenlan_v2_core::threadsend()
             {
                 if(nstate == 1)
                 {
+                    std::cout<<" unactivate. "<<std::endl;
                     UnAcitvate();
                     nstate = 0;
                 }
@@ -194,6 +210,84 @@ void adc_controller_shenlan_v2_core::threadsend()
     std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" thread send exit."<<std::endl;
 }
 
+void adc_controller_shenlan_v2_core::threadtest()
+{
+    bool brun = true;
+    while(brun)
+    {
+    iv::brain::decition xdecition;
+    double facc = 0;
+    double ftorque = 0;
+    double fbrake = 0;
+    double fwheelangle = 0;
+
+ //   std::cout<<" receive cmd. "<<std::endl;
+
+    facc = 0;
+
+        double fVehWeight = 1800;
+     //   double fg = 9.8;
+        double fRollForce = 50;
+        const double fRatio = 2.5;
+        double fNeed = fRollForce + fVehWeight*facc;
+
+        ftorque = fNeed/fRatio;
+        fbrake = 0;
+        if(ftorque<0)
+        {
+            fbrake = facc;
+            ftorque = 0;
+        }
+
+//    facc = xctrlcmd.linear_acceleration();
+    if(ftorque >600)ftorque = 600;
+    fwheelangle = 90;
+    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(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);
+
+    xdecition.set_angle_mode(1);
+    xdecition.set_angle_active(1);
+    xdecition.set_acc_active(1);
+    xdecition.set_brake_active(1);
+//    xdecition.set_brake_type(1);
+    xdecition.set_auto_mode(3);
+
+    gMutex.lock();
+    gdecition.CopyFrom(xdecition);
+    gMutex.unlock();
+
+    gnDecitionNum = gnDecitionNumMax;
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+}
+
 void adc_controller_shenlan_v2_core::topic_cmd_callback(const autoware_control_msgs::msg::Control & msg)
 {
     (void)msg;
@@ -203,6 +297,17 @@ void adc_controller_shenlan_v2_core::topic_cmd_callback(const autoware_control_m
     double fbrake = 0;
     double fwheelangle = 0;
 
+    static int topiccmdcount = 0;
+    static int nlastprint = 0;
+    topiccmdcount++;
+    if((topiccmdcount - nlastprint)>= 100)
+    {
+        std::cout<<" receicve topic count: "<<topiccmdcount<<std::endl;
+        nlastprint = topiccmdcount;
+    }
+    
+ //   std::cout<<" receive topic cmd. "<<std::endl;
+
     facc = msg.longitudinal.acceleration;
 
         double fVehWeight = 1800;
@@ -220,10 +325,11 @@ void adc_controller_shenlan_v2_core::topic_cmd_callback(const autoware_control_m
         }
 
 //    facc = xctrlcmd.linear_acceleration();
+    if(ftorque > mfTorqueLim)ftorque = mfTorqueLim;
     fwheelangle = msg.lateral.steering_tire_angle * (180.0/M_PI) * 15.0;
     if(fwheelangle>430)fwheelangle = 430;
     if(fwheelangle<-430)fwheelangle = -430;
-    std::cout<<" acc: "<<facc<<"  wheel: "<<fwheelangle<<std::endl;
+ //   std::cout<<" acc: "<<facc<<"  wheel: "<<fwheelangle<<std::endl;
     xdecition.set_accelerator(facc);
     xdecition.set_brake(fbrake);
     xdecition.set_leftlamp(false);
@@ -251,6 +357,13 @@ void adc_controller_shenlan_v2_core::topic_cmd_callback(const autoware_control_m
     xdecition.set_door(0);
     xdecition.set_dippedlight(0);
 
+    xdecition.set_angle_mode(1);
+    xdecition.set_angle_active(1);
+    xdecition.set_acc_active(1);
+    xdecition.set_brake_active(1);
+//    xdecition.set_brake_type(1);
+    xdecition.set_auto_mode(3);
+
     gMutex.lock();
     gdecition.CopyFrom(xdecition);
     gMutex.unlock();

src/ros2/AutoWare2025/src/adc_controller_shenlan_v2/src/adc_controller_shenlan_v2_node.cpp

@@ -27,6 +27,7 @@
 #include "nav_msgs/msg/odometry.hpp"
 #include "sensor_msgs/msg/imu.hpp"
 #include "autoware_vehicle_msgs/msg/velocity_report.hpp"
+#include "autoware_vehicle_msgs/msg/control_mode_report.hpp"
 
 using geometry_msgs::msg::AccelWithCovarianceStamped;
 using geometry_msgs::msg::Pose;
@@ -38,6 +39,7 @@ using geometry_msgs::msg::TwistStamped;
 using nav_msgs::msg::Odometry;
 using sensor_msgs::msg::Imu;
 using autoware_vehicle_msgs::msg::VelocityReport;
+using autoware_vehicle_msgs::msg::ControlModeReport;
 
 class adc_gps_hcp2_core : public rclcpp::Node
 {
@@ -84,6 +86,8 @@ private:
     std::string mstrserialportname; 
     bool mbUseVelocity = true;
 
+    ControlModeReport current_control_mode_{};
+
 private:
     rclcpp::Publisher<VelocityReport>::SharedPtr pub_velocity_;
     rclcpp::Publisher<Odometry>::SharedPtr pub_odom_;
@@ -92,6 +96,7 @@ private:
     rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr pub_tf_;
 //    rclcpp::Publisher<PoseStamped>::SharedPtr pub_current_pose_;
     rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr pub_twist_;
+    rclcpp::Publisher<ControlModeReport>::SharedPtr pub_control_mode_report_;
 
 private:
     void publish_velocity(const VelocityReport & velocity);

src/ros2/AutoWare2025/src/adc_gps_hcp2/launch/adc_gps_hcp2.py

@@ -112,7 +112,8 @@ adc_gps_hcp2_core::adc_gps_hcp2_core() : Node("adc_gps_hcp2")
 
     simulated_frame_id_ = declare_parameter("simulated_frame_id", "base_link");
     origin_frame_id_ = declare_parameter("origin_frame_id", "map");
-    mstrserialportname = declare_parameter("SerialPortName","/dev/ttyTHS1");
+    mstrserialportname = declare_parameter("SerialPortName","/dev/ttyUART_232_C");
+    std::cout<<" tty Port: "<<mstrserialportname<<std::endl;
     mbUseVelocity = declare_parameter("UseVelocity",true);
 
     std::cout<<" use velocity: "<<mbUseVelocity<<std::endl;
@@ -127,6 +128,11 @@ adc_gps_hcp2_core::adc_gps_hcp2_core() : Node("adc_gps_hcp2")
     if(mbUseVelocity) pub_velocity_ = create_publisher<VelocityReport>("/vehicle/status/velocity_status", durable_qos);
     pub_acc_ = create_publisher<AccelWithCovarianceStamped>("/localization/acceleration", durable_qos);
 
+    pub_control_mode_report_ =
+        create_publisher<ControlModeReport>("/vehicle/status/control_mode",durable_qos);
+
+        current_control_mode_.mode = ControlModeReport::AUTONOMOUS;
+
     std::cout<<" Serial Port Name: "<<mstrserialportname.data()<<std::endl;
 
     QString struartdev = QString(mstrserialportname.data());
@@ -336,10 +342,8 @@ void adc_gps_hcp2_core::onTimer()
         return;
     }
 
-    std::cout<<"run hear1 "<<std::endl;
     if((QDateTime::currentMSecsSinceEpoch() - mnLastOpenTime)>=100)
     {
-        std::cout<<"run hear2."<<std::endl;
         if(m_serialPort_GPS->open(QSerialPort::ReadWrite))
         {
             mbSerialOpen = true;
@@ -554,10 +558,13 @@ void adc_gps_hcp2_core::SerialGPSDecodeSen(QString strsen)
     velocity.heading_rate = 0;
     if(mbUseVelocity)publish_velocity(velocity);
 
-    std::cout<<" publish."<<std::endl;
+ //   std::cout<<" publish."<<std::endl;
 
     publish_acceleration();
 
+      current_control_mode_.stamp = get_clock()->now();
+  pub_control_mode_report_->publish(current_control_mode_);
+
 
 }
 

src/ros2/AutoWare2025/src/adc_gps_hcp2/src/adc_gps_hcp2_node.cpp

@@ -11,7 +11,7 @@ def generate_launch_description():
             output="screen",
             emulate_tty=True,
             parameters=[
-                {"SerialPortName": "/dev/ttyUSB0"},
+                {"SerialPortName": "/dev/ttyUART_232_C"},
                 {"UseVelocity": True}
             ]
         ),
@@ -21,7 +21,10 @@ def generate_launch_description():
             executable="adc_controller_shenlan_v2",
             name="adc_controller_shenlan_v2",
             output="screen",
-            emulate_tty=True
+            emulate_tty=True,
+            parameters=[
+                {"TorqueLim": 1000.0}
+            ]
         ),
 
         Node(

src/ros2/src/adc_autowaremodule_launch/package.xml

@@ -22,7 +22,7 @@ find_package(geometry_msgs REQUIRED)
 find_package(nav_msgs REQUIRED) 
 find_package(sensor_msgs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
-find_package(autoware_auto_control_msgs REQUIRED)
+find_package(autoware_control_msgs REQUIRED)
 find_package(adc_autoware_msgs REQUIRED)
 
 find_package(Qt5 COMPONENTS Core Widgets REQUIRED)
@@ -58,10 +58,8 @@ add_executable(adc_can_nvidia_agx
 # modulecomm 
 
 ament_target_dependencies(adc_can_nvidia_agx rclcpp std_msgs geometry_msgs 
-  tf2_geometry_msgs nav_msgs sensor_msgs can_msgs autoware_auto_control_msgs autoware_auto_geometry_msgs
-  autoware_auto_mapping_msgs autoware_auto_planning_msgs autoware_auto_vehicle_msgs
-  tier4_autoware_utils  autoware_auto_perception_msgs
-  tier4_perception_msgs adc_autoware_msgs)
+  tf2_geometry_msgs nav_msgs sensor_msgs can_msgs autoware_control_msgs  Qt5Core
+ adc_autoware_msgs)
 
 
 install(TARGETS

src/ros2/src/adc_can_nvidia_agx/README.md

@@ -10,8 +10,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <depend>autoware_auto_control_msgs</depend>
-  <depend>autoware_auto_mapping_msgs</depend>
+  <depend>autoware_control_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
   <depend>autoware_auto_tf2</depend>
   <depend>autoware_auto_vehicle_msgs</depend>

src/ros2/src/adc_can_nvidia_agx/src/adc_can_nvidia_agx_core.cpp

@@ -14,7 +14,8 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(adc_autoware_msgs REQUIRED)
-find_package(autoware_auto_control_msgs REQUIRED)
+find_package(autoware_control_msgs REQUIRED)
+find_package(autoware_vehicle_msgs REQUIRED)
 find_package(Protobuf REQUIRED)
 #find_package(Qt5 COMPONENTS Core Widgets REQUIRED)
 
@@ -43,7 +44,7 @@ add_executable(${PROJECT_NAME}
 target_link_libraries(${PROJECT_NAME}  ${Protobuf_LIBRARIES})  
 
 ament_target_dependencies(${PROJECT_NAME} rclcpp std_msgs 
-  adc_autoware_msgs autoware_auto_control_msgs)
+  adc_autoware_msgs autoware_control_msgs autoware_vehicle_msgs)
 
 install(TARGETS
   ${PROJECT_NAME}

src/ros2/src/adc_controller_shenlan_v2/LICENSE

@@ -12,9 +12,12 @@
 #include "adc_autoware_msgs/msg/adc_can_frame.hpp"
 #include "adc_autoware_msgs/msg/adc_can_msgs.hpp"
 #include "adc_autoware_msgs/msg/adc_chassis.hpp"
-#include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp"
+#include "autoware_control_msgs/msg/control.hpp"
 #include "adc_autoware_msgs/srv/chassis_enable_ctrl.hpp"
+#include "autoware_vehicle_msgs/msg/steering_report.hpp"
 
+
+using autoware_vehicle_msgs::msg::SteeringReport;
 #include "decition.pb.h"
 
 
@@ -39,13 +42,15 @@ private:
 
     rclcpp::Publisher<adc_autoware_msgs::msg::AdcCanMsgs>::SharedPtr pub_msgs_cansend0;
 
-    rclcpp::Subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr sub_msgs_cmd;
+    rclcpp::Subscription<autoware_control_msgs::msg::Control>::SharedPtr sub_msgs_cmd;
 
     rclcpp::Service<adc_autoware_msgs::srv::ChassisEnableCtrl>::SharedPtr srv_chassisenable;
 
+    rclcpp::Publisher<SteeringReport>::SharedPtr pub_steer_;
+
 private:
     void topic_chassis_callback(const adc_autoware_msgs::msg::AdcChassis & msg);
-    void topic_cmd_callback(const autoware_auto_control_msgs::msg::AckermannControlCommand & msg) ;
+    void topic_cmd_callback(const autoware_control_msgs::msg::Control & msg) ;
 
     void service_enablechassis(const std::shared_ptr<adc_autoware_msgs::srv::ChassisEnableCtrl::Request> request,
           std::shared_ptr<adc_autoware_msgs::srv::ChassisEnableCtrl::Response>      response);
@@ -62,7 +67,12 @@ private:
     void UnAcitvate();
     void initial();
 
+    void threadtest();
+
 private:
+
+SteeringReport current_steer_;
+
     bool gbHaveVehSpd = false;
     double gfVehSpd = 0.0;
 
@@ -83,6 +93,11 @@ private:
 
     bool mbEnableChassis = true;
 
+    std::thread * mptestthread;
+
+    const double MAXTORQUE = 1000;
+    double mfTorqueLim = 600;
+
 
 
 };

src/ros2/src/adc_controller_shenlan_v2/include/adc_controller_shenlan_v2/control_status.h

@@ -13,8 +13,8 @@
   <test_depend>ament_lint_common</test_depend>
   <depend>rclcpp</depend>
   <depend>adc_autoware_msgs</depend>
-  <depend>autoware_auto_control_msgs</depend>
-
+  <depend>autoware_control_msgs</depend>
+  <depend>autoware_vehicle_msgs</depend>
   <export>
     <build_type>ament_cmake</build_type>
   </export>

src/ros2/src/adc_controller_shenlan_v2/src/adc_controller_shenlan_v2_core.cpp

@@ -87,6 +87,8 @@ void service1_enablechassis(const std::shared_ptr<adc_autoware_msgs::srv::Chassi
 
 adc_controller_shenlan_v2_core::adc_controller_shenlan_v2_core() : Node("adc_controller_shenlan_v2")
 {
+    mfTorqueLim =  declare_parameter<double >("TorqueLim",600.0);
+    std::cout<<" torque limit : "<<mfTorqueLim<<std::endl;
     gdecition_def.set_brake(0);
     gdecition_def.set_rightlamp(false);
     gdecition_def.set_leftlamp(false);
@@ -102,15 +104,21 @@ adc_controller_shenlan_v2_core::adc_controller_shenlan_v2_core() : Node("adc_con
     sub_msgs_chassis = this->create_subscription<adc_autoware_msgs::msg::AdcChassis>("chassis",10,
                         std::bind(&adc_controller_shenlan_v2_core::topic_chassis_callback,this,_1));
 
+    pub_steer_ = create_publisher<SteeringReport>("/vehicle/status/steering_status", 10);
+
     pub_msgs_cansend0 = this->create_publisher<adc_autoware_msgs::msg::AdcCanMsgs>("cansend0",10);
 
-    sub_msgs_cmd = this->create_subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>("xx",10,
+    sub_msgs_cmd = this->create_subscription<autoware_control_msgs::msg::Control>("/control/command/control_cmd",10,
                         std::bind(&adc_controller_shenlan_v2_core::topic_cmd_callback,this,_1));
 
+
+
     srv_chassisenable = this->create_service<adc_autoware_msgs::srv::ChassisEnableCtrl>("/adc_controller_shenlan_v2/chassisenable",
                         std::bind(&adc_controller_shenlan_v2_core::service_enablechassis,this,_1,_2));
 
     mpthreadsend = new std::thread(&adc_controller_shenlan_v2_core::threadsend,this);
+
+ //   mptestthread = new std::thread(&adc_controller_shenlan_v2_core::threadtest,this);
 }
 
 adc_controller_shenlan_v2_core::~adc_controller_shenlan_v2_core()
@@ -131,6 +139,11 @@ void adc_controller_shenlan_v2_core::topic_chassis_callback(const adc_autoware_m
     gfWheelAngle = msg.angle_feedback;
     gbHaveWheelAngle = true;
 
+    current_steer_.steering_tire_angle = static_cast<double>(gfWheelAngle/(1.0* (180.0/M_PI) * 15.0));
+    current_steer_.stamp = get_clock()->now();
+
+    pub_steer_->publish(current_steer_);
+
 }
 
 void adc_controller_shenlan_v2_core::threadsend()
@@ -141,6 +154,7 @@ void adc_controller_shenlan_v2_core::threadsend()
     UnAcitvate();
  //   UnAcitvate();
 
+mbEnableChassis = true;
     int nstate = 0; //0 Un 1 Activate
 //    Activate();
     while(mbtheadrun)
@@ -160,6 +174,7 @@ void adc_controller_shenlan_v2_core::threadsend()
             {
                 if(nstate == 0)
                 {
+                    std::cout<<" activate. "<<std::endl;
                     Activate();
                     nstate = 1;
                 }
@@ -168,6 +183,7 @@ void adc_controller_shenlan_v2_core::threadsend()
             {
                 if(nstate == 1)
                 {
+                    std::cout<<" unactivate. "<<std::endl;
                     UnAcitvate();
                     nstate = 0;
                 }
@@ -194,7 +210,85 @@ void adc_controller_shenlan_v2_core::threadsend()
     std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" thread send exit."<<std::endl;
 }
 
-void adc_controller_shenlan_v2_core::topic_cmd_callback(const autoware_auto_control_msgs::msg::AckermannControlCommand & msg)
+void adc_controller_shenlan_v2_core::threadtest()
+{
+    bool brun = true;
+    while(brun)
+    {
+    iv::brain::decition xdecition;
+    double facc = 0;
+    double ftorque = 0;
+    double fbrake = 0;
+    double fwheelangle = 0;
+
+ //   std::cout<<" receive cmd. "<<std::endl;
+
+    facc = 0;
+
+        double fVehWeight = 1800;
+     //   double fg = 9.8;
+        double fRollForce = 50;
+        const double fRatio = 2.5;
+        double fNeed = fRollForce + fVehWeight*facc;
+
+        ftorque = fNeed/fRatio;
+        fbrake = 0;
+        if(ftorque<0)
+        {
+            fbrake = facc;
+            ftorque = 0;
+        }
+
+//    facc = xctrlcmd.linear_acceleration();
+    if(ftorque >600)ftorque = 600;
+    fwheelangle = 90;
+    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(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);
+
+    xdecition.set_angle_mode(1);
+    xdecition.set_angle_active(1);
+    xdecition.set_acc_active(1);
+    xdecition.set_brake_active(1);
+//    xdecition.set_brake_type(1);
+    xdecition.set_auto_mode(3);
+
+    gMutex.lock();
+    gdecition.CopyFrom(xdecition);
+    gMutex.unlock();
+
+    gnDecitionNum = gnDecitionNumMax;
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+}
+
+void adc_controller_shenlan_v2_core::topic_cmd_callback(const autoware_control_msgs::msg::Control & msg)
 {
     (void)msg;
         iv::brain::decition xdecition;
@@ -203,6 +297,17 @@ void adc_controller_shenlan_v2_core::topic_cmd_callback(const autoware_auto_cont
     double fbrake = 0;
     double fwheelangle = 0;
 
+    static int topiccmdcount = 0;
+    static int nlastprint = 0;
+    topiccmdcount++;
+    if((topiccmdcount - nlastprint)>= 100)
+    {
+        std::cout<<" receicve topic count: "<<topiccmdcount<<std::endl;
+        nlastprint = topiccmdcount;
+    }
+    
+ //   std::cout<<" receive topic cmd. "<<std::endl;
+
     facc = msg.longitudinal.acceleration;
 
         double fVehWeight = 1800;
@@ -220,15 +325,16 @@ void adc_controller_shenlan_v2_core::topic_cmd_callback(const autoware_auto_cont
         }
 
 //    facc = xctrlcmd.linear_acceleration();
+    if(ftorque > mfTorqueLim)ftorque = mfTorqueLim;
     fwheelangle = msg.lateral.steering_tire_angle * (180.0/M_PI) * 15.0;
     if(fwheelangle>430)fwheelangle = 430;
     if(fwheelangle<-430)fwheelangle = -430;
-    std::cout<<" acc: "<<facc<<"  wheel: "<<fwheelangle<<std::endl;
+ //   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(msg.longitudinal.speed * 3.6);
+    xdecition.set_speed(msg.longitudinal.velocity * 3.6);
     xdecition.set_wheelangle(fwheelangle);
     xdecition.set_wheelspeed(300);
     xdecition.set_torque(ftorque);
@@ -251,6 +357,13 @@ void adc_controller_shenlan_v2_core::topic_cmd_callback(const autoware_auto_cont
     xdecition.set_door(0);
     xdecition.set_dippedlight(0);
 
+    xdecition.set_angle_mode(1);
+    xdecition.set_angle_active(1);
+    xdecition.set_acc_active(1);
+    xdecition.set_brake_active(1);
+//    xdecition.set_brake_type(1);
+    xdecition.set_auto_mode(3);
+
     gMutex.lock();
     gdecition.CopyFrom(xdecition);
     gMutex.unlock();

src/ros2/src/adc_controller_shenlan_v2/src/adc_controller_shenlan_v2_node.cpp

@@ -18,7 +18,7 @@ find_package(geometry_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
-find_package(autoware_auto_vehicle_msgs REQUIRED)
+find_package(autoware_vehicle_msgs REQUIRED)
 find_package(Protobuf REQUIRED)
 find_package(Qt5 COMPONENTS Core SerialPort REQUIRED)
 
@@ -40,7 +40,7 @@ add_executable(${PROJECT_NAME}
 target_link_libraries(${PROJECT_NAME}  Geographic)  
 
 ament_target_dependencies(${PROJECT_NAME} rclcpp std_msgs geometry_msgs nav_msgs sensor_msgs tf2_geometry_msgs
-  autoware_auto_vehicle_msgs
+  autoware_vehicle_msgs
   Qt5Core Qt5SerialPort)
 
 install(TARGETS

src/ros2/src/adc_gps_hcp2/LICENSE

@@ -26,7 +26,8 @@
 #include "geometry_msgs/msg/twist_stamped.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "sensor_msgs/msg/imu.hpp"
-#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
+#include "autoware_vehicle_msgs/msg/velocity_report.hpp"
+#include "autoware_vehicle_msgs/msg/control_mode_report.hpp"
 
 using geometry_msgs::msg::AccelWithCovarianceStamped;
 using geometry_msgs::msg::Pose;
@@ -37,7 +38,8 @@ using geometry_msgs::msg::Twist;
 using geometry_msgs::msg::TwistStamped;
 using nav_msgs::msg::Odometry;
 using sensor_msgs::msg::Imu;
-using autoware_auto_vehicle_msgs::msg::VelocityReport;
+using autoware_vehicle_msgs::msg::VelocityReport;
+using autoware_vehicle_msgs::msg::ControlModeReport;
 
 class adc_gps_hcp2_core : public rclcpp::Node
 {
@@ -84,6 +86,8 @@ private:
     std::string mstrserialportname; 
     bool mbUseVelocity = true;
 
+    ControlModeReport current_control_mode_{};
+
 private:
     rclcpp::Publisher<VelocityReport>::SharedPtr pub_velocity_;
     rclcpp::Publisher<Odometry>::SharedPtr pub_odom_;
@@ -92,6 +96,7 @@ private:
     rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr pub_tf_;
 //    rclcpp::Publisher<PoseStamped>::SharedPtr pub_current_pose_;
     rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr pub_twist_;
+    rclcpp::Publisher<ControlModeReport>::SharedPtr pub_control_mode_report_;
 
 private:
     void publish_velocity(const VelocityReport & velocity);
@@ -103,4 +108,4 @@ private:
 
 };
 
-#endif
+#endif

src/ros2/src/adc_gps_hcp2/launch/adc_gps_hcp2.py

@@ -14,7 +14,7 @@
   <depend>nav_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>tf2_geometry_msgs</depend>
-  <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>autoware_vehicle_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/ros2/src/adc_gps_hcp2/src/adc_gps_hcp2_core.cpp

@@ -112,7 +112,8 @@ adc_gps_hcp2_core::adc_gps_hcp2_core() : Node("adc_gps_hcp2")
 
     simulated_frame_id_ = declare_parameter("simulated_frame_id", "base_link");
     origin_frame_id_ = declare_parameter("origin_frame_id", "map");
-    mstrserialportname = declare_parameter("SerialPortName","/dev/ttyTHS1");
+    mstrserialportname = declare_parameter("SerialPortName","/dev/ttyUART_232_C");
+    std::cout<<" tty Port: "<<mstrserialportname<<std::endl;
     mbUseVelocity = declare_parameter("UseVelocity",true);
 
     std::cout<<" use velocity: "<<mbUseVelocity<<std::endl;
@@ -127,6 +128,11 @@ adc_gps_hcp2_core::adc_gps_hcp2_core() : Node("adc_gps_hcp2")
     if(mbUseVelocity) pub_velocity_ = create_publisher<VelocityReport>("/vehicle/status/velocity_status", durable_qos);
     pub_acc_ = create_publisher<AccelWithCovarianceStamped>("/localization/acceleration", durable_qos);
 
+    pub_control_mode_report_ =
+        create_publisher<ControlModeReport>("/vehicle/status/control_mode",durable_qos);
+
+        current_control_mode_.mode = ControlModeReport::AUTONOMOUS;
+
     std::cout<<" Serial Port Name: "<<mstrserialportname.data()<<std::endl;
 
     QString struartdev = QString(mstrserialportname.data());
@@ -255,6 +261,8 @@ void adc_gps_hcp2_core::ThreadRecvRTK()
 
     int fd;
 	int len, i,ret;
+	(void)i;
+	(void)ret;
  
  
 	fd = open(mstrserialportname.data(), O_RDWR | O_NOCTTY);
@@ -268,7 +276,7 @@ void adc_gps_hcp2_core::ThreadRecvRTK()
 
     char strbuf[1000];
     bool bSerialPortOpen  = true;
-    
+    (void)bSerialPortOpen;
 
 
 /*
@@ -334,10 +342,8 @@ void adc_gps_hcp2_core::onTimer()
         return;
     }
 
-    std::cout<<"run hear1 "<<std::endl;
     if((QDateTime::currentMSecsSinceEpoch() - mnLastOpenTime)>=100)
     {
-        std::cout<<"run hear2."<<std::endl;
         if(m_serialPort_GPS->open(QSerialPort::ReadWrite))
         {
             mbSerialOpen = true;
@@ -546,16 +552,19 @@ void adc_gps_hcp2_core::SerialGPSDecodeSen(QString strsen)
     publish_odometry(msg);
     publish_tf(msg);
 
-    autoware_auto_vehicle_msgs::msg::VelocityReport velocity;
+    autoware_vehicle_msgs::msg::VelocityReport velocity;
     velocity.longitudinal_velocity = sqrt(pow(vn,2)+pow(ve,2));
     velocity.lateral_velocity = 0.0F;
     velocity.heading_rate = 0;
     if(mbUseVelocity)publish_velocity(velocity);
 
-    std::cout<<" publish."<<std::endl;
+ //   std::cout<<" publish."<<std::endl;
 
     publish_acceleration();
 
+      current_control_mode_.stamp = get_clock()->now();
+  pub_control_mode_report_->publish(current_control_mode_);
+
 
 }