change pilot_autoware_bridge add chassis info. add simplesmshare, use communication between pilot and autoware.

src/driver/driver_pilotautoware/driver_pilotautoware.pro

@@ -15,8 +15,8 @@ DEFINES += QT_DEPRECATED_WARNINGS
 #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
 
 SOURCES += \
-        ../../include/msgtype/gpsimu.pb.cc \
-        main.cpp
+        main.cpp \
+        simplesmshare.cpp
 
 # Default rules for deployment.
 qnx: target.path = /tmp/$${TARGET}/bin
@@ -33,6 +33,6 @@ else: unix:!android: target.path = /opt/$${TARGET}/bin
 }
 
 HEADERS += \
-    ../../include/msgtype/gpsimu.pb.h
+    simplesmshare.h
 
 

src/driver/driver_pilotautoware/main.cpp

@@ -1,59 +1,78 @@
 #include <QCoreApplication>
 
-
+#include "simplesmshare.h"
 #include "modulecomm.h"
 
-#include "gpsimu.pb.h"
+#include  <thread>
 
-#include <QSharedMemory>
+void * gpagps;
+void * gpachassis;
+void * gpactrlcmd;
 
-QSharedMemory * gpsharegps;
+simplesmshare * gpsharegps;
+simplesmshare * gpsharechassis;
+simplesmshare * gpsharectrlcmd;
 
-void * gpagps;
+bool gbthreadrun = true;
 
-void ListenRaw(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+void UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
 {
     (void)index;
     (void)dt;
     (void)strmemname;
-    iv::gps::gpsimu xgpsimu;
-    if(!xgpsimu.ParseFromArray(strdata,nSize))
-    {
-        std::cout<<"ListenRaw Parse error."<<std::endl;
-    }
+
+    gpsharechassis->write(strdata,nSize);
+
+}
 
 
 
-    std::cout<<" update gps."<<std::endl;
-    int npostime = sizeof(int);
-    int npossize = sizeof(int64_t) + npostime;
-    int nposdata = npossize + sizeof(int);
+void ListenRaw(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    (void)index;
+    (void)dt;
+    (void)strmemname;
 
-    char * pmem = (char *)gpsharegps->data();
-    int * pmark = (int * )pmem;
-    int64_t * ptime = (int64_t * )(pmem + npostime);
-    int * psize = (int * )(pmem + npossize);
-    char * pdata = (char * )(pmem + nposdata);
-    *pmark = 1;
-    *ptime = std::chrono::system_clock::now().time_since_epoch().count();
-    *psize = nSize;
-    memcpy(pdata,strdata,nSize);
-    *pmark = 0;
+    gpsharegps->write(strdata,nSize);
 
 
 }
 
-
+void treadupdatectrl()
+{
+    char * strdata;
+    int nbufsize = 10000;
+    strdata = new char[nbufsize];
+    while(gbthreadrun)
+    {
+        int nread = gpsharectrlcmd->read(strdata,nbufsize);
+        if(nread > 0)
+        {
+            iv::modulecomm::ModuleSendMsg(gpactrlcmd,strdata,nbufsize);
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+    delete strdata;
+}
 
 int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
 
-    gpsharegps = new QSharedMemory("gps");
-    gpsharegps->create(10000);
+    gpsharegps = new simplesmshare("aw_gps",10000);
+    gpsharegps->create();
     gpsharegps->attach();
 
+    gpsharechassis = new simplesmshare("aw_chassis",10000);
+    gpsharechassis->create();
+    gpsharechassis->attach();
+
+    gpsharectrlcmd = new simplesmshare("aw_ctrlcmd",10000);
+
+
     gpagps = iv::modulecomm::RegisterRecv("hcp2_gpsimu",ListenRaw);
+    gpachassis = iv::modulecomm::RegisterRecv("chassis",UpdateChassis);
+    gpactrlcmd = iv::modulecomm::RegisterSend("ctrlcmd",10000,1);
 
     return a.exec();
 }

src/driver/driver_pilotautoware/simplesmshare.cpp

@@ -0,0 +1,127 @@
+#include "simplesmshare.h"
+
+#include <thread>
+#include <iostream>
+#include <chrono>
+
+simplesmshare::simplesmshare(const char * strsmname,int nsize)
+{
+    strncpy(mstrsmane,strsmname,255);
+    mnsize = nsize;
+    mpshare = new QSharedMemory(mstrsmane);
+}
+
+
+void simplesmshare::create()
+{
+    mpshare->create(mnsize);
+}
+
+bool simplesmshare::attach()
+{
+    return mpshare->attach();
+}
+
+bool simplesmshare::write(const char * strdata,int nsize)
+{
+
+    if(!mpshare->isAttached())
+    {
+        mpshare->create(mnsize);
+        mpshare->attach();
+    }
+
+    if(!mpshare->isAttached())
+    {
+        std::cout<<getcurrentms()<<" simplesmshare::write not attach"<<std::endl;
+        return false;
+    }
+
+    if(nsize >  (mnsize - 16))
+    {
+        std::cout<<" data size is big. buffer size is: "<<(mnsize-16)<<" data size is: "<<nsize<<std::endl;
+        return false;
+    }
+
+    int npostime = sizeof(int);
+    int npossize = sizeof(int64_t) + npostime;
+    int nposdata = npossize + sizeof(int);
+
+    char * pmem = (char *)mpshare->data();
+    int * pmark = (int * )pmem;
+    int64_t * ptime = (int64_t * )(pmem + npostime);
+    int * psize = (int * )(pmem + npossize);
+    char * pdata = (char * )(pmem + nposdata);
+    *pmark = 1;
+    *ptime = std::chrono::system_clock::now().time_since_epoch().count();
+    *psize = nsize;
+    memcpy(pdata,strdata,nsize);
+    *pmark = 0;
+
+    return true;
+}
+
+int simplesmshare::read(char * strdata,unsigned int nreadmax)
+{
+    if(!mpshare->isAttached())
+    {
+        mpshare->attach();
+    }
+    if(!mpshare->isAttached())
+    {
+        std::cout<<getcurrentms()<<" simplesmshare::read not attach"<<std::endl;
+        return 0;
+    }
+
+    static int64_t nLastUpdate = 0;
+
+    int npostime = sizeof(int);
+     int npossize = sizeof(int64_t) + npostime;
+     int nposdata = npossize + sizeof(int);
+
+     char * pmem = (char *)mpshare->data();
+     int * pmark = (int * )pmem;
+     int64_t * ptime = (int64_t * )(pmem + npostime);
+     int * psize = (int * )(pmem + npossize);
+     char * pdata = (char * )(pmem + nposdata);
+
+     if(*pmark == 1)return 0;
+
+
+
+     if(*ptime == nLastUpdate)
+     {
+         return 0;
+     }
+
+     if(abs(*ptime -nLastUpdate)/1000000 > 30000)
+     {
+         std::cout<<" data is more than 30 seconds."<<std::endl;
+         return 0;
+     }
+
+     nLastUpdate = *ptime;
+
+
+
+
+
+     unsigned int nSize;
+     nSize = *psize ;
+
+     if(nSize > nreadmax)
+     {
+         std::cout<<" data size is: "<<nSize<<" nread is : "<<nreadmax<<". please change nread."<<std::endl;
+         return 0;
+     }
+
+     std::cout<<" nSize: "<<nSize<<std::endl;
+     memcpy(strdata,pdata,nSize);
+
+     return nSize;
+}
+
+int64_t simplesmshare::getcurrentms()
+{
+    return std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+}

src/driver/driver_pilotautoware/simplesmshare.h

@@ -0,0 +1,27 @@
+#ifndef SIMPLESMSHARE_H
+#define SIMPLESMSHARE_H
+
+#include <QSharedMemory>
+
+class simplesmshare
+{
+public:
+    simplesmshare(const char * strsmname,int nsize);
+
+    void create();
+    bool attach();
+
+    bool write(const char * strdata,int nsize);
+    int read(char * strdata,unsigned int nreadmax);
+
+private:
+    char mstrsmane[256];
+    int mnsize;
+
+    QSharedMemory * mpshare;
+
+    int64_t getcurrentms();
+
+};
+
+#endif // SIMPLESMSHARE_H

src/ros2/src/pilot_autoware_bridge/CMakeLists.txt

@@ -48,11 +48,14 @@ link_directories(
 add_executable(pilot_autoware_bridge
   src/pilot_autoware_bridge_node.cpp
   src/pilot_autoware_bridge_core.cpp
+  src/simplesmshare.cpp
   ./../include/msgtype/gpsimu.pb.cc
   ./../include/msgtype/autowarectrlcmd.pb.cc
+  ./../include/msgtype/chassis.pb.cc
 )
 
-target_link_libraries(pilot_autoware_bridge  ${Protobuf_LIBRARIES}  Geographic  modulecomm )
+target_link_libraries(pilot_autoware_bridge  ${Protobuf_LIBRARIES}  Geographic )  
+# modulecomm 
 
 ament_target_dependencies(pilot_autoware_bridge rclcpp std_msgs geometry_msgs 
   tf2_geometry_msgs nav_msgs sensor_msgs autoware_auto_control_msgs autoware_auto_geometry_msgs

src/ros2/src/pilot_autoware_bridge/README.md

@@ -1,11 +1,10 @@
-# pose2twist
+# pilot_autoware_bridge
 
 ## Purpose
 
-This `pose2twist` calculates the velocity from the input pose history. In addition to the computed twist, this node outputs the linear-x and angular-z components as a float message to simplify debugging.
+This `pilot_autoware_bridge` is a bridge between pilot and autoware.
+
 
-The `twist.linear.x` is calculated as `sqrt(dx * dx + dy * dy + dz * dz) / dt`, and the values in the `y` and `z` fields are zero.
-The `twist.angular` is calculated as `d_roll / dt`, `d_pitch / dt` and `d_yaw / dt` for each field.
 
 ## Inputs / Outputs
 

src/ros2/src/pilot_autoware_bridge/include/pilot_autoware_bridge/pilot_autoware_bridge_core.hpp

@@ -70,11 +70,14 @@
 
 #include "gpsimu.pb.h"
 #include "autowarectrlcmd.pb.h"
+#include "chassis.pb.h"
 
-#include "modulecomm.h"
+//#include "modulecomm.h"
 
 #include <QSharedMemory>
 
+#include "simplesmshare.h"
+
 
 using autoware_auto_control_msgs::msg::AckermannControlCommand;
 using autoware_auto_geometry_msgs::msg::Complex32;
@@ -175,7 +178,11 @@ private:
   void * mpagpsimu;
   void * mpactrlcmd;
   bool mbGPSAttach = false;
-  QSharedMemory * mpsharegps;
+//  QSharedMemory * mpsharegps;
+
+  simplesmshare * mpsharegps;
+  simplesmshare * mpsharecmd;
+  simplesmshare * mpsharechassis;
 
 
 };

src/ros2/src/pilot_autoware_bridge/src/pilot_autoware_bridge_core.cpp

@@ -34,7 +34,9 @@
 
 #include <QString>
 
-#include "modulecomm.h"
+//#include "modulecomm.h"
+
+
 
 using namespace std;
 
@@ -69,16 +71,26 @@ pub_acc_ = create_publisher<AccelWithCovarianceStamped>("/localization/accelerat
 sub_cmd_ = create_subscription<AckermannControlCommand>(
     "/control/command/control_cmd", 1, [this](const AckermannControlCommand::SharedPtr msg) { mpcmd_ptr = msg;mbcmdupdate = true; std::cout<<"new cmd. "<<std::endl;});
 
-mpsharegps = new QSharedMemory("gps");
+
+ mpsharecmd =  new simplesmshare("aw_ctrlcmd",1000);
+ mpsharecmd->create();
+ mpsharecmd->attach();
+
+ mpsharechassis = new simplesmshare("aw_chassis",1000);
+ mpsharegps = new simplesmshare("aw_gps",10000);
+
+
   mptimer= create_wall_timer(100ms,std::bind(&pilot_autoware_bridge::callbackTimer, this));
   mptimerGPS = create_wall_timer(10ms,std::bind(&pilot_autoware_bridge::callbackTimerGPS, this));
 
   
 
- ModuleFun fungpsimu =std::bind(&pilot_autoware_bridge::UpdateGPSIMU,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+// ModuleFun fungpsimu =std::bind(&pilot_autoware_bridge::UpdateGPSIMU,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
 // mpagpsimu = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",fungpsimu);
 
- mpactrlcmd = iv::modulecomm::RegisterSend("ctrlcmd",1000,1);
+// mpactrlcmd = iv::modulecomm::RegisterSend("ctrlcmd",1000,1);
+
+
 
 
 }
@@ -215,48 +227,32 @@ void pilot_autoware_bridge::callbackPose(geometry_msgs::msg::PoseStamped::Shared
 
 void pilot_autoware_bridge::callbackTimerGPS()
 {
-  static int64_t nLastUpdate = 0;
-  if(mbGPSAttach == false)
-  {
-  	mbGPSAttach = mpsharegps->attach();
-  }
-	if(mbGPSAttach == false)return;
-    int npostime = sizeof(int);
-    int npossize = sizeof(int64_t) + npostime;
-    int nposdata = npossize + sizeof(int);
-
-    char * pmem = (char *)mpsharegps->data();
-    int * pmark = (int * )pmem;
-    int64_t * ptime = (int64_t * )(pmem + npostime);
-    int * psize = (int * )(pmem + npossize);
-    char * pdata = (char * )(pmem + nposdata);
-    
-    if(*pmark == 1)return;
-    
-    
-    
-    if(*ptime == nLastUpdate)
+
+  
+    unsigned int nSize;
+    char * strdata = new char[10000];
+
+    int nread = mpsharegps->read(strdata,10000);
+
+    if(nread < 1)
     {
-    	return;
+
     }
-    nLastUpdate = *ptime;
-    
-    std::cout<<" run hear. "<<std::endl;
-    
-    
-    unsigned int nSize;
-    nSize = *psize ;
-    char * strdata = new char[nSize];
+    else
+    {
+
+    nSize = nread;
     
     std::cout<<" nSize: "<<nSize<<std::endl;
-    memcpy(strdata,pdata,nSize);
     
        iv::gps::gpsimu  xgpsimu;
     if(!xgpsimu.ParseFromArray(strdata,nSize))
     {
-        delete strdata;
+        std::cout<<" parse gps msg fail."<<std::endl;
         return;
     }
+    else
+    {
 
     double flat = xgpsimu.lat();
     double flon = xgpsimu.lon();  
@@ -308,8 +304,37 @@ void pilot_autoware_bridge::callbackTimerGPS()
   publish_acceleration();
     
     
-    delete strdata;
     std::cout<<" rec gps."<<std::endl;
+  }
+
+  }
+
+
+  nread = mpsharechassis->read(strdata,10000);
+
+  if(nread < 1)
+  {
+
+  }
+  else
+  {
+        iv::chassis xchassis;
+    //    static int ncount = 0;
+    if(!xchassis.ParseFromArray(strdata,nread))
+    {
+        std::cout<<"Chassis ParseFrom Array Error."<<std::endl;
+    }
+    else
+    {
+        steear_now = (xchassis.angle_feedback() *M_PI/180.0)/15.0;
+    }
+
+  }
+
+  delete strdata;
+  
+
+
 
     
 }
@@ -328,12 +353,12 @@ void pilot_autoware_bridge::callbackTimer()
   xcmd.set_linear_acceleration(accel);
   xcmd.set_linear_velocity(vel);
   xcmd.set_steering_angle(steer);
-  steear_now = steer;
+ // steear_now = steer;
   int ndatasize = xcmd.ByteSizeLong();
   std::shared_ptr<char> pstr_ptr = std::shared_ptr<char>(new char[ndatasize]);
   if(xcmd.SerializeToArray(pstr_ptr.get(),ndatasize))
   {
-    iv::modulecomm::ModuleSendMsg(mpactrlcmd ,pstr_ptr.get(),ndatasize);
+    mpsharecmd->write(pstr_ptr.get(),ndatasize);
   }
   else
   {