Update ROS1 version (#6)

* Support for gpstime and seamscontroller * Fixed the program to publish to diagnostics. * Fixed the program * rm respawn * pandar decoder. edge case when sensor returns timestamps larger than 2^32 Signed-off-by: amc-nu <[email protected]> --------- Signed-off-by: amc-nu <[email protected]> Co-authored-by: Takuro Suzuki <[email protected]> Co-authored-by: yudai.yamazaki <[email protected]>
MapIV · Sep 3, 2024 · 39f50e7 · 39f50e7
1 parent fd7cf23
commit 39f50e7
Show file tree

Hide file tree

Showing 5 changed files with 67 additions and 6 deletions.
diff --git a/pandar_monitor/include/pandar_monitor/pandar_monitor.hpp b/pandar_monitor/include/pandar_monitor/pandar_monitor.hpp
@@ -42,6 +42,12 @@ class PandarMonitor
   void checkPTP(
     diagnostic_updater::DiagnosticStatusWrapper & stat);
 
+  void checkGPSPPS(
+    diagnostic_updater::DiagnosticStatusWrapper & stat);
+
+  void checkGPSGPRMC(
+    diagnostic_updater::DiagnosticStatusWrapper & stat);
+
   void onTimer(const ros::TimerEvent & event);
 
   ros::NodeHandle nh_{""};
@@ -85,6 +91,12 @@ class PandarMonitor
     "locked",
     "frozen",
   };
+
+  const char *gps_message_[3] = {
+    "unlocked",
+    "locked",
+    "warning"
+  };
 };
 
 #endif  // PANDAR_MONITOR_PANDAR_MONITOR_H_
diff --git a/pandar_monitor/launch/pandar_monitor.launch b/pandar_monitor/launch/pandar_monitor.launch
@@ -1,10 +1,10 @@
 <launch>
   <arg name="config_file" default="$(find pandar_monitor)/config/pandar_monitor.yaml"/>
-  <arg name="ip_address" default="192.168.1.204" />
-  <arg name="output" default="log"/>
+  <arg name="ip_address" default="192.168.1.201" />
+  <arg name="output" default="screen"/>
 
-  <node pkg="pandar_monitor" type="pandar_monitor" name="pandar_monitor" output="$(arg output)" respawn="true">
+  <node pkg="pandar_monitor" type="pandar_monitor" name="pandar_monitor" output="$(arg output)">
     <rosparam command="load" file="$(arg config_file)"/>
     <param name="ip_address" value="$(arg ip_address)" />
-  </node>/>
+  </node>
 </launch>
diff --git a/pandar_monitor/src/pandar_monitor.cpp b/pandar_monitor/src/pandar_monitor.cpp
@@ -33,6 +33,8 @@ PandarMonitor::PandarMonitor()
   updater_.add("pandar_connection", this, &PandarMonitor::checkConnection);
   updater_.add("pandar_temperature", this, &PandarMonitor::checkTemperature);
   updater_.add("pandar_ptp", this, &PandarMonitor::checkPTP);
+  updater_.add("pandar_gpspps",this, &PandarMonitor::checkGPSPPS);
+  updater_.add("pandar_gpsgprmc",this, &PandarMonitor::checkGPSGPRMC);
 
   client_ = std::make_unique<pandar_api::TCPClient>(ip_address_, static_cast<int>(timeout_ * 1000));
 
@@ -112,3 +114,47 @@ void PandarMonitor::checkPTP(diagnostic_updater::DiagnosticStatusWrapper & stat)
 }
 
 void PandarMonitor::onTimer(const ros::TimerEvent & event) { updater_.force_update(); }
+
+void PandarMonitor::checkGPSPPS(diagnostic_updater::DiagnosticStatusWrapper & stat)
+{
+  /* get LiDAR status*/
+  pandar_api::LidarStatus status;
+  auto code = client_->getLidarStatus(status);
+  if (code == pandar_api::TCPClient::ReturnCode::SUCCESS)
+  {
+    if(status.gps_pps_lock == 0)
+    {
+      stat.summary(DiagStatus::ERROR, gps_message_[0]);
+    }
+    else if(status.gps_pps_lock == 1)
+    {
+      stat.summary(DiagStatus::OK, gps_message_[1]);
+    }
+    else
+    {
+      stat.summary(DiagStatus::WARN, gps_message_[2]);
+    }
+  }
+}
+
+void PandarMonitor::checkGPSGPRMC(diagnostic_updater::DiagnosticStatusWrapper & stat)
+{
+  /* get LiDAR status*/
+  pandar_api::LidarStatus status;
+  auto code = client_->getLidarStatus(status);
+  if (code == pandar_api::TCPClient::ReturnCode::SUCCESS)
+  {
+    if(status.gps_gprmc_status == 0)
+    {
+      stat.summary(DiagStatus::ERROR, gps_message_[0]);
+    }
+    else if(status.gps_gprmc_status == 1)
+    {
+      stat.summary(DiagStatus::OK, gps_message_[1]);
+    }
+    else
+    {
+      stat.summary(DiagStatus::WARN, gps_message_[2]);
+    }
+  }
+}
diff --git a/pandar_pointcloud/include/pandar_pointcloud/pandar_cloud.hpp b/pandar_pointcloud/include/pandar_pointcloud/pandar_cloud.hpp
@@ -18,7 +18,7 @@ class PandarCloud
 public:
   PandarCloud(ros::NodeHandle node, ros::NodeHandle private_nh);
   ~PandarCloud();
-
+  const double MAX_ROS_TIME = 4294967296.;//edge case when ROS Time exceeds 2^32
 private:
   bool setupCalibration();
   void onProcessScan(const pandar_msgs::PandarScan::ConstPtr& msg);

diff --git a/pandar_pointcloud/src/pandar_cloud.cpp b/pandar_pointcloud/src/pandar_cloud.cpp
@@ -11,6 +11,7 @@
 
 #include <chrono>
 #include <thread>
+#include <limits>
 
 namespace
 {
@@ -216,7 +217,9 @@ void PandarCloud::onProcessScan(const pandar_msgs::PandarScan::ConstPtr& scan_ms
     decoder_->unpack(packet);
     if (decoder_->hasScanned()) {
       pointcloud = decoder_->getPointcloud();
-      if (pointcloud->points.size() > 0) {
+      if (pointcloud->points.size() > 0 
+          && (pointcloud->points[0].time_stamp < PandarCloud::MAX_ROS_TIME)
+        ) {
         pointcloud->header.stamp = pcl_conversions::toPCL(ros::Time(pointcloud->points[0].time_stamp));
         pointcloud->header.frame_id = scan_msg->header.frame_id;
         pointcloud->height = 1;