From ddfb619d3f026d208fdc0d31c02fa027bdcbd1d8 Mon Sep 17 00:00:00 2001 From: Haneol Kim Date: Tue, 23 Jan 2024 19:10:49 +0900 Subject: [PATCH] Fix pointcloud frame rate drop --- CMakeLists.txt | 28 ++++++++++++-- config/slambox_rviz2.rviz | 53 ++++++++++++++++++++++---- docker/Dockerfile | 3 +- include/applications/driver_client.hpp | 20 +++++++++- package.xml | 4 ++ src/applications/driver_client.cpp | 42 +++++++++++++++++--- src/node_client.cpp | 5 ++- 7 files changed, 134 insertions(+), 21 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 61429f4..51fca6d 100644 --- a/CMakeLists.txt +++ b/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,6 +30,10 @@ 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) @@ -25,6 +41,9 @@ 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 $) - 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,7 +112,7 @@ 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 @@ -101,6 +120,7 @@ if(BUILD_TESTING) glog::glog ${slambox_sdk_LIBRARIES} ${YAML_CPP_LIBRARIES} + ${PCL_LIBRARIES} ) ENDFOREACH() endif() diff --git a/config/slambox_rviz2.rviz b/config/slambox_rviz2.rviz index 623d5ab..29bd9d9 100644 --- a/config/slambox_rviz2.rviz +++ b/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,8 +136,8 @@ 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 @@ -108,17 +145,17 @@ Visualization Manager: 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 diff --git a/docker/Dockerfile b/docker/Dockerfile index fc46be7..d9d77ce 100644 --- a/docker/Dockerfile +++ b/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 diff --git a/include/applications/driver_client.hpp b/include/applications/driver_client.hpp index 9ce541f..5c3faf8 100644 --- a/include/applications/driver_client.hpp +++ b/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 +#include + +#include // NOLINT +#include #include #include #include #include +#include #include #include -#include +#include #include #include @@ -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::Ptr cur_pcl_; + + /// @brief current message for metadata + sensor_msgs::msg::PointCloud2 cur_pcl_msg_; }; } // namespace sbox diff --git a/package.xml b/package.xml index 5d16078..8b6f616 100644 --- a/package.xml +++ b/package.xml @@ -17,6 +17,10 @@ std_msgs sensor_msgs nav_msgs + rclcpp_components + pcl + pcl_ros + pcl_conversions message_generation diff --git a/src/applications/driver_client.cpp b/src/applications/driver_client.cpp index 35c479d..dd34ba1 100644 --- a/src/applications/driver_client.cpp +++ b/src/applications/driver_client.cpp @@ -5,13 +5,16 @@ #include "applications/driver_client.hpp" +#include +#include +#include + #include // NOLINT #include -#include #include +#include #include -#include #include #include @@ -71,16 +74,20 @@ SLAMBOXDriverClient::SLAMBOXDriverClient(const rclcpp::NodeOptions &options) this->get_parameter_or( "subscribe/request_topic", subscribe_request_topic_, "/SLAMBOX/request"); + // rclcpp::QoS qos = rclcpp::QoS(rclcpp::KeepLast(20)).best_effort(); + odom_pub_ = - this->create_publisher(publish_odom_topic_, 1); + this->create_publisher(publish_odom_topic_, 10); pointcloud_pub_ = this->create_publisher( - publish_pointcloud_topic_, 1); + publish_pointcloud_topic_, rclcpp::SystemDefaultsQoS()); request_sub_ = this->create_subscription( subscribe_request_topic_, 1, std::bind(&SLAMBOXDriverClient::callback_request_, this, std::placeholders::_1)); + cur_pcl_ = std::make_shared>(); + 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::Ptr cloud = + std::make_shared>(); + 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 requested_mode, bool SLAMBOXDriverClient::is_server_alive() { std::chrono::duration dt = std::chrono::system_clock::now() - last_ping_time_; + // LOG(INFO) << "Server live check: " << dt.count(); if (dt.count() > 3.0) { return false; } diff --git a/src/node_client.cpp b/src/node_client.cpp index 0e6938b..4caf1d0 100644 --- a/src/node_client.cpp +++ b/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(); - rclcpp::spin(slambox_client_node); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(slambox_client_node); + + executor.spin(); rclcpp::shutdown(); return 0; }