change pilot_autoware_bridge. remove some debug code.

src/driver/driver_pilotautoware/simplesmshare.cpp

@@ -58,7 +58,7 @@ bool simplesmshare::write(const char * strdata,int nsize)
     memcpy(pdata,strdata,nsize);
     *pmark = 0;
 
-    std::cout<<"write msg. "<<std::endl;
+ //   std::cout<<"write msg. "<<std::endl;
 
     return true;
 }
@@ -118,7 +118,7 @@ int simplesmshare::read(char * strdata,unsigned int nreadmax)
          return 0;
      }
 
-     std::cout<<" nSize: "<<nSize<<std::endl;
+ //    std::cout<<" nSize: "<<nSize<<std::endl;
      memcpy(strdata,pdata,nSize);
 
      return nSize;

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

@@ -34,6 +34,8 @@
 
 #include <QString>
 
+#include <chrono>
+
 //#include "modulecomm.h"   
 using namespace std;
 
@@ -55,8 +57,8 @@ pilot_autoware_bridge::pilot_autoware_bridge() : Node("pilot_autoware_bridge")
     pose_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>(
                 "pose", queue_size, std::bind(&pilot_autoware_bridge::callbackPose, this, _1));
 
-    detected_object_with_feature_sub_ = create_subscription<tier4_perception_msgs::msg::DetectedObjectsWithFeature>(
-                "/perception/object_recognition/detection/labeled_clusters", queue_size, std::bind(&pilot_autoware_bridge::callbackObject, this, _1));
+//    detected_object_with_feature_sub_ = create_subscription<tier4_perception_msgs::msg::DetectedObjectsWithFeature>(
+//                "/perception/object_recognition/detection/labeled_clusters", queue_size, std::bind(&pilot_autoware_bridge::callbackObject, this, _1));
 
 
     pointcloud_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/perception/obstacle_segmentation/pointcloud", durable_qos);
@@ -73,7 +75,7 @@ pilot_autoware_bridge::pilot_autoware_bridge() : Node("pilot_autoware_bridge")
     pub_acc_ = create_publisher<AccelWithCovarianceStamped>("/localization/acceleration", durable_qos);
 
     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;});
+                "/control/command/control_cmd", 1, [this](const AckermannControlCommand::SharedPtr msg) { mpcmd_ptr = msg;mbcmdupdate = true; });
 
 
     mpsharecmd =  new simplesmshare("aw_ctrlcmd",1000);
@@ -241,16 +243,23 @@ void pilot_autoware_bridge::callbackTimerGPS()
 
     int nread = mpsharegps->read(strdata,10000);
 
+    int nogpscount = 0;
+
     if(nread < 1)
     {
-
+        nogpscount++;
+        if(nogpscount >= 100)
+        {
+            std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" no gps."<<std::endl;
+            nogpscount = 0;
+        }
     }
     else
     {
 
         nSize = nread;
 
-        std::cout<<" nSize: "<<nSize<<std::endl;
+ //       std::cout<<" nSize: "<<nSize<<std::endl;
 
         iv::gps::gpsimu  xgpsimu;
         if(!xgpsimu.ParseFromArray(strdata,nSize))
@@ -313,17 +322,24 @@ void pilot_autoware_bridge::callbackTimerGPS()
             publish_acceleration();
 
 
-            std::cout<<" rec gps."<<std::endl;
+ //           std::cout<<" share gps."<<std::endl;
+            nogpscount = 0;
         }
 
     }
 
+    int nochassiscount = 0;
 
     nread = mpsharechassis->read(strdata,10000);
 
     if(nread < 1)
     {
-
+        nochassiscount++;
+        if(nochassiscount >= 100)
+        {
+            std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" no chassis."<<std::endl;
+            nochassiscount = 0;
+        }
     }
     else
     {
@@ -336,15 +352,22 @@ void pilot_autoware_bridge::callbackTimerGPS()
         else
         {
             steear_now = (xchassis.angle_feedback() *M_PI/180.0)/15.0;
-            std::cout<<" steer : "<<steear_now<<std::endl;
+            nochassiscount = 0;
+  //          std::cout<<" steer : "<<steear_now<<std::endl;
         }
 
     }
 
+    int noobjectcount = 0;
+
     nread = mpsharelidartrack->read(strdata,20000000);
     if(nread<1)
     {
-
+        noobjectcount++;
+        if(noobjectcount >= 100)
+        {
+            std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" no object."<<std::endl;
+        }
     }
     else
     {
@@ -357,6 +380,7 @@ void pilot_autoware_bridge::callbackTimerGPS()
         {
 
             ConvertObjAndPub(&xobjarray);
+            noobjectcount = 0;
         }
     }
 
@@ -370,7 +394,7 @@ void pilot_autoware_bridge::callbackTimerGPS()
 
 void pilot_autoware_bridge::callbackTimer()
 {
-    std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" testpose1. "<<std::endl;
+   // std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" testpose1. "<<std::endl;
 
 
     if(mbcmdupdate == true)
@@ -393,7 +417,7 @@ void pilot_autoware_bridge::callbackTimer()
         {
             std::cout<<" command serialize fail."<<std::endl;
         }
-        std::cout<<" stear: "<<steer<<" vel: "<<vel<<" accel: "<<accel<<std::endl;
+        std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" stear: "<<steer<<" vel: "<<vel<<" accel: "<<accel<<std::endl;
         mbcmdupdate = false;
     }
     return;
@@ -790,7 +814,7 @@ void pilot_autoware_bridge::ConvertObjAndPub(iv::lidar::objectarray * pobjarray)
 
         feature_object.object.set__shape(shape);
 
-        std::cout<<" set shape. "<<std::endl;
+ //       std::cout<<" set shape. "<<std::endl;
 
         output_dynamic_object_msg.feature_objects.push_back(feature_object);