test ros2 adc_can_nvidia_agx adc_chassis ok. adc_gps_hcp2 can receive data.

src/ros2/src/adc_can_nvidia_agx/src/canctrl.cpp

@@ -160,8 +160,10 @@ void canctrl::sharecanmsg(rclcpp::Publisher<adc_autoware_msgs::msg::AdcCanMsgs>:
         {
             std::cout<<" convert canmsg to ros2 message fail, because data len more than 64. len:"<<pxmsg[i].nLen<<std::endl;
         }
+        message.frames.push_back(frame1);
     }
 
+  //  std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" ch: "<<nch<<"  publish msg . size: "<<ncount<<std::endl;
     pubcanrecv->publish(message);
 }
 

src/ros2/src/adc_chassis/src/adc_chassis_core.cpp

@@ -22,10 +22,15 @@ void adc_chassis_core::topic_canrecv0_callback(const adc_autoware_msgs::msg::Adc
     static bool bhaveang = false;
     static double vehspeed = 0;
     static double ang = 0;
+
+ //   std::cout<<" size : "<<msg.frames.size()<<std::endl;
+    
     for(i=0;i<ncount;i++)
     {
         unsigned char bytedata[64];
 
+//        std::cout<<" frame id:"<<msg.frames[i].id<<std::endl;
+
         if(msg.frames[i].id == 0x1CC)
         {
             memcpy(bytedata,msg.frames[i].data.data(),64);
@@ -58,6 +63,7 @@ void adc_chassis_core::topic_canrecv0_callback(const adc_autoware_msgs::msg::Adc
         msgchassis.header.stamp.nanosec = nnow - msgchassis.header.stamp.sec * 1e9;
         msgchassis.vel = vehspeed;
         msgchassis.angle_feedback = ang;
+        std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" vel: "<<msgchassis.vel<<" ang: "<<msgchassis.angle_feedback<<std::endl;
         pub_msgs_chassis->publish(msgchassis);
         bhaveang = false;
         bhavevel = false;

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

@@ -12,6 +12,8 @@
 
 #include <QSerialPort>
 
+#include <thread>
+
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
@@ -66,6 +68,9 @@ private:
 
     double mfacc;
 
+    std::thread * mpthread;
+    bool mbthreadrun = true;
+
 private:
     void SerialGPSDecode();
     void SerialGPSDecodeSen(QString strsen);
@@ -94,6 +99,8 @@ private:
     void publish_acceleration();
     void publish_tf(const Odometry & odometry);
 
+    void ThreadRecvRTK();
+
 };
 
 #endif

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

@@ -112,7 +112,7 @@ 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/ttyUSB0");
+    mstrserialportname = declare_parameter("SerialPortName","/dev/ttyTHS1");
     mbUseVelocity = declare_parameter("UseVelocity",true);
 
     std::cout<<" use velocity: "<<mbUseVelocity<<std::endl;
@@ -139,7 +139,174 @@ adc_gps_hcp2_core::adc_gps_hcp2_core() : Node("adc_gps_hcp2")
     m_serialPort_GPS->setFlowControl(QSerialPort::NoFlowControl);
     m_serialPort_GPS->setReadBufferSize(0);
 
-    mptimer = create_wall_timer(1ms,std::bind(&adc_gps_hcp2_core::onTimer,this));
+    mpthread = new std::thread(&adc_gps_hcp2_core::ThreadRecvRTK,this);
+
+//    mptimer = create_wall_timer(1ms,std::bind(&adc_gps_hcp2_core::onTimer,this));
+}
+
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+//#include <asm/termios.h>
+#include <termios.h>
+
+static void set_baudrate (struct termios *opt, unsigned int baudrate)
+{
+	cfsetispeed(opt, baudrate);
+	cfsetospeed(opt, baudrate);
+}
+
+static void set_data_bit (struct termios *opt, unsigned int databit)
+{
+    opt->c_cflag &= ~CSIZE;
+    switch (databit) {
+    case 8:
+        opt->c_cflag |= CS8;
+        break;
+    case 7:
+        opt->c_cflag |= CS7;
+        break;
+    case 6:
+        opt->c_cflag |= CS6;
+        break;
+    case 5:
+        opt->c_cflag |= CS5;
+        break;
+    default:
+        opt->c_cflag |= CS8;
+        break;
+    }
+
+}
+
+static void set_parity (struct termios *opt, char parity)
+{
+    switch (parity) {
+    case 'N':                  /* no parity check */
+        opt->c_cflag &= ~PARENB;
+        break;
+    case 'E':                  /* even */
+        opt->c_cflag |= PARENB;
+        opt->c_cflag &= ~PARODD;
+        break;
+    case 'O':                  /* odd */
+        opt->c_cflag |= PARENB;
+        opt->c_cflag |= ~PARODD;
+        break;
+    default:                   /* no parity check */
+        opt->c_cflag &= ~PARENB;
+        break;
+    }
+}
+
+static void set_stopbit (struct termios *opt, const char *stopbit)
+{
+    if (0 == strcmp (stopbit, "1")) {
+        opt->c_cflag &= ~CSTOPB; /* 1 stop bit */
+    }	else if (0 == strcmp (stopbit, "1")) {
+        opt->c_cflag &= ~CSTOPB; /* 1.5 stop bit */
+    }   else if (0 == strcmp (stopbit, "2")) {
+        opt->c_cflag |= CSTOPB;  /* 2 stop bits */
+    } else {
+        opt->c_cflag &= ~CSTOPB; /* 1 stop bit */
+    }
+}
+
+//https://blog.csdn.net/morixinguan/article/details/80898172
+//串口设置
+int  set_port_attr (
+              int fd,
+              int  baudrate,          // B1200 B2400 B4800 B9600 .. B115200
+              int  databit,           // 5, 6, 7, 8
+              const char *stopbit,    //  "1", "1.5", "2"
+              char parity,            // N(o), O(dd), E(ven)
+              int vtime,
+              int vmin )
+{
+     struct termios opt;
+     tcgetattr(fd, &opt);
+     //设置波特率
+     set_baudrate(&opt, baudrate);
+     opt.c_cflag 		 |= CLOCAL | CREAD;      /* | CRTSCTS */
+     //设置数据位
+     set_data_bit(&opt, databit);
+     //设置校验位
+     set_parity(&opt, parity);
+     //设置停止位
+     set_stopbit(&opt, stopbit);
+     //其它设置
+     opt.c_oflag 		 = 0;
+     opt.c_lflag            	|= 0;
+     opt.c_oflag          	&= ~OPOST;
+     opt.c_cc[VTIME]     	 = vtime;
+     opt.c_cc[VMIN]         	 = vmin;
+     tcflush (fd, TCIFLUSH);
+     return (tcsetattr (fd, TCSANOW, &opt));
+}
+
+
+void adc_gps_hcp2_core::ThreadRecvRTK()
+{
+    std::cout<<" enter thread "<<std::endl;
+
+
+
+    int fd;
+	int len, i,ret;
+ 
+ 
+	fd = open(mstrserialportname.data(), O_RDWR | O_NOCTTY);
+        if(fd < 0) {
+                perror(mstrserialportname.data());
+                return;
+        }
+
+    set_port_attr(fd,B230400,8,"1",'N',0,0);
+ 
+
+    char strbuf[1000];
+    bool bSerialPortOpen  = true;
+    
+
+
+/*
+    bool bSerialPortOpen = false;
+    std::cout<<" open "<<std::endl;
+     if(m_serialPort_GPS->open(QSerialPort::ReadWrite))
+    {
+        bSerialPortOpen = true;
+        std::cout<<" succ "<<std::endl;
+    }
+    else
+    {
+        std::cout<<" Open Port "<<gstrserialportname<<"  Fail."<<std::endl;
+        return;
+    }
+    */
+
+    while(mbthreadrun)
+    {
+  //      QByteArray ba = m_serialPort_GPS->readAll();
+
+        len = read(fd, strbuf, 999);
+        if(len > 0)
+        {
+            strbuf[len] = 0;
+     //       std::cout<<strbuf<<std::endl;
+     //       std::cout<<" ba len : "<<len<<std::endl;
+            mstrHCP2.append(strbuf);
+            SerialGPSDecode();
+         //   mnNotRecvCount = 0;
+            //Decode
+        }
+        else
+        {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+ //           std::cout<<" no data"<<std::endl;
+        }
+    }
 }
 
 void adc_gps_hcp2_core::onTimer()
@@ -167,11 +334,18 @@ 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;
+            std::cout<<" open port successfully."<<std::endl;
+        }
+        else
+        {
+            std::cout<<" open port fail."<<std::endl;
         }
         mnLastOpenTime = QDateTime::currentMSecsSinceEpoch();
     }
@@ -378,6 +552,8 @@ void adc_gps_hcp2_core::SerialGPSDecodeSen(QString strsen)
     velocity.heading_rate = 0;
     if(mbUseVelocity)publish_velocity(velocity);
 
+    std::cout<<" publish."<<std::endl;
+
     publish_acceleration();