complete apollodriver_hcp2. add nvcan.

src/apollo/apollolib/nvcan/.gitignore

@@ -0,0 +1,73 @@
+# This file is used to ignore files which are generated
+# ----------------------------------------------------------------------------
+
+*~
+*.autosave
+*.a
+*.core
+*.moc
+*.o
+*.obj
+*.orig
+*.rej
+*.so
+*.so.*
+*_pch.h.cpp
+*_resource.rc
+*.qm
+.#*
+*.*#
+core
+!core/
+tags
+.DS_Store
+.directory
+*.debug
+Makefile*
+*.prl
+*.app
+moc_*.cpp
+ui_*.h
+qrc_*.cpp
+Thumbs.db
+*.res
+*.rc
+/.qmake.cache
+/.qmake.stash
+
+# qtcreator generated files
+*.pro.user*
+
+# xemacs temporary files
+*.flc
+
+# Vim temporary files
+.*.swp
+
+# Visual Studio generated files
+*.ib_pdb_index
+*.idb
+*.ilk
+*.pdb
+*.sln
+*.suo
+*.vcproj
+*vcproj.*.*.user
+*.ncb
+*.sdf
+*.opensdf
+*.vcxproj
+*vcxproj.*
+
+# MinGW generated files
+*.Debug
+*.Release
+
+# Python byte code
+*.pyc
+
+# Binaries
+# --------
+*.dll
+*.exe
+

src/apollo/apollolib/nvcan/basecan.cpp

@@ -0,0 +1,39 @@
+#include "basecan.h"
+
+#include <iostream>
+
+basecan::basecan()
+{
+
+}
+
+basecan::~basecan()
+{
+    std::cout<<"del basecan. "<<std::endl;
+}
+
+int basecan::GetMessage(const int nch,basecan_msg *pMsg, const int nCap)
+{
+    return 0;
+}
+
+int basecan::SetMessage(const int nch, basecan_msg *pMsg)
+{
+    return 0;
+}
+
+
+void basecan::startdev()
+{
+
+}
+
+void basecan::stopdev()
+{
+
+}
+
+void basecan::CmdSend()
+{
+
+}

src/apollo/apollolib/nvcan/basecan.h

@@ -0,0 +1,39 @@
+#ifndef BASECAN_H
+#define BASECAN_H
+
+
+class basecan_msg
+{
+  public:
+    unsigned int id;
+    bool isExtern;
+    bool isRemote;
+    unsigned char nLen;
+    unsigned char data[8];
+    double frecvtime;
+#ifdef SEND_STAT
+    int64_t mSetTime;  //Used for calucate send latency
+#endif
+};
+
+class basecan
+{
+public:
+    basecan();
+    ~basecan();
+    virtual int GetMessage(const int nch,basecan_msg * pMsg,const int nCap);
+    virtual int SetMessage(const int nch,basecan_msg * pMsg); //Send Message
+
+    virtual void startdev();
+    virtual void stopdev();
+
+
+//signals:
+//    void SIG_CANOPENSTATE(bool bCAN,int nR,const char * strres);
+//    void SIGTEST();
+
+public:
+    virtual void CmdSend();
+};
+
+#endif // BASECAN_H

src/apollo/apollolib/nvcan/nvcan.cpp

@@ -0,0 +1,385 @@
+#include "nvcan.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <unistd.h>
+#include <string.h>
+#include <signal.h>
+#include <ctype.h>
+#include <libgen.h>
+#include <time.h>
+#include <errno.h>
+
+#include <sys/time.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/ioctl.h>
+#include <sys/uio.h>
+#include <net/if.h>
+
+
+#include <linux/can.h>
+#include <linux/can/raw.h>
+
+
+#include <iostream>
+
+/* for hardware timestamps - since Linux 2.6.30 */
+#ifndef SO_TIMESTAMPING
+#define SO_TIMESTAMPING 37
+#endif
+
+/* from #include <linux/net_tstamp.h> - since Linux 2.6.30 */
+#define SOF_TIMESTAMPING_SOFTWARE (1<<4)
+#define SOF_TIMESTAMPING_RX_SOFTWARE (1<<3)
+#define SOF_TIMESTAMPING_RAW_HARDWARE (1<<6)
+
+#define MAXSOCK 16    /* max. number of CAN interfaces given on the cmdline */
+#define MAXIFNAMES 30 /* size of receive name index to omit ioctls */
+#define MAXCOL 6      /* number of different colors for colorized output */
+#define ANYDEV "any"  /* name of interface to receive from any CAN interface */
+#define ANL "\r\n"    /* newline in ASC mode */
+
+#define SILENT_INI 42 /* detect user setting on commandline */
+#define SILENT_OFF 0  /* no silent mode */
+#define SILENT_ANI 1  /* silent mode with animation */
+#define SILENT_ON  2  /* silent mode (completely silent) */
+
+
+#define BUF_SIZE 1000
+
+std::string CANNAME[] = {"can0","can1"};
+
+nvcan::nvcan()
+{
+    mpthreadnvcan = new std::thread(&nvcan::threadrun,this);
+}
+
+nvcan::~nvcan()
+{
+    mbRun = false;
+    mpthreadnvcan->join();
+}
+
+void nvcan::threadrun()
+{
+
+    int currmax = 2;
+    fd_set rdfs;
+    int s[MAXSOCK];
+    int ret;
+
+    int canfd_on = 1;
+    
+    struct sockaddr_can addr;
+    char ctrlmsg[CMSG_SPACE(sizeof(struct timeval) + 3*sizeof(struct timespec) + sizeof(__u32))];
+    struct iovec iov;
+    struct msghdr xmsghdr;
+    struct canfd_frame frame;
+    int nbytes, i, maxdlen;
+    struct ifreq ifr;
+    struct timeval tv, last_tv;
+    struct timeval timeout_config = { 0, 0 }, *timeout_current = 0;
+
+    struct cmsghdr *cmsg;
+
+    std::cout<<"Initializing."<<std::endl;
+    
+    for(i=0;i<currmax;i++)
+    {
+        s[i] = socket(PF_CAN, SOCK_RAW, CAN_RAW);
+        if (s[i] < 0) {
+            std::cout<<"Create Socket Error."<<std::endl;
+            return;
+        }
+
+        addr.can_family = AF_CAN;
+
+        memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
+        strncpy(ifr.ifr_name, CANNAME[i].data(), 5);
+
+        if (ioctl(s[i], SIOCGIFINDEX, &ifr) < 0) {
+            std::cout<<"SIOCGIFINDEX."<<std::endl;
+            return;
+        }
+        addr.can_ifindex = ifr.ifr_ifindex;
+
+        //CANFD Support
+        setsockopt(s[i], SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &canfd_on, sizeof(canfd_on));
+
+        const int timestamp_on = 1;
+
+        if (setsockopt(s[i], SOL_SOCKET, SO_TIMESTAMP,
+                   &timestamp_on, sizeof(timestamp_on)) < 0) {
+            perror("setsockopt SO_TIMESTAMP");
+        }
+
+        if (bind(s[i], (struct sockaddr *)&addr, sizeof(addr)) < 0) {
+            std::cout<<"bind error."<<std::endl;
+            return;
+        }
+    }
+
+    mbCANOpen = true;
+    std::cout<<"open can succesfully."<<std::endl;
+
+    iov.iov_base = &frame;
+    xmsghdr.msg_name = &addr;
+    xmsghdr.msg_iov = &iov;
+    xmsghdr.msg_iovlen = 1;
+    xmsghdr.msg_control = &ctrlmsg;
+
+    int64_t nLastRecv = std::chrono::system_clock::now().time_since_epoch().count()/1e6;
+    int nRecvState = 0; // 0 Have Data  1 No Data;
+
+    mbRunning = true;
+
+    int nrecvcount = 0;
+
+    while((mbRun)&&(mbCANOpen))
+    {
+        FD_ZERO(&rdfs);
+        for (i=0; i<currmax; i++)
+            FD_SET(s[i], &rdfs);
+
+        if (timeout_current)
+            *timeout_current = timeout_config;
+
+        timeout_config.tv_sec= 0;
+        timeout_config.tv_usec = 100;;
+        timeout_current = &timeout_config;
+        ret = select(s[currmax-1]+1, &rdfs, NULL, NULL, timeout_current);
+        if (ret < 0) {
+            std::cout<<"select error. "<<std::endl;
+            mbCANOpen = false;
+            continue;
+        }
+
+        for (i=0; i<currmax; i++) {  /* check all CAN RAW sockets */
+
+            if (FD_ISSET(s[i], &rdfs)) {
+
+                nLastRecv = std::chrono::system_clock::now().time_since_epoch().count()/1e6;
+                /* these settings may be modified by recvmsg() */
+                iov.iov_len = sizeof(frame);
+                xmsghdr.msg_namelen = sizeof(addr);
+                xmsghdr.msg_controllen = sizeof(ctrlmsg);
+                xmsghdr.msg_flags = 0;
+
+                nbytes = recvmsg(s[i], &xmsghdr, 0);
+
+                if (nbytes < 0) {
+ //                   if ((errno == ENETDOWN) && !down_causes_exit) {
+                    if ((errno == ENETDOWN)) {
+                        std::cout<<CANNAME[i]<<" interface down"<<std::endl;
+                        fprintf(stderr, "%s: interface down\n", CANNAME[i].data());
+                        return;
+                    }
+                    continue;
+//                    perror("read");
+//                    return 1;
+                }
+
+                if ((size_t)nbytes == CAN_MTU)
+                    maxdlen = CAN_MAX_DLEN;
+                else if ((size_t)nbytes == CANFD_MTU)
+                    maxdlen = CANFD_MAX_DLEN;
+                else {
+                    std::cout<<"read incomplete message"<<std::endl;
+                    continue;
+                }
+
+                nrecvcount++;
+  //              qDebug("receive msg.");
+                mMutex.lock();
+
+                basecan_msg msg;
+                msg.id = frame.can_id&0x1fffffff;
+                if((frame.can_id&0x80000000)!= 0)msg.isExtern = true;
+                else msg.isExtern = false;
+                if((frame.can_id&0x40000000)!= 0)msg.isRemote = true;
+                else msg.isRemote = false;
+                msg.nLen = frame.len;
+//                if(msg.id == 0x1c2)qDebug("id = %08x",msg.id);
+                if((frame.len<=64)&&(frame.len>0))memcpy(msg.data,frame.data,frame.len);
+
+                msg.frecvtime = 0.0;
+                for (cmsg = CMSG_FIRSTHDR(&xmsghdr);
+                     cmsg && (cmsg->cmsg_level == SOL_SOCKET);
+                     cmsg = CMSG_NXTHDR(&xmsghdr,cmsg)) {
+                    if (cmsg->cmsg_type == SO_TIMESTAMP) {
+                        memcpy(&tv, CMSG_DATA(cmsg), sizeof(tv));
+                    } else if (cmsg->cmsg_type == SO_TIMESTAMPING) {
+
+                        struct timespec *stamp = (struct timespec *)CMSG_DATA(cmsg);
+
+                        /*
+                         * stamp[0] is the software timestamp
+                         * stamp[1] is deprecated
+                         * stamp[2] is the raw hardware timestamp
+                         * See chapter 2.1.2 Receive timestamps in
+                         * linux/Documentation/networking/timestamping.txt
+                         */
+                        tv.tv_sec = stamp[2].tv_sec;
+                        tv.tv_usec = stamp[2].tv_nsec/1000;
+
+                        msg.frecvtime = tv.tv_usec;
+                        msg.frecvtime = tv.tv_usec/((double)1e6);
+                        msg.frecvtime = tv.tv_sec + msg.frecvtime;
+                        std::cout<<" set frecvtime."<<std::endl;
+
+                    } else if (cmsg->cmsg_type == SO_RXQ_OVFL)
+                        std::cout<<" type is SO_RXQ_OVFL"<<std::endl;
+ //                       memcpy(&dropcnt[i], CMSG_DATA(cmsg), sizeof(__u32));
+                }
+
+                if(mMsgRecvBuf[i].size()<BUF_SIZE)
+                {
+                    mMsgRecvBuf[i].push_back(msg);
+                }
+
+                mMutex.unlock();
+
+            }
+        }
+
+        if((std::chrono::steady_clock::now().time_since_epoch().count()/1e6 - nLastRecv)> 1000)
+        {
+            if(nRecvState == 0)
+            {
+                nRecvState = -1;
+//                std::cout<<"More than 1 second not receive data."<<std::endl;
+            }
+        }
+        else
+        {
+            if(nRecvState == -1)
+            {
+                nRecvState = 0;
+//                mfault->SetFaultState(0,0,"CAN OK.");
+            }
+        }
+
+        struct canfd_frame framesend[2500];
+
+
+        for(int nch =0;nch<currmax;nch++)
+        {
+            int nsend = 0;
+            mMutex.lock();
+            for(i=0;i<mMsgSendBuf[nch].size();i++)
+            {
+                if(i>=2500)break;
+                framesend[i].len = mMsgSendBuf[nch].at(i).nLen;
+                memcpy(framesend[i].data,mMsgSendBuf[nch].at(i).data,framesend[i].len);
+                framesend[i].can_id = mMsgSendBuf[nch].at(i).id;
+                if(mMsgSendBuf[nch].at(i).isExtern)
+                {
+                    framesend[i].can_id = framesend[i].can_id|0x80000000;
+                }
+                else
+                {
+                    framesend[i].can_id = framesend[i].can_id&0x7ff;
+                }
+                if(mMsgSendBuf[nch].at(i).isRemote)
+                {
+                    framesend[i].can_id= framesend[i].can_id|0x40000000;
+                }
+
+
+
+                nsend++;
+            }
+            mMsgSendBuf[nch].clear();
+            mMutex.unlock();
+            if(nsend > 0)
+            {
+                for(i=0;i<nsend;i++)
+                {
+                    int nsendbyte = 16;
+                    if(framesend[i].len>8)
+                    {
+                        nsendbyte = CANFD_MTU;
+                    }
+                if (write(s[nch], &framesend[i],nsendbyte) != nsendbyte) {
+                    perror("write error 1.");
+                    continue;
+                }
+                }
+            }
+
+        }
+
+    }
+
+    for (i=0; i<currmax; i++)
+    {
+        close(s[i]);
+    }
+    std::cout<<"nvcan thread close."<<std::endl;
+    mbRunning = false;
+}
+
+void nvcan::startdev()
+{
+//    start();
+}
+
+void nvcan::stopdev()
+{
+//    requestInterruption();
+//    QTime xTime;
+//    xTime.start();
+//    while(xTime.elapsed()<100)
+//    {
+//        if(mbRunning == false)
+//        {
+//            mfault->SetFaultState(1, 0, "can closed");
+//            mivlog->error("can is closed at %d",xTime.elapsed());
+//            qDebug("can is closed.");
+//            break;
+//        }
+//    }
+}
+
+int nvcan::GetMessage(const int nch,basecan_msg *pMsg, const int nCap)
+{
+    if((nch>1)||(nch < 0))return -1;
+    if(mMsgRecvBuf[nch].size() == 0)return 0;
+
+    int nRtn;
+    nRtn = nCap;
+    mMutex.lock();
+    if(nRtn > mMsgRecvBuf[nch].size())nRtn = mMsgRecvBuf[nch].size();
+    int i;
+    for(i=0;i<nRtn;i++)
+    {
+        memcpy(&pMsg[i],&(mMsgRecvBuf[nch].at(i)),sizeof(basecan_msg));
+    }
+
+    std::vector<basecan_msg>::iterator iter;
+    iter = mMsgRecvBuf[nch].begin();
+    for(i=0;i<nRtn;i++)
+    {
+        iter = mMsgRecvBuf[nch].erase(iter);
+    }
+
+    mMutex.unlock();
+
+    return nRtn;
+
+}
+
+int nvcan::SetMessage(const int nch, basecan_msg *pMsg)
+{
+    if((nch>1)||(nch < 0))return -1;
+
+    if(mMsgSendBuf[nch].size() > BUF_SIZE)return -2;
+
+    mMutex.lock();
+    mMsgSendBuf[nch].push_back(*pMsg);
+    mMutex.unlock();
+    return 0;
+}
+

src/apollo/apollolib/nvcan/nvcan.h

@@ -0,0 +1,41 @@
+#ifndef NVCAN_H
+#define NVCAN_H
+#include "basecan.h"
+
+#include <vector>
+#include <mutex>
+
+#include <thread>
+
+class nvcan : public basecan
+{
+public:
+    nvcan();
+    ~nvcan();
+public:
+    void startdev();
+    void stopdev();
+
+    int GetMessage(const int nch,basecan_msg * pMsg,const int nCap);
+    int SetMessage(const int nch,basecan_msg * pMsg);
+
+//private slots:
+//    void onMsg(bool bCAN,int nR,const char * strres);
+
+private:
+    void threadrun();
+    std::vector<basecan_msg> mMsgRecvBuf[2];
+
+    std::vector<basecan_msg> mMsgSendBuf[2];
+
+    std::mutex mMutex;
+
+    bool mbCANOpen = false;
+    bool mbRunning = false;
+    int mnDevNum;
+
+    bool mbRun = true;
+    std::thread * mpthreadnvcan;
+};
+
+#endif // NVCAN_H

src/apollo/apollolib/nvcan/nvcan.pro

@@ -0,0 +1,33 @@
+CONFIG -= qt
+
+TEMPLATE = lib
+DEFINES += NVCAN_LIBRARY
+
+CONFIG += c++11
+
+CONFIG += plugin
+
+# The following define makes your compiler emit warnings if you use
+# any Qt feature that has been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if it uses deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+    basecan.cpp \
+    nvcan.cpp
+
+HEADERS += \
+    basecan.h \
+    nvcan.h
+
+# Default rules for deployment.
+unix {
+    target.path = /usr/lib
+}
+!isEmpty(target.path): INSTALLS += target

src/apollo/code/apollodriver_hcp2/apollodriver_hcp2.pro

@@ -1,3 +1,7 @@
+#sudo apt install libgeographic-dev
+#qmake apollodriver_hcp2.pro  
+#make
+
 QT -= gui
 
 CONFIG += c++11 console
@@ -15,16 +19,26 @@ DEFINES += QT_DEPRECATED_WARNINGS
 #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
 
 SOURCES += \
-        main.cpp
+        main.cpp \
+        /apollo_workspace/modules/common_msgs/localization_msgs/localization.pb.cc  \
+        /apollo_workspace/modules/common_msgs/localization_msgs/localization_status.pb.cc  \
+        /apollo_workspace/modules/common_msgs/localization_msgs/pose.pb.cc  \
+        /apollo_workspace/modules/common_msgs/basic_msgs/header.pb.cc  \
+        /apollo_workspace/modules/common_msgs/basic_msgs/pnc_point.pb.cc  \ 
+        /apollo_workspace/modules/common_msgs/basic_msgs/geometry.pb.cc  \
+        /apollo_workspace/modules/common_msgs/basic_msgs/error_code.pb.cc
 
 # Default rules for deployment.
 qnx: target.path = /tmp/$${TARGET}/bin
 else: unix:!android: target.path = /opt/$${TARGET}/bin
 !isEmpty(target.path): INSTALLS += target
 
+INCLUDEPATH += /usr/include/eigen-3.4.0
 INCLUDEPATH += /opt/apollo/neo/include
 INCLUDEPATH += /apollo_workspace/bazel-bin/external/apollo_src
 
+LIBS += -lGeographic
+
 LIBS += -L/opt/apollo/neo/lib/cyber -lcyber -lcyber_binary -lcyber_state
 
 LIBS += /opt/apollo/neo/lib/cyber/component/*.so

src/apollo/code/apollodriver_hcp2/main.cpp

@@ -1,13 +1,99 @@
 #include <QCoreApplication>
 
+#include <GeographicLib/MGRS.hpp>
+#include <GeographicLib/UTMUPS.hpp>
+
 #include "cyber/cyber.h"
 #include "cyber/time/rate.h"
 #include "cyber/time/time.h"
 #include "hcp2.h"
 
+#include "modules/common_msgs/localization_msgs/localization.pb.h"
+
+#include "modules/common/math/quaternion.h"
+#include "modules/common/math/quaternion.h"
+#include "modules/common/util/message_util.h"
+#include "modules/common/util/util.h"
+
+
+std::shared_ptr<apollo::cyber::Writer<apollo::localization::LocalizationEstimate>>
+      localization_writer_;
+
+using apollo::common::Point3D;
+using apollo::common::math::HeadingToQuaternion;
+using apollo::common::Quaternion;
+
+using apollo::common::math::InverseQuaternionRotate;
+using apollo::common::util::FillHeader;
+
+void TransformToVRF(const Point3D &point_mrf, const Quaternion &orientation,
+                    Point3D *point_vrf) {
+  Eigen::Vector3d v_mrf(point_mrf.x(), point_mrf.y(), point_mrf.z());
+  auto v_vrf = InverseQuaternionRotate(orientation, v_mrf);
+  point_vrf->set_x(v_vrf.x());
+  point_vrf->set_y(v_vrf.y());
+  point_vrf->set_z(v_vrf.z());
+}
+
+void PublishLocalization(const double x,const double y,const double z,const double yaw,const double v,const double kappa,const double a) {
+  auto localization = std::make_shared<apollo::localization::LocalizationEstimate>();
+  FillHeader("SimPerfectControl", localization.get());
+  auto *pose = localization->mutable_pose();
+
+  pose->mutable_position()->set_x(x);
+  pose->mutable_position()->set_y(y);
+  pose->mutable_position()->set_z(z);
+  // Set orientation and heading
+  double cur_theta = yaw;
+
+
+
+  Eigen::Quaternion<double> cur_orientation =
+      HeadingToQuaternion<double>(cur_theta);
+  pose->mutable_orientation()->set_qw(cur_orientation.w());
+  pose->mutable_orientation()->set_qx(cur_orientation.x());
+  pose->mutable_orientation()->set_qy(cur_orientation.y());
+  pose->mutable_orientation()->set_qz(cur_orientation.z());
+  pose->set_heading(cur_theta);
+
+  // Set linear_velocity
+  pose->mutable_linear_velocity()->set_x(std::cos(cur_theta) * v);
+  pose->mutable_linear_velocity()->set_y(std::sin(cur_theta) * v);
+  pose->mutable_linear_velocity()->set_z(0);
+
+  // Set angular_velocity in both map reference frame and vehicle reference
+  // frame
+  pose->mutable_angular_velocity()->set_x(0);
+  pose->mutable_angular_velocity()->set_y(0);
+  pose->mutable_angular_velocity()->set_z(v*kappa);
+
+  TransformToVRF(pose->angular_velocity(), pose->orientation(),
+                 pose->mutable_angular_velocity_vrf());
+
+  // Set linear_acceleration in both map reference frame and vehicle reference
+  // frame
+  auto *linear_acceleration = pose->mutable_linear_acceleration();
+  linear_acceleration->set_x(std::cos(cur_theta) * a);
+  linear_acceleration->set_y(std::sin(cur_theta) * a);
+  linear_acceleration->set_z(0);
+
+  TransformToVRF(pose->linear_acceleration(), pose->orientation(),
+                 pose->mutable_linear_acceleration_vrf());
+  localization_writer_->Write(localization);
+
+}
+
 void hcp2callback(iv::rawgps xrawgps)
 {
-
+    double flon = xrawgps.mfLon;
+    double flat = xrawgps.mfLat;
+    
+    double utm_x,utm_y; 
+    int zone{};
+    bool is_north{};
+    GeographicLib::UTMUPS::Forward(flat, flon, zone, is_north, utm_x,utm_y);
+    
+    PublishLocalization(utm_x,utm_y,xrawgps.mfHgt,(90-xrawgps.mfheading)*M_PI/180.0,xrawgps.mfVel,0,0);
 }
 
 
@@ -18,9 +104,15 @@ int main(int argc, char *argv[])
     apollo::cyber::Init("apllodriver_hcp2");
     std::unique_ptr<apollo::cyber::Node> pilot_node  = apollo::cyber::CreateNode("apllodriver_hcp2");
 
+    localization_writer_ = pilot_node->CreateWriter<apollo::localization::LocalizationEstimate>("/apollo/localization/adcpose");
+
     std::string strgpsport =  "/dev/ttyUSB0";
     hcp2fun funcall = std::bind(&hcp2callback,std::placeholders::_1);
     hcp2 xhcp2(strgpsport,funcall);
 
-    return a.exec();
+    apollo::cyber::WaitForShutdown();
+
+    
+
+    return 0;
 }