change pilot_autoware_bridge. test ctlr run, not complete.

src/driver/driver_pilotautoware/.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/driver/driver_pilotautoware/driver_pilotautoware.pro

@@ -0,0 +1,38 @@
+QT -= gui
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+# 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 += \
+        ../../include/msgtype/gpsimu.pb.cc \
+        main.cpp
+
+# Default rules for deployment.
+qnx: target.path = /tmp/$${TARGET}/bin
+else: unix:!android: target.path = /opt/$${TARGET}/bin
+!isEmpty(target.path): INSTALLS += target
+
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+HEADERS += \
+    ../../include/msgtype/gpsimu.pb.h
+
+

src/driver/driver_pilotautoware/main.cpp

@@ -0,0 +1,59 @@
+#include <QCoreApplication>
+
+
+#include "modulecomm.h"
+
+#include "gpsimu.pb.h"
+
+#include <QSharedMemory>
+
+QSharedMemory * gpsharegps;
+
+void * gpagps;
+
+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;
+    iv::gps::gpsimu xgpsimu;
+    if(!xgpsimu.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"ListenRaw Parse error."<<std::endl;
+    }
+
+
+
+    std::cout<<" update gps."<<std::endl;
+    int npostime = sizeof(int);
+    int npossize = sizeof(int64_t) + npostime;
+    int nposdata = npossize + sizeof(int);
+
+    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;
+
+
+}
+
+
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    gpsharegps = new QSharedMemory("gps");
+    gpsharegps->create(10000);
+    gpsharegps->attach();
+
+    gpagps = iv::modulecomm::RegisterRecv("hcp2_gpsimu",ListenRaw);
+
+    return a.exec();
+}

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

@@ -73,6 +73,8 @@
 
 #include "modulecomm.h"
 
+#include <QSharedMemory>
+
 
 using autoware_auto_control_msgs::msg::AckermannControlCommand;
 using autoware_auto_geometry_msgs::msg::Complex32;
@@ -111,6 +113,7 @@ public:
 private:
   void callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr);
   void callbackTimer();
+  void callbackTimerGPS();
 
   rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
 
@@ -134,6 +137,7 @@ private:
 
 
   rclcpp::TimerBase::SharedPtr mptimer;
+  rclcpp::TimerBase::SharedPtr mptimerGPS;
 
   void publish_odometry(const Odometry & odometry);
   void publish_tf(const Odometry & odometry);
@@ -170,6 +174,8 @@ private:
 
   void * mpagpsimu;
   void * mpactrlcmd;
+  bool mbGPSAttach = false;
+  QSharedMemory * mpsharegps;
 
 
 };

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

@@ -69,12 +69,14 @@ 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");
   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);
- mpagpsimu = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",fungpsimu);
+// mpagpsimu = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",fungpsimu);
 
  mpactrlcmd = iv::modulecomm::RegisterSend("ctrlcmd",1000,1);
 
@@ -177,6 +179,7 @@ EulerAngles ToEulerAngles(Quaternion q) {
     return angles;
 }
 
+double steear_now = 0;
 
 Quaternion ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
 {
@@ -207,11 +210,113 @@ void pilot_autoware_bridge::callbackPose(geometry_msgs::msg::PoseStamped::Shared
   // TODO(YamatoAndo) apply low pass filter
 
 
+}
+
+
+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)
+    {
+    	return;
+    }
+    nLastUpdate = *ptime;
+    
+    std::cout<<" run hear. "<<std::endl;
+    
+    
+    unsigned int nSize;
+    nSize = *psize ;
+    char * strdata = new char[nSize];
+    
+    std::cout<<" nSize: "<<nSize<<std::endl;
+    memcpy(strdata,pdata,nSize);
+    
+       iv::gps::gpsimu  xgpsimu;
+    if(!xgpsimu.ParseFromArray(strdata,nSize))
+    {
+        delete strdata;
+        return;
+    }
+
+    double flat = xgpsimu.lat();
+    double flon = xgpsimu.lon();  
+
+    double pitch,roll,yaw;
+    pitch = 0;
+    roll = 0;
+
+    yaw = (90 - xgpsimu.heading()) *M_PI/180.0;
+    Quaternion quat = ToQuaternion(yaw,pitch,roll);
+
+    nav_msgs::msg::Odometry msg;
+
+    double fx,fy;
+    CalcXY(flon,flat,fx,fy);
+
+    double rel_x = fx;//7600;
+    double rel_y = fy;//32100;
+    double rel_z = 0;
+    double vn = xgpsimu.vn();
+    double ve = xgpsimu.ve();
+   // rel_x+=0.01;
+
+    msg.pose.pose.position.x = rel_x;
+    msg.pose.pose.position.y = rel_y;
+    msg.pose.pose.position.z = rel_z;
+
+    msg.pose.pose.orientation.x = quat.x;
+    msg.pose.pose.orientation.y = quat.y;
+    msg.pose.pose.orientation.z = quat.z;
+    msg.pose.pose.orientation.w = quat.w;
+
+    msg.twist.twist.linear.x = sqrt(pow(vn,2)+pow(ve,2));
+
+    publish_odometry(msg);
+    publish_tf(msg);
+
+      autoware_auto_vehicle_msgs::msg::VelocityReport velocity;
+  velocity.longitudinal_velocity = 0;
+  velocity.lateral_velocity = 0.0F;
+  velocity.heading_rate = 0;
+  publish_velocity(velocity);
+
+    current_steer_.steering_tire_angle = static_cast<double>(steear_now);
+  current_steer_.stamp = get_clock()->now();
+
+  publish_steering(current_steer_);
+
+  publish_acceleration();
+    
+    
+    delete strdata;
+    std::cout<<" rec gps."<<std::endl;
+
+    
 }
 
 void pilot_autoware_bridge::callbackTimer()
 {
-  std::cout<<" testpose. "<<std::endl;
+  std::cout<<" testpose1. "<<std::endl;
 
 
   if(mbcmdupdate == true)
@@ -223,6 +328,7 @@ void pilot_autoware_bridge::callbackTimer()
   xcmd.set_linear_acceleration(accel);
   xcmd.set_linear_velocity(vel);
   xcmd.set_steering_angle(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))
@@ -257,8 +363,8 @@ void pilot_autoware_bridge::callbackTimer()
     static double rel_x = fx;//7600;
     static double rel_y = fy;//32100;
     static double rel_z = 0;
-    static double vn = 0;
-    static double ve = 1;
+    static double vn =  0;
+    static double ve =  1;
    // rel_x+=0.01;
 
     msg.pose.pose.position.x = rel_x;
@@ -281,7 +387,7 @@ void pilot_autoware_bridge::callbackTimer()
   velocity.heading_rate = 0;
   publish_velocity(velocity);
 
-    current_steer_.steering_tire_angle = static_cast<double>(0);
+    current_steer_.steering_tire_angle = static_cast<double>(steear_now);
   current_steer_.stamp = get_clock()->now();
 
   publish_steering(current_steer_);
@@ -349,6 +455,7 @@ void pilot_autoware_bridge::CalcXY(double flon,double flat,double & fx,double &
     }
     fx = fmod(utm_x,1e5);
     fy = fmod(utm_y,1e5);
+  //  std::cout<<" fx: "<<fx<<" fy: "<<fy<<std::endl;
     return;
 }
 
@@ -381,6 +488,7 @@ void pilot_autoware_bridge::publish_acceleration()
 
 void pilot_autoware_bridge::UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
+  std::cout<<" recv gps."<<std::endl;
 
   (void)index;
   (void)dt;