Change pointcloud publish method

CMakeLists.txt

@@ -16,6 +16,9 @@ find_package(catkin REQUIRED COMPONENTS
 find_package(glog 0.6.0 REQUIRED)
 find_package(ZLIB REQUIRED)
 find_package(yaml-cpp REQUIRED )
+find_package(PCL REQUIRED)
+find_package(pcl_ros REQUIRED)
+find_package(pcl_conversions REQUIRED)
 
 find_package(slambox_sdk 0.2.0 REQUIRED)
 
@@ -27,7 +30,10 @@ include_directories(
   include
   ${catkin_INCLUDE_DIRS}
 )
-include_directories(${YAML_CPP_INCLUDE_DIRS})
+include_directories(
+  ${YAML_CPP_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+)
 
 file(GLOB_RECURSE SOURCES CONFIGURE_DEPENDS "${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp")
 file(GLOB SOURCES_EXECUTABLE "${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp")
@@ -54,6 +60,7 @@ FOREACH(source_executable ${SOURCES_EXECUTABLE})
     ZLIB::ZLIB
     fmt::fmt
     ${YAML_CPP_LIBRARIES}
+    ${PCL_LIBRARIES}
   )
 ENDFOREACH()
 
@@ -86,6 +93,7 @@ if (CATKIN_ENABLE_TESTING)
             ${slambox_sdk_LIBRARIES}
             fmt::fmt
             ${YAML_CPP_LIBRARIES}
+            ${PCL_LIBRARIES}
         )
 
         add_test(NAME ${TEST_NAME} COMMAND ${TEST_NAME})

docker/Dockerfile

@@ -62,6 +62,8 @@ RUN git clone https://github.com/doxygen/doxygen.git -b Release_1_9_8 \
 RUN sudo apt-get install -y ros-noetic-catkin python3-catkin-tools \
     && sudo apt-get purge -y libtbb-dev
 
+RUN sudo apt-get install -y libpcl-dev libeigen3-dev ros-noetic-pcl-ros
+
 RUN sudo usermod -aG dialout user
 
 RUN echo "export LD_LIBRARY_PATH=\$LD_LIBRARY_PATH:/home/user/.local/lib" >> /home/user/.bashrc

include/applications/driver_client.hpp

@@ -6,7 +6,11 @@
 #ifndef SLAMBOX_ROS_INCLUDE_APPLICATIONS_DRIVER_CLIENT_HPP_
 #define SLAMBOX_ROS_INCLUDE_APPLICATIONS_DRIVER_CLIENT_HPP_
 
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
 #include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
 #include <std_msgs/String.h>
 
 #include <memory>
@@ -157,6 +161,22 @@ class SLAMBOXDriverClient : public ParsedMessageInterface {
 
   /// @brief Last ping time. This is used to check if server is alive.
   ros::Time last_ping_time_;
+
+  /// @brief concat point cloud and publish to ros
+  /// @param msg ROS sensor message of pointcloud
+  void concat_pub_pcl_(sensor_msgs::PointCloud2 msg);
+
+  /// @brief check it is the first call
+  bool have_called_ = false;
+
+  /// @brief ros time from message timestamp
+  ros::Time cur_pcl_timestamp_;
+
+  /// @brief current pointcloud
+  pcl::PointCloud<pcl::PointXYZ> cur_pcl_;
+
+  /// @brief current message
+  sensor_msgs::PointCloud2 cur_msg_;
 };
 
 }  // namespace sbox

package.xml

@@ -15,6 +15,8 @@
 
   <depend>roscpp</depend>
   <depend>std_msgs</depend>
+  <depend>pcl_ros</depend>
+  <depend>pcl_conversions</depend>
 
   <build_depend>message_generation</build_depend>
 

src/applications/driver_client.cpp

@@ -5,6 +5,10 @@
 
 #include "applications/driver_client.hpp"
 
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
 #include <nav_msgs/Odometry.h>
 #include <sensor_msgs/PointCloud2.h>
 #include <std_msgs/String.h>
@@ -53,6 +57,8 @@ SLAMBOXDriverClient::SLAMBOXDriverClient(ros::NodeHandle nh,
   request_sub_ = nh_.subscribe(subscribe_request_topic_, 1,
                                &SLAMBOXDriverClient::callback_request_, this);
 
+  cur_pcl_ = pcl::PointCloud<pcl::PointXYZ>();
+
   if (is_serial_enabled_) {
     serial_communication_ = std::make_unique<SerialCommunication>(
         serial_port_name_, serial_baud_rate_);
@@ -123,26 +129,31 @@ void SLAMBOXDriverClient::on_push_odometry(const sbox_msgs::Odometry &odom) {
 void SLAMBOXDriverClient::on_push_pointcloud(
     const sbox_msgs::PointCloud2 &pointcloud) {
   sensor_msgs::PointCloud2 pointcloud_msg = sbox_msgs::to_ros_msg(pointcloud);
-  pointcloud_pub_.publish(pointcloud_msg);
+  this->concat_pub_pcl_(pointcloud_msg);
+  // pointcloud_pub_.publish(pointcloud_msg);
 }
 
+// cppcheck-suppress unusedFunction
 void SLAMBOXDriverClient::on_response_mavlink_communication_config(
     bool enabled, uint32_t baudrate) {
   LOG(INFO) << "[mavlink] " << (enabled ? "Enabled" : "Disabled")
             << ", baudrate: " << baudrate << std::endl;
 }
 
+// cppcheck-suppress unusedFunction
 void SLAMBOXDriverClient::on_response_serial_communication_config(
     bool enabled, uint32_t baudrate) {
   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) {
   LOG(INFO) << "[ethernet] " << (enabled ? "Enabled" : "Disabled")
             << ", port: " << port << std::endl;
 }
 
+// cppcheck-suppress unusedFunction
 void SLAMBOXDriverClient::on_acknowledge(std::array<uint8_t, 2> requested_mode,
                                          uint8_t status) {
   LOG(INFO) << fmt::format(
@@ -199,4 +210,30 @@ void SLAMBOXDriverClient::callback_request_(const std_msgs::String &msg) {
     udp_communication_->write(data);
   }
 }
+
+void SLAMBOXDriverClient::concat_pub_pcl_(sensor_msgs::PointCloud2 msg) {
+  pcl::PointCloud<pcl::PointXYZ> cloud_dst;
+  if (!have_called_) {
+    have_called_ = true;
+    cur_pcl_timestamp_ = msg.header.stamp;
+    cur_msg_ = msg;
+    pcl::fromROSMsg(msg, this->cur_pcl_);
+  }
+
+  if (cur_pcl_timestamp_ == msg.header.stamp) {
+    // Convert msg to pointcloud
+    pcl::fromROSMsg(msg, cloud_dst);
+    this->cur_pcl_ += cloud_dst;
+  } else {
+    sensor_msgs::PointCloud2 temp;
+    pcl::toROSMsg(this->cur_pcl_, temp);
+    temp.header.stamp = cur_pcl_timestamp_;
+    pointcloud_pub_.publish(temp);
+
+    cur_pcl_timestamp_ = msg.header.stamp;
+    cur_msg_ = msg;
+    // Convert msg to pointcloud
+    pcl::fromROSMsg(msg, this->cur_pcl_);
+  }
+}
 }  // namespace sbox