complete adc_controller_shenlan_v2. add adc_gps_hcp2, not complete.

src/ros2/src/adc_autoware_msgs/CMakeLists.txt

@@ -18,6 +18,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/AdcCanFrame.msg"
   "msg/AdcCanMsgs.msg"
   "msg/AdcChassis.msg"
+  "srv/ChassisEnableCtrl.srv"
   DEPENDENCIES std_msgs # Add packages that above messages depend on, in this case geometry_msgs for Sphere.msg
 )
 

src/ros2/src/adc_autoware_msgs/srv/ChassisEnableCtrl.srv

@@ -0,0 +1,3 @@
+bool benable
+---
+bool feedback_enable

src/ros2/src/adc_can_nvidia_agx/include/adc_can_nvidia_agx/adc_can_nvidia_agx_core.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PILOT_AUTOWARE_BRIDGE_CORE_HPP_
-#define PILOT_AUTOWARE_BRIDGE_CORE_HPP_
+#ifndef ADC_CAN_NVIDIA_AGX_CORE_HPP_
+#define ADC_CAN_NVIDIA_AGX_CORE_HPP_
 
 #include <rclcpp/rclcpp.hpp>
 

src/ros2/src/adc_controller_shenlan_v2/CMakeLists.txt

@@ -16,6 +16,7 @@ find_package(std_msgs REQUIRED)
 find_package(adc_autoware_msgs REQUIRED)
 find_package(autoware_auto_control_msgs REQUIRED)
 find_package(Protobuf REQUIRED)
+#find_package(Qt5 COMPONENTS Core Widgets REQUIRED)
 
 # uncomment the following section in order to fill in
 # further dependencies manually.

src/ros2/src/adc_controller_shenlan_v2/include/adc_controller_shenlan_v2/adc_controller_shenlan_v2_core.hpp

@@ -13,6 +13,7 @@
 #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 "adc_autoware_msgs/srv/chassis_enable_ctrl.hpp"
 
 #include "decition.pb.h"
 
@@ -40,10 +41,15 @@ private:
 
     rclcpp::Subscription<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr sub_msgs_cmd;
 
+    rclcpp::Service<adc_autoware_msgs::srv::ChassisEnableCtrl>::SharedPtr srv_chassisenable;
+
 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 service_enablechassis(const std::shared_ptr<adc_autoware_msgs::srv::ChassisEnableCtrl::Request> request,
+          std::shared_ptr<adc_autoware_msgs::srv::ChassisEnableCtrl::Response>      response);
+
 private:
     void threadsend();
     std::thread * mpthreadsend;
@@ -75,6 +81,8 @@ private:
     double gfWheelSpeedLim = 300; //300 degrees per second
     std::mutex gMutex;
 
+    bool mbEnableChassis = true;
+
 
 
 };

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

@@ -3,6 +3,7 @@
 #include <functional>
 
 using std::placeholders::_1;
+using std::placeholders::_2;
 
 // signal: @ACC_LatAngReq    //更改CANid
 #define ECU_1C4_ACC_LatAngReq_CovFactor (0.1)
@@ -78,6 +79,11 @@ ECU_24E_t _m24E = {0,0,0,0,0,0,0,0,0,0,0};
 ECU_25E_t _m25E = {0,0,0,0};
 
 
+void service1_enablechassis(const std::shared_ptr<adc_autoware_msgs::srv::ChassisEnableCtrl::Request> request,
+          std::shared_ptr<adc_autoware_msgs::srv::ChassisEnableCtrl::Response>      response)
+{
+    response->feedback_enable = request->benable;
+}
 
 adc_controller_shenlan_v2_core::adc_controller_shenlan_v2_core() : Node("adc_controller_shenlan_v2")
 {
@@ -91,11 +97,19 @@ adc_controller_shenlan_v2_core::adc_controller_shenlan_v2_core() : Node("adc_con
     gdecition_def.set_acc_active(0);
 //    gdecition_def.set_brake_active(1);
     gdecition_def.set_brake_type(0);
-    gdecition_def.set_auto_mode(0);
+    gdecition_def.set_auto_mode(0); 
 
     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_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,
+                        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);
 }
 
@@ -142,10 +156,21 @@ void adc_controller_shenlan_v2_core::threadsend()
         }
         else
         {
-            if(nstate == 0)
+            if(mbEnableChassis)
             {
-                Activate();
-                nstate = 1;
+                if(nstate == 0)
+                {
+                    Activate();
+                    nstate = 1;
+                }
+            }
+            else
+            {
+                if(nstate == 1)
+                {
+                    UnAcitvate();
+                    nstate = 0;
+                }
             }
             gMutex.lock();
             xdecition.CopyFrom(gdecition);
@@ -501,6 +526,8 @@ void adc_controller_shenlan_v2_core::ExecSend()
     xmsg.header.stamp.sec = nnow/1e9;
     xmsg.header.stamp.nanosec = nnow - xraw.header.stamp.sec * 1e9;
 
+ //   std::cout<<" pub msg. "<<std::endl;
+
     pub_msgs_cansend0->publish(xmsg);
 
 }
@@ -589,6 +616,24 @@ void adc_controller_shenlan_v2_core::initial()
 }
 
 
+//ros2 service call /adc_controller_shenlan_v2/chassisenable adc_autoware_msgs/srv/ChassisEnableCtrl "{benable: true}"
+//ros2 service call /adc_controller_shenlan_v2/chassisenable adc_autoware_msgs/srv/ChassisEnableCtrl "{benable: false}"
+void adc_controller_shenlan_v2_core::service_enablechassis(const std::shared_ptr<adc_autoware_msgs::srv::ChassisEnableCtrl::Request> request,
+          std::shared_ptr<adc_autoware_msgs::srv::ChassisEnableCtrl::Response>      response)
+{
+    response->feedback_enable = request->benable;
+    mbEnableChassis = request->benable;
+    if(mbEnableChassis)
+    {
+        std::cout<<"service enable chassis."<<std::endl;
+    }
+    else
+    {
+        std::cout<<"service disable chassis."<<std::endl;
+    }
+}
+
+
 
 
 

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

@@ -11,6 +11,10 @@ int main(int argc, char ** argv)
   rclcpp::init(argc, argv);
 
   rclcpp::spin(std::make_shared<adc_controller_shenlan_v2_core>());
+
+  std::cout<<" shut. "<<std::endl;
+  
+
   rclcpp::shutdown();
 
   return 0;

src/ros2/src/adc_gps_hcp2/CMakeLists.txt

@@ -0,0 +1,52 @@
+cmake_minimum_required(VERSION 3.8)
+project(adc_gps_hcp2)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> 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(Protobuf REQUIRED)
+find_package(Qt5 COMPONENTS Core SerialPort REQUIRED)
+
+include_directories(
+##  INCLUDE_DIRS include
+  ${CMAKE_CURRENT_BINARY_DIR}/..
+  include
+  include/adc_gps_hcp2
+)
+
+#message(ERROR "catkin_INCLUDE_DIRS: ${catkin_INCLUDE_DIRS}")
+#message(STATUS "CMAKE_CURRENT_BINARY_DIR: ${CMAKE_CURRENT_BINARY_DIR}")
+
+add_executable(${PROJECT_NAME}
+  src/adc_gps_hcp2_node.cpp
+  src/adc_gps_hcp2_core.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}  Geographic)  
+
+ament_target_dependencies(${PROJECT_NAME} rclcpp std_msgs 
+  Qt5Core Qt5SerialPort)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/ros2/src/adc_gps_hcp2/LICENSE

@@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

src/ros2/src/adc_gps_hcp2/include/adc_gps_hcp2/adc_gps_hcp2_core.hpp

@@ -0,0 +1,52 @@
+
+#ifndef ADC_GPS_HCP2_CORE_HPP_
+#define ADC_GPS_HCP2_CORE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+//#include <QObject>
+#include <QThread>
+#include <QDateTime>
+#include <QTimer>
+#include <iostream>
+
+#include <QSerialPort>
+
+
+class adc_gps_hcp2_core : public rclcpp::Node
+{
+public:
+    adc_gps_hcp2_core();
+    ~adc_gps_hcp2_core() = default;
+
+private:
+    void onTimer();
+
+
+private:
+    rclcpp::TimerBase::SharedPtr mptimer;
+
+
+private:
+    QByteArray mbaGPSBuf;
+    bool mbSerialOpen;
+    QSerialPort *m_serialPort_GPS;
+    int mnNotRecvCount;
+    qint64 mnLastOpenTime;
+
+    QString mstrHCP2;
+
+    double mfOldVel;
+    qint64 mOldTime;
+    double mfCalc_acc;
+    QTime mTime;
+
+private:
+    void SerialGPSDecode();
+    void SerialGPSDecodeSen(QString strsen);
+
+    unsigned char calccheck(unsigned char * p);
+
+};
+
+#endif

src/ros2/src/adc_gps_hcp2/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>adc_gps_hcp2</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="yuchuli@catarc.ac.cn">yuchuli</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  
+  <depend>rclcpp</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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

@@ -0,0 +1,272 @@
+
+#include "adc_gps_hcp2_core.hpp"
+
+
+
+using namespace std;
+
+static bool checknmeasen(const char * strsen,const unsigned int nlen)
+{
+    if(nlen< 4)return false;
+
+    int i;
+    char check;
+    int nstarpos = -1;
+    check = strsen[1]^strsen[2];
+    for(i=3;i<static_cast<int>(nlen);i++)
+    {
+        if(strsen[i] == '*')
+        {
+            nstarpos = i;
+            break;
+        }
+        check = check^strsen[i];
+    }
+    if(nstarpos < 0)return false;
+    if(nstarpos >(static_cast<int>(nlen) -3))return false;
+    char strcheck[3];
+    int ncheck = check;
+    snprintf(strcheck,3,"%02X",ncheck);
+    char strsencheck[3];
+    strsencheck[2] = 0;
+    strsencheck[0] = strsen[nstarpos + 1];
+    strsencheck[1] = strsen[nstarpos + 2];
+    if(strncmp(strcheck,strsencheck,2) == 0)
+    {
+   //     qDebug("  r sen is %s",strsen);
+        return true;
+    }
+
+    std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" cal check is "<<strcheck<<" sen check is "<<strsencheck<<std::endl;
+
+//    qDebug("  sen is %s",strsen);
+    return false;
+}
+
+adc_gps_hcp2_core::adc_gps_hcp2_core() : Node("adc_gps_hcp2")
+{
+    mTime.start();
+    mfCalc_acc = 0;
+    mfOldVel = 0;
+    mOldTime = 0;
+
+    mbSerialOpen = false;
+    mbaGPSBuf.clear();
+    mnNotRecvCount = 0;
+    mnLastOpenTime = 0;
+
+
+    QString struartdev = "/dev/ttyUSB0";
+    m_serialPort_GPS = new QSerialPort();
+    m_serialPort_GPS->setPortName(struartdev);
+    m_serialPort_GPS->setBaudRate(230400);
+    m_serialPort_GPS->setParity(QSerialPort::NoParity);
+    m_serialPort_GPS->setDataBits(QSerialPort::Data8);
+    m_serialPort_GPS->setStopBits(QSerialPort::OneStop);
+    m_serialPort_GPS->setFlowControl(QSerialPort::NoFlowControl);
+    m_serialPort_GPS->setReadBufferSize(0);
+
+    mptimer = create_wall_timer(1ms,std::bind(&adc_gps_hcp2_core::onTimer,this));
+}
+
+void adc_gps_hcp2_core::onTimer()
+{
+    if(mbSerialOpen)
+    {
+        QByteArray ba = m_serialPort_GPS->readAll();
+
+        if(ba.length() > 0)
+        {
+            mstrHCP2.append(ba);
+            SerialGPSDecode();
+            mnNotRecvCount = 0;
+            //Decode
+        }
+        else
+        {
+            mnNotRecvCount++;
+            if(mnNotRecvCount > 1000)
+            {
+                m_serialPort_GPS->close();
+                mbSerialOpen = false;
+            }
+        }
+        return;
+    }
+
+    if((QDateTime::currentMSecsSinceEpoch() - mnLastOpenTime)>=100)
+    {
+        if(m_serialPort_GPS->open(QSerialPort::ReadWrite))
+        {
+            mbSerialOpen = true;
+        }
+        mnLastOpenTime = QDateTime::currentMSecsSinceEpoch();
+    }
+}
+
+
+
+unsigned char adc_gps_hcp2_core::calccheck(unsigned char *p)
+{
+    unsigned char rtn;
+    rtn = p[0]^p[1];
+    int i;
+    for(i=2;i<=56;i++)
+    {
+        rtn = rtn ^ p[i];
+    }
+    return rtn;
+}
+
+void adc_gps_hcp2_core::SerialGPSDecode()
+{
+    int xpos = mstrHCP2.indexOf('\n');
+    if(xpos>=0)
+    {
+
+        QString xsen = mstrHCP2.left(xpos+1);
+        SerialGPSDecodeSen(xsen);
+        mstrHCP2.remove(0,xpos+1);
+        SerialGPSDecode();
+    }
+}
+
+void adc_gps_hcp2_core::SerialGPSDecodeSen(QString strsen)
+{
+    QStringList strlistrmc;
+    strlistrmc = strsen.split(",");
+
+   if(strlistrmc.size() < 23)return;
+      if(strlistrmc.at(0) != "$GPCHC")return;
+
+   if(!checknmeasen(strsen.toLatin1().data(),strsen.length()))
+   {
+       int nPos = strsen.indexOf('$',10);
+       if(nPos > 0)
+       {
+           QString strnewsen = strsen.right(strsen.size()-nPos);
+//           qDebug("new sen is %s",strnewsen.toLatin1().data());
+           SerialGPSDecodeSen(strnewsen);
+       }
+       return;
+   }
+
+   double fheading,fLat,fLon,fVel,fPitch,fRoll;
+   double fHgt,gyro_x,gyro_y,gyro_z,acc_x,acc_y,acc_z;
+   int nsv1,nsv2;
+   int gpsweek,gpstime;
+   int insstate,rtkstate;
+   QString strx = strlistrmc.at(3);
+   fheading = strx.toDouble();
+
+   strx = strlistrmc.at(1);
+   gpsweek = strx.toInt();
+
+   strx = strlistrmc.at(2);
+   gpstime = strx.toInt();
+
+   strx = strlistrmc.at(12);
+   fLat = strx.toDouble();
+
+   strx  = strlistrmc.at(13);
+   fLon = strx.toDouble();
+
+   strx = strlistrmc.at(14);
+   fHgt = strx.toDouble();
+
+   double ve,vn,vu;
+   strx = strlistrmc.at(15);
+   ve = strx.toDouble();
+
+   strx = strlistrmc.at(16);
+   vn = strx.toDouble();
+
+   strx = strlistrmc.at(17);
+   vu = strx.toDouble();
+
+//   fVel = sqrt(ve*ve + vn*vn + vu*vu);
+   strx = strlistrmc.at(18);
+   fVel = strx.toDouble();
+
+   if((mTime.elapsed()-mOldTime) >= 100)
+   {
+       if(mOldTime == 0)
+       {
+           mfCalc_acc = 0;
+           mfOldVel = fVel;
+           mOldTime = mTime.elapsed();
+       }
+       else
+       {
+           mfCalc_acc = (fVel - mfOldVel)/((mTime.elapsed() - mOldTime)*0.001);
+           mfOldVel = fVel;
+           mOldTime = mTime.elapsed();
+       }
+   }
+
+   strx = strlistrmc.at(4);
+   fPitch = strx.toDouble();
+
+   strx = strlistrmc.at(5);
+   fRoll = strx.toDouble();
+
+   strx = strlistrmc.at(6);
+   gyro_x = strx.toDouble();
+
+   strx = strlistrmc.at(7);
+   gyro_y = strx.toDouble();
+
+   strx = strlistrmc.at(8);
+   gyro_z = strx.toDouble();
+
+   strx = strlistrmc.at(9);
+   acc_x = strx.toDouble();
+
+   strx = strlistrmc.at(10);
+   acc_y = strx.toDouble();
+
+   strx = strlistrmc.at(11);
+   acc_z = strx.toDouble();
+
+   strx = strlistrmc.at(19);
+   nsv1 = strx.toInt();
+
+   strx = strlistrmc.at(20);
+   nsv2 = strx.toInt();
+
+   strx = strlistrmc.at(21);
+   char strstate[3];
+   strstate[2] = 0;
+   if(strx.size() >= 2)
+   {
+
+       memcpy(strstate,strx.data(),2);
+  //     qDebug(strstate);
+       char xstate;
+       xstate = strstate[0];
+       rtkstate = 4;
+       int xs;
+       if(xstate == '0')xs = 0;
+       if(xstate == '1')xs = 3;
+       if(xstate == '2')xs = 4;
+       if(xstate == '3')xs = 8;
+       if(xstate == '4')xs = 6;
+       if(xstate == '5')xs = 5;
+       if(xstate == '6')xs = 3;
+       if(xstate == '7')xs = 4;
+       if(xstate == '8')xs = 5;
+       if(xstate == '9')xs = 5;
+
+       rtkstate = xs;
+//       if(mstate == 0)minsstate = 0;
+//       else minsstate = 4;
+
+       xstate = strstate[1];
+       if(xstate == '0')insstate = 3;
+       else insstate = 4;
+       if(rtkstate == 0)insstate = 3;
+   }
+
+
+}
+

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

@@ -0,0 +1,19 @@
+#include <rclcpp/rclcpp.hpp>
+
+#include <memory>
+
+//#include "adc_controller_shenlan_v2/adc_controller_shenlan_v2_core.hpp"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+
+//  rclcpp::spin(std::make_shared<adc_controller_shenlan_v2_core>());
+
+  std::cout<<" shut. "<<std::endl;
+  
+
+  rclcpp::shutdown();
+
+  return 0;
+}