change controllertocan driver_can_kvaser rtk_hcp2. all ok.run with autoware.

src/driver/driver_gps_hcp2/hcp2.cpp

@@ -444,7 +444,7 @@ unsigned char hcp2::calccheck(unsigned char *p)
 void hcp2::SerialGPSDecode()
 {
     int xpos = mstrHCP2.indexOf('\n');
-    if(xpos>0)
+    if(xpos>=0)
     {
 
         QString xsen = mstrHCP2.left(xpos+1);

src/ros/catkin/src/controllertocan/src/ge3code.cpp

@@ -3,6 +3,14 @@
 
 GE3Code::GE3Code(std::string strtablepath)
 {
+    ServiceControlStatus.command.byte[0] = 0xEB;
+    ServiceControlStatus.command.byte[1] = 0;
+    ServiceControlStatus.command.byte[2] = 0;
+    ServiceControlStatus.command.byte[3] = 0;
+    ServiceControlStatus.command.byte[4] = 0;
+    ServiceControlStatus.command.byte[5] = 0;
+    ServiceControlStatus.command.byte[6] = 0;
+    ServiceControlStatus.command.byte[7] = 0;
     mpLT = new LookupTable(strtablepath);
 }
 

src/ros/catkin/src/controllertocan/src/main.cpp

@@ -29,7 +29,7 @@ static std::string _ctrlcmd_topic = "/ctrl_raw";
 static std::string _canmsg_topic = "/can_msg";
 
 static std::string gstrvehtype = "GE3";
-static std::string gstrtablepath = "/home/yuchuli/velacctable.txt";
+static std::string gstrtablepath = "/home/nvidia/velacctable.txt";
 
 ros::Subscriber ctrlcmd_sub;
 ros::Subscriber twist_sub;
@@ -57,7 +57,7 @@ void ctrlrawCallback(const autoware_msgs::ControlCommandStampedConstPtr &msg)
 
 int main(int argc, char *argv[])
 {
-	ros::init(argc, argv, "pilottoros");
+	ros::init(argc, argv, "controllertocan");
 //	ros::NodeHandle n;
     ros::NodeHandle private_nh("~");
 	ros::Rate loop_rate(100);
@@ -88,7 +88,7 @@ canmsg_pub = private_nh.advertise<adc_system_msg::canmsg>(_canmsg_topic,10);
 ctrlcmd_sub = private_nh.subscribe(_ctrlcmd_topic, 1, ctrlrawCallback);
 twist_sub = private_nh.subscribe(_twistraw_topic, 1, twistrawCalllback);
 
-
+//std::cout<<" go hear. "<<std::endl;
 ros::spin();
 
 }

src/ros/catkin/src/driver_can_kvaser/src/canctrl.cpp

@@ -28,7 +28,7 @@ static int getbaud(const char * strbaud)
 
 
 canctrl::canctrl(const char * strmemsend0,const char * strmemsend1,const char * strmemrecv0,const char * strmemrecv1,
-                 const char *strmemsend_canstate,const char * strdevnum,const char * strbaud0, const char * strbaud1)
+                 const char *strmemsend_canstate,const char * strdevnum,const char * strbaud0, const char * strbaud1):nh_("~")
 {
 
     ros::Rate loop_rate(100);
@@ -57,11 +57,15 @@ canctrl::canctrl(const char * strmemsend0,const char * strmemsend1,const char *
     mbRun = true;
     mpthread = new std::thread(&canctrl::run,this);
 
-    msub1 =  nh_.subscribe(mstrmemsend0,10,&canctrl::CallbackSend1,this);
-    msub2 =  nh_.subscribe(mstrmemsend1,10,&canctrl::CallbackSend2,this);
+    std::cout<<" go hear . "<<std::endl;
+
 
     mpub1 = nh_.advertise<adc_system_msg::canmsg>(mstrmemrecv0,1);
     mpub2 = nh_.advertise<adc_system_msg::canmsg>(mstrmemrecv1,1);
+    
+    msub1 =  nh_.subscribe(mstrmemsend0,10,&canctrl::CallbackSend1,this);
+    msub2 =  nh_.subscribe(mstrmemsend1,10,&canctrl::CallbackSend2,this);
+
 }
 
 canctrl::~canctrl()

src/ros/catkin/src/driver_can_kvaser/src/kvasercan.cpp

@@ -186,7 +186,9 @@ void kvasercan::run()
  //               qDebug("%ld ch  is %d ",QDateTime::currentMSecsSinceEpoch(),nch);
 //                qDebug("%02x %02x %02x %02x ",xMsgSendBuf[j].data[0],xMsgSendBuf[j].data[1],xMsgSendBuf[j].data[3],
 //                        xMsgSendBuf[j].data[4]);
+                std::cout<<nch<<" id : "<<xMsgSendBuf[j].id<<std::endl;
                 canWrite(mkvaserch[nch],xMsgSendBuf[j].id,(void *)xMsgSendBuf[j].data,xMsgSendBuf[j].nLen,canMSG_STD);
+                std::cout<<" send can msg "<<std::endl;
 //                canFlushTransmitQueue(mkvaserch[nch]);
 //                qDebug("time is %ld",QDateTime::currentMSecsSinceEpoch());
             }

src/ros/catkin/src/driver_can_kvaser/src/main.cpp

@@ -21,12 +21,12 @@ int main(int argc, char *argv[])
 {
 	ros::init(argc, argv, "driver_can_kvaser");
 //	ros::NodeHandle n;
-    ros::NodeHandle private_nh("~");
-	ros::Rate loop_rate(100);
+ //   ros::NodeHandle private_nh("~");
+//	ros::Rate loop_rate(100);
     
 
-canctrl  * pcanctrl = new canctrl("cansen0","cansend1","canrecv0","canrecv1","can_state");
-
+//canctrl  * pcanctrl = new canctrl("cansen0","cansend1","canrecv0","canrecv1","can_state");
+canctrl  xcanctrl("/can_msg","cansend1","canrecv0","canrecv1","can_state");
 ros::spin();
 
 }

src/ros/catkin/src/rtk_hcp2/CMakeLists.txt

@@ -104,4 +104,3 @@ include_directories(
 ## Declare a C++ executable
 add_executable(${PROJECT_NAME}_node src/main.cpp  src/gnss_coordinate_convert.cpp)
 target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES}  ${OpenCV_LIBRARIES}    ${Protobuf_LIBRARIES}   Qt5Core Qt5SerialPort)
-

src/ros/catkin/src/rtk_hcp2/src/main.cpp

@@ -1,6 +1,14 @@
 #include "ros/ros.h"
 
 
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+//#include <asm/termios.h>
+#include <termios.h>
+
+
 #include <QMutex>
 #include <QSerialPort>
 
@@ -19,8 +27,8 @@ static std::string _twist_topic =  "/current_velocity";
 static std::string gstrserialportname = "/dev/ttyUSB0";
 
 static int gnBaudRate = 230400;
-static double glon0 = 117;
-static double glat0 = 39;
+static double glon0 = 117.0273054;
+static double glat0 = 39.1238777;
 static double ghead0 = 0;
 
 ros::Publisher pose_pub;
@@ -90,10 +98,12 @@ Quaternion ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch
 static void SerialGPSDecode()
 {
     int xpos = mstrHCP2.indexOf('\n');
-    if(xpos>0)
+//    std::cout<<" xpos : "<<xpos<<std::endl;
+    if(xpos>=0)
     {
 
         QString xsen = mstrHCP2.left(xpos+1);
+  //     std::cout<<" decode "<<std::endl;
         SerialGPSDecodeSen(xsen);
         mstrHCP2.remove(0,xpos+1);
         SerialGPSDecode();
@@ -134,6 +144,7 @@ static bool checknmeasen(const char * strsen,const unsigned int nlen)
     }
 
 
+//    std::cout<<" check fail. "<<std::endl;
 //    qDebug("  sen is %s",strsen);
     return false;
 }
@@ -292,18 +303,117 @@ void SerialGPSDecodeSen(QString strsen)
     msg.twist.twist.linear.x = sqrt(pow(vn,2)+pow(ve,2));
 
     geometry_msgs::PoseStamped  xPose;
+    xPose.header.frame_id = "map";
     xPose.pose = msg.pose.pose;
     geometry_msgs::TwistStamped xtwist;
+    xtwist.header.frame_id = "map";
     xtwist.twist = msg.twist.twist;
 
     pose_pub.publish(xPose);
     twist_pub.publish(xtwist);
 
+//    std::cout<<"current pose"<<std::endl;
+
+}
+
+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 ThreadRecvRTK()
 {
+    std::cout<<" enter thread "<<std::endl;
     QSerialPort  *m_serialPort_GPS;
     m_serialPort_GPS = new QSerialPort();
     m_serialPort_GPS->setPortName(gstrserialportname.data());
@@ -313,25 +423,54 @@ void ThreadRecvRTK()
     m_serialPort_GPS->setStopBits(QSerialPort::OneStop);
     m_serialPort_GPS->setFlowControl(QSerialPort::NoFlowControl);
     m_serialPort_GPS->setReadBufferSize(0);
+  //  m_serialPort_GPS->open(QIODevice::ReadWrite);
+  //  m_serialPort_GPS->close();
+
+
+    int fd;
+	int len, i,ret;
+ 
+ 
+	fd = open(gstrserialportname.data(), O_RDWR | O_NOCTTY);
+        if(fd < 0) {
+                perror(gstrserialportname.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(bSerialPortOpen)
     {
-        QByteArray ba = m_serialPort_GPS->readAll();
+  //      QByteArray ba = m_serialPort_GPS->readAll();
 
-        if(ba.length() > 0)
+        len = read(fd, strbuf, 999);
+        if(len > 0)
         {
-            mstrHCP2.append(ba);
+            strbuf[len] = 0;
+     //       std::cout<<strbuf<<std::endl;
+     //       std::cout<<" ba len : "<<len<<std::endl;
+            mstrHCP2.append(strbuf);
             SerialGPSDecode();
          //   mnNotRecvCount = 0;
             //Decode
@@ -339,10 +478,12 @@ void ThreadRecvRTK()
         else
         {
             std::this_thread::sleep_for(std::chrono::milliseconds(1));
+ //           std::cout<<" no data"<<std::endl;
         }
     }
 }
 
+
 int main(int argc, char *argv[])
 {
 	ros::init(argc, argv, "pilottoros");