Refactor directory and namespace

CMakeLists.txt

@@ -17,14 +17,20 @@ catkin_package(
 add_compile_options(-std=c++11)
 include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} include)
 
-add_executable(urg_stamped src/urg_stamped.cpp src/scip2/logger.cpp src/ros_logger.cpp)
+add_executable(urg_stamped
+  src/urg_stamped.cpp
+  src/urg_stamped_node.cpp
+  src/scip2/logger.cpp
+  src/ros_logger.cpp
+)
 target_link_libraries(urg_stamped ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 
 
 if(CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)
   find_package(rosunit REQUIRED)
   find_package(roslint REQUIRED)
+  set(ROSLINT_CPP_OPTS "--filter=-runtime/references,-build/c++11")
   roslint_cpp()
   roslint_add_test()
 

include/device_time_origin.h → include/urg_stamped/device_time_origin.h

@@ -1,5 +1,5 @@
 /*
- * Copyright 2020 The urg_stamped Authors
+ * Copyright 2020-2021 The urg_stamped Authors
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -14,13 +14,15 @@
  * limitations under the License.
  */
 
-#ifndef DEVICE_TIME_ORIGIN_H
-#define DEVICE_TIME_ORIGIN_H
+#ifndef URG_STAMPED_DEVICE_TIME_ORIGIN_H
+#define URG_STAMPED_DEVICE_TIME_ORIGIN_H
 
 #include <ros/ros.h>
 
 #include <cmath>
 
+namespace urg_stamped
+{
 namespace device_time_origin
 {
 class DriftedTime
@@ -81,5 +83,6 @@ inline bool detectTimeJump(
 }
 }  // namespace jump_detector
 }  // namespace device_time_origin
+}  // namespace urg_stamped
 
-#endif  // DEVICE_TIME_ORIGIN_H
+#endif  // URG_STAMPED_DEVICE_TIME_ORIGIN_H

include/first_order_filter.h → include/urg_stamped/first_order_filter.h

@@ -1,5 +1,5 @@
 /*
- * Copyright 2018 The urg_stamped Authors
+ * Copyright 2018-2021 The urg_stamped Authors
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -14,9 +14,11 @@
  * limitations under the License.
  */
 
-#ifndef FIRST_ORDER_FILTER_H
-#define FIRST_ORDER_FILTER_H
+#ifndef URG_STAMPED_FIRST_ORDER_FILTER_H
+#define URG_STAMPED_FIRST_ORDER_FILTER_H
 
+namespace urg_stamped
+{
 template <typename FLT>
 class FirstOrderFilter
 {
@@ -66,5 +68,6 @@ class FirstOrderHPF : public FirstOrderFilter<FLT>
     this->k_[0] = 2 * time_constant * (-this->k_[1] + 1.0);
   }
 };
+}  // namespace urg_stamped
 
-#endif  // FIRST_ORDER_FILTER_H
+#endif  // URG_STAMPED_FIRST_ORDER_FILTER_H

include/ros_logger.h → include/urg_stamped/ros_logger.h

@@ -1,5 +1,5 @@
 /*
- * Copyright 2019 The urg_stamped Authors
+ * Copyright 2019-2021 The urg_stamped Authors
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -14,8 +14,8 @@
  * limitations under the License.
  */
 
-#ifndef ROS_LOGGER_H
-#define ROS_LOGGER_H
+#ifndef URG_STAMPED_ROS_LOGGER_H
+#define URG_STAMPED_ROS_LOGGER_H
 
 #include <iostream>
 
@@ -24,4 +24,4 @@ namespace urg_stamped
 void setROSLogger();
 }  // namespace urg_stamped
 
-#endif  // ROS_LOGGER_H
+#endif  // URG_STAMPED_ROS_LOGGER_H

include/timestamp_moving_average.h → include/urg_stamped/timestamp_moving_average.h

@@ -1,5 +1,5 @@
 /*
- * Copyright 2018 The urg_stamped Authors
+ * Copyright 2018-2021 The urg_stamped Authors
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -14,14 +14,16 @@
  * limitations under the License.
  */
 
-#ifndef TIMESTAMP_MOVING_AVERAGE_H
-#define TIMESTAMP_MOVING_AVERAGE_H
+#ifndef URG_STAMPED_TIMESTAMP_MOVING_AVERAGE_H
+#define URG_STAMPED_TIMESTAMP_MOVING_AVERAGE_H
 
 #include <ros/ros.h>
 
 #include <cmath>
 #include <vector>
 
+namespace urg_stamped
+{
 class TimestampMovingAverage
 {
 protected:
@@ -63,5 +65,6 @@ class TimestampMovingAverage
     pos_ = 0;
   }
 };
+}  // namespace urg_stamped
 
-#endif  // TIMESTAMP_MOVING_AVERAGE_H
+#endif  // URG_STAMPED_TIMESTAMP_MOVING_AVERAGE_H

include/timestamp_outlier_remover.h → include/urg_stamped/timestamp_outlier_remover.h

@@ -1,5 +1,5 @@
 /*
- * Copyright 2018 The urg_stamped Authors
+ * Copyright 2018-2021 The urg_stamped Authors
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -14,13 +14,15 @@
  * limitations under the License.
  */
 
-#ifndef TIMESTAMP_OUTLIER_REMOVER_H
-#define TIMESTAMP_OUTLIER_REMOVER_H
+#ifndef URG_STAMPED_TIMESTAMP_OUTLIER_REMOVER_H
+#define URG_STAMPED_TIMESTAMP_OUTLIER_REMOVER_H
 
 #include <ros/ros.h>
 
 #include <cmath>
 
+namespace urg_stamped
+{
 class TimestampOutlierRemover
 {
 protected:
@@ -72,5 +74,6 @@ class TimestampOutlierRemover
     stamp_ = ros::Time();
   }
 };
+}  // namespace urg_stamped
 
-#endif  // TIMESTAMP_OUTLIER_REMOVER_H
+#endif  // URG_STAMPED_TIMESTAMP_OUTLIER_REMOVER_H

include/urg_stamped/urg_stamped.h

@@ -0,0 +1,158 @@
+/*
+ * Copyright 2018-2021 The urg_stamped Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef URG_STAMPED_URG_STAMPED_H
+#define URG_STAMPED_URG_STAMPED_H
+
+#include <ros/ros.h>
+#include <sensor_msgs/LaserScan.h>
+
+#include <boost/bind/bind.hpp>
+#include <boost/format.hpp>
+#include <boost/thread.hpp>
+
+#include <algorithm>
+#include <list>
+#include <map>
+#include <random>
+#include <string>
+#include <vector>
+
+#include <scip2/scip2.h>
+#include <scip2/walltime.h>
+
+#include <urg_stamped/device_time_origin.h>
+#include <urg_stamped/first_order_filter.h>
+#include <urg_stamped/timestamp_moving_average.h>
+#include <urg_stamped/timestamp_outlier_remover.h>
+#include <urg_stamped/ros_logger.h>
+
+namespace urg_stamped
+{
+class UrgStampedNode
+{
+protected:
+  ros::NodeHandle nh_;
+  ros::NodeHandle pnh_;
+  ros::Publisher pub_scan_;
+  ros::Timer timer_sync_;
+  ros::Timer timer_delay_estim_;
+  ros::Timer timer_try_tm_;
+
+  sensor_msgs::LaserScan msg_base_;
+  uint32_t step_min_;
+  uint32_t step_max_;
+
+  scip2::Connection::Ptr device_;
+  scip2::Protocol::Ptr scip_;
+
+  bool publish_intensity_;
+
+  boost::posix_time::ptime time_tm_request;
+  std::list<ros::Duration> communication_delays_;
+  std::list<ros::Time> device_time_origins_;
+  ros::Duration estimated_communication_delay_;
+  size_t tm_iter_num_;
+  size_t tm_median_window_;
+  bool estimated_communication_delay_init_;
+  double communication_delay_filter_alpha_;
+
+  boost::posix_time::ptime time_ii_request;
+  std::vector<ros::Duration> on_scan_communication_delays_;
+
+  device_time_origin::DriftedTime device_time_origin_;
+  double allowed_device_time_origin_diff_;
+
+  scip2::Walltime<24> walltime_;
+
+  std::default_random_engine random_engine_;
+  std::uniform_real_distribution<double> sync_interval_;
+  ros::Time last_sync_time_;
+
+  ros::Time t0_;
+  FirstOrderLPF<double> timestamp_lpf_;
+  FirstOrderHPF<double> timestamp_hpf_;
+  TimestampOutlierRemover timestamp_outlier_removal_;
+  TimestampMovingAverage timestamp_moving_average_;
+
+  struct ResponseErrorCount
+  {
+    inline ResponseErrorCount()
+      : abnormal_error(0)
+      , error(0)
+    {
+    }
+    int abnormal_error;
+    int error;
+  };
+  ResponseErrorCount error_count_;
+  int error_count_max_;
+
+  void cbM(
+      const boost::posix_time::ptime& time_read,
+      const std::string& echo_back,
+      const std::string& status,
+      const scip2::ScanData& scan,
+      const bool has_intensity);
+  void cbTMSend(const boost::posix_time::ptime& time_send);
+  void cbTM(
+      const boost::posix_time::ptime& time_read,
+      const std::string& echo_back,
+      const std::string& status,
+      const scip2::Timestamp& time_device);
+  void cbPP(
+      const boost::posix_time::ptime& time_read,
+      const std::string& echo_back,
+      const std::string& status,
+      const std::map<std::string, std::string>& params);
+  void cbVV(
+      const boost::posix_time::ptime& time_read,
+      const std::string& echo_back,
+      const std::string& status,
+      const std::map<std::string, std::string>& params);
+  void cbIISend(const boost::posix_time::ptime& time_send);
+  void cbII(
+      const boost::posix_time::ptime& time_read,
+      const std::string& echo_back,
+      const std::string& status,
+      const std::map<std::string, std::string>& params);
+  void cbQT(
+      const boost::posix_time::ptime& time_read,
+      const std::string& echo_back,
+      const std::string& status);
+  void cbRB(
+      const boost::posix_time::ptime& time_read,
+      const std::string& echo_back,
+      const std::string& status);
+  void cbConnect();
+
+  void timeSync(const ros::TimerEvent& event = ros::TimerEvent());
+  void delayEstimation(const ros::TimerEvent& event = ros::TimerEvent());
+  void tryTM(const ros::TimerEvent& event = ros::TimerEvent());
+
+  void errorCountIncrement(const std::string& status);
+
+  bool detectDeviceTimeJump(
+      const boost::posix_time::ptime& time_response,
+      const uint64_t& device_timestamp);
+
+public:
+  UrgStampedNode();
+  void spin();
+};
+}  // namespace urg_stamped
+
+#endif  // URG_STAMPED_URG_STAMPED_H

src/ros_logger.cpp

@@ -1,5 +1,5 @@
 /*
- * Copyright 2019 The urg_stamped Authors
+ * Copyright 2019-2021 The urg_stamped Authors
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -22,8 +22,6 @@
 
 #include <scip2/logger.h>
 
-namespace urg_stamped
-{
 namespace
 {
 class ROSOutStreamBuffer : public std::stringbuf
@@ -84,6 +82,8 @@ std::ostream error_logger(&error_buf);
 std::ostream fatal_logger(&fatal_buf);
 }  // namespace
 
+namespace urg_stamped
+{
 void setROSLogger()
 {
   scip2::logger::setDebugLogger(&debug_logger);