Fix pointcloud frame rate drop

CMakeLists.txt

@@ -4,8 +4,20 @@ project(slambox_ros2)
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON CACHE INTERNAL "")
 
 ## Compile as C++17, supported in ROS Kinetic and newer
-add_compile_options(-std=c++17 -O3)
+# add_compile_options(-std=c++17 -O3)
+
+if (NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+set(CMAKE_CXX_EXTENSIONS OFF)
 set(BUILD_SHARED_LIBS OFF)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -fexceptions -g -ggdb")
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
@@ -18,13 +30,20 @@ find_package(nav_msgs REQUIRED)
 find_package(glog 0.6.0 REQUIRED)
 find_package(ZLIB REQUIRED)
 find_package(yaml-cpp REQUIRED )
+find_package(rclcpp_components REQUIRED)
+find_package(PCL REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(pcl_ros REQUIRED)
 
 find_package(slambox_sdk 0.2.0 REQUIRED)
 
 include_directories(
   include
 )
 include_directories(${YAML_CPP_INCLUDE_DIRS})
+include_directories(${EIGEN3_INCLUDE_DIR} ${PCL_INCLUDE_DIRS} ${pcl_conversions_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
 
 file(GLOB_RECURSE SOURCES CONFIGURE_DEPENDS "${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp")
 file(GLOB SOURCES_EXECUTABLE "${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp")
@@ -45,17 +64,17 @@ FOREACH(source_executable ${SOURCES_EXECUTABLE})
   )
 
   target_link_libraries(${executable_name}
-    ${catkin_LIBRARIES}
     glog::glog
     ${slambox_sdk_LIBRARIES}
     ZLIB::ZLIB
     ${YAML_CPP_LIBRARIES}
+    ${PCL_LIBRARIES}
   )
 
   target_include_directories(${executable_name} PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include
     $<INSTALL_INTERFACE:include>)
 
-  ament_target_dependencies(${executable_name} rclcpp std_msgs sensor_msgs nav_msgs)
+  ament_target_dependencies(${executable_name} rclcpp std_msgs sensor_msgs nav_msgs rclcpp_components pcl_ros pcl_conversions PCL)
 
   install(TARGETS
     ${executable_name}
@@ -93,14 +112,15 @@ if(BUILD_TESTING)
 
     ament_add_gtest(${TEST_NAME} ${unittest_path} ${SOURCES})
 
-    ament_target_dependencies(${TEST_NAME} nav_msgs std_msgs sensor_msgs rclcpp)
+    ament_target_dependencies(${TEST_NAME} nav_msgs std_msgs sensor_msgs rclcpp PCL)
 
     target_link_libraries(${TEST_NAME}
       gtest
       gtest_main
       glog::glog
       ${slambox_sdk_LIBRARIES}
       ${YAML_CPP_LIBRARIES}
+      ${PCL_LIBRARIES}
     )
   ENDFOREACH()
 endif()

config/slambox_rviz2.rviz

@@ -9,6 +9,9 @@ Panels:
         - /Odometry1
         - /Odometry1/Shape1
         - /PointCloud21
+        - /PointCloud21/Topic1
+        - /PointCloud22
+        - /PointCloud22/Topic1
       Splitter Ratio: 0.5
     Tree Height: 1555
   - Class: rviz_common/Selection
@@ -28,7 +31,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: ""
+    SyncSource: PointCloud2
 Visualization Manager:
   Class: ""
   Displays:
@@ -68,12 +71,12 @@ Visualization Manager:
           Value: true
         Value: true
       Enabled: true
-      Keep: 100
+      Keep: 1
       Name: Odometry
       Position Tolerance: 0.10000000149011612
       Shape:
         Alpha: 1
-        Axes Length: 0.5
+        Axes Length: 1
         Axes Radius: 0.10000000149011612
         Color: 255; 25; 0
         Head Length: 0.30000001192092896
@@ -89,6 +92,40 @@ Visualization Manager:
         Reliability Policy: Reliable
         Value: /SLAMBOX/odom
       Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 2.9292795658111572
+        Min Value: 0.539630651473999
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: AxisColor
+      Decay Time: 0.10000000149011612
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.10000000149011612
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Best Effort
+        Value: /SLAMBOX/pointcloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -99,26 +136,26 @@ Visualization Manager:
       Channel Name: intensity
       Class: rviz_default_plugins/PointCloud2
       Color: 255; 255; 255
-      Color Transformer: ""
-      Decay Time: 250
+      Color Transformer: Intensity
+      Decay Time: 10
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 4096
       Min Color: 0; 0; 0
       Min Intensity: 0
       Name: PointCloud2
-      Position Transformer: ""
+      Position Transformer: XYZ
       Selectable: true
       Size (Pixels): 3
-      Size (m): 0.25
+      Size (m): 0.009999999776482582
       Style: Flat Squares
       Topic:
         Depth: 5
         Durability Policy: Volatile
         Filter size: 10
         History Policy: Keep Last
-        Reliability Policy: Reliable
+        Reliability Policy: Best Effort
         Value: /SLAMBOX/pointcloud
       Use Fixed Frame: true
       Use rainbow: true

docker/Dockerfile

@@ -59,7 +59,8 @@ RUN git clone https://github.com/doxygen/doxygen.git -b Release_1_9_8 \
     && make -j \
     && sudo make install
 
-RUN sudo apt-get purge -y libtbb-dev
+RUN sudo apt-get purge -y libtbb-dev && \
+    sudo apt-get install -y ros-humble-pcl-ros ros-humble-pcl-conversions libpcl-dev libeigen3-dev
 
 RUN sudo usermod -aG dialout user
 

include/applications/driver_client.hpp

@@ -6,14 +6,20 @@
 #ifndef SLAMBOX_ROS2_INCLUDE_APPLICATIONS_DRIVER_CLIENT_HPP_
 #define SLAMBOX_ROS2_INCLUDE_APPLICATIONS_DRIVER_CLIENT_HPP_
 
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <chrono>  // NOLINT
+#include <deque>
 #include <memory>
 #include <string>
 #include <vector>
 
 #include <nav_msgs/msg/odometry.hpp>
+#include <rclcpp/duration.hpp>
 #include <rclcpp/publisher.hpp>
 #include <rclcpp/rclcpp.hpp>
-#include <sensor_msgs/msg/detail/point_cloud2__struct.hpp>
+#include <rclcpp/time.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <std_msgs/msg/string.hpp>
 
@@ -152,8 +158,20 @@ class SLAMBOXDriverClient : public ParsedMessageInterface, public rclcpp::Node {
   /// @brief Last ping time. This is used to check if server is alive.
   std::chrono::system_clock::time_point last_ping_time_;
 
+  /// @brief check the function is called
+  bool have_called_ = false;
+
   /// @brief ROS2 clock
   rclcpp::Clock::SharedPtr clock_;
+
+  /// @brief timestamp
+  rclcpp::Time cur_pcl_timestamp_;
+
+  /// @brief current timestamp pointcloud
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cur_pcl_;
+
+  /// @brief current message for metadata
+  sensor_msgs::msg::PointCloud2 cur_pcl_msg_;
 };
 
 }  // namespace sbox

package.xml

@@ -17,6 +17,10 @@
   <depend>std_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>nav_msgs</depend>
+  <depend>rclcpp_components</depend>
+  <depend>pcl</depend>
+  <depend>pcl_ros</depend>
+  <depend>pcl_conversions</depend>
 
   <build_depend>message_generation</build_depend>
 

src/applications/driver_client.cpp

@@ -5,13 +5,16 @@
 
 #include "applications/driver_client.hpp"
 
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
 #include <chrono>  // NOLINT
 #include <memory>
 
-#include <nav_msgs/msg/detail/odometry__struct.hpp>
 #include <nav_msgs/msg/odometry.hpp>
+#include <rclcpp/qos.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
-#include <std_msgs/msg/detail/string__struct.hpp>
 #include <std_msgs/msg/string.hpp>
 
 #include <sbox/communication/serial_communication.hpp>
@@ -71,16 +74,20 @@ SLAMBOXDriverClient::SLAMBOXDriverClient(const rclcpp::NodeOptions &options)
   this->get_parameter_or<std::string>(
       "subscribe/request_topic", subscribe_request_topic_, "/SLAMBOX/request");
 
+  // rclcpp::QoS qos = rclcpp::QoS(rclcpp::KeepLast(20)).best_effort();
+
   odom_pub_ =
-      this->create_publisher<nav_msgs::msg::Odometry>(publish_odom_topic_, 1);
+      this->create_publisher<nav_msgs::msg::Odometry>(publish_odom_topic_, 10);
   pointcloud_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(
-      publish_pointcloud_topic_, 1);
+      publish_pointcloud_topic_, rclcpp::SystemDefaultsQoS());
 
   request_sub_ = this->create_subscription<std_msgs::msg::String>(
       subscribe_request_topic_, 1,
       std::bind(&SLAMBOXDriverClient::callback_request_, this,
                 std::placeholders::_1));
 
+  cur_pcl_ = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+
   clock_ = this->get_clock();
 
   if (is_serial_enabled_) {
@@ -154,7 +161,29 @@ void SLAMBOXDriverClient::on_push_pointcloud(
     const sbox_msgs::PointCloud2 &pointcloud) {
   sensor_msgs::msg::PointCloud2 pointcloud_msg =
       sbox_msgs::to_ros_msg(pointcloud);
-  pointcloud_pub_->publish(pointcloud_msg);
+
+  if (!have_called_) {
+    cur_pcl_timestamp_ = pointcloud_msg.header.stamp;
+    pcl::fromROSMsg(pointcloud_msg, *this->cur_pcl_);
+    this->cur_pcl_msg_ = pointcloud_msg;
+    have_called_ = true;
+  }
+  if (cur_pcl_timestamp_ == pointcloud_msg.header.stamp) {
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud =
+        std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+    pcl::fromROSMsg(pointcloud_msg, *cloud);
+    *this->cur_pcl_ += *cloud;
+  } else {
+    cur_pcl_timestamp_ = pointcloud_msg.header.stamp;
+    sensor_msgs::msg::PointCloud2 concat_pcl_msg;
+    pcl::toROSMsg(*this->cur_pcl_, concat_pcl_msg);
+    concat_pcl_msg.header = cur_pcl_msg_.header;
+    pointcloud_pub_->publish(concat_pcl_msg);
+
+    // cur_pcl_.reset();
+    pcl::fromROSMsg(pointcloud_msg, *this->cur_pcl_);
+    this->cur_pcl_msg_ = pointcloud_msg;
+  }
 }
 
 // cppcheck-suppress unusedFunction
@@ -170,7 +199,7 @@ void SLAMBOXDriverClient::on_response_serial_communication_config(
   LOG(INFO) << "[serial] " << (enabled ? "Enabled" : "Disabled")
             << ", baudrate: " << baudrate << std::endl;
 }
-//
+
 // cppcheck-suppress unusedFunction
 void SLAMBOXDriverClient::on_response_ethernet_communication_config(
     bool enabled, uint32_t port) {
@@ -192,6 +221,7 @@ void SLAMBOXDriverClient::on_acknowledge(std::array<uint8_t, 2> requested_mode,
 bool SLAMBOXDriverClient::is_server_alive() {
   std::chrono::duration<double> dt =
       std::chrono::system_clock::now() - last_ping_time_;
+  // LOG(INFO) << "Server live check: " << dt.count();
   if (dt.count() > 3.0) {
     return false;
   }

src/node_client.cpp

@@ -21,7 +21,10 @@ int main(int argc, char **argv) {
   rclcpp::init(argc, argv);
   auto slambox_client_node = std::make_shared<sbox::SLAMBOXDriverClient>();
 
-  rclcpp::spin(slambox_client_node);
+  rclcpp::executors::MultiThreadedExecutor executor;
+  executor.add_node(slambox_client_node);
+
+  executor.spin();
   rclcpp::shutdown();
   return 0;
 }