Merge pull request #162 from namiota/ros-queue-length-as-param

Added ros_queue_length parameter

Author: FelixHuang18

config/config.yaml

@@ -41,3 +41,4 @@ lidar:
       ros_send_packet_topic: /rslidar_packets          #Topic used to send lidar packets through ROS
       ros_send_imu_data_topic: /rslidar_imu_data         #Topic used to send imu data through ROS
       ros_send_point_cloud_topic: /rslidar_points      #Topic used to send point cloud through ROS
+      ros_queue_length: 100                            #Topic QoS history depth

src/source/source_packet_ros.hpp

@@ -326,12 +326,15 @@ inline void DestinationPacketRos::init(const YAML::Node& config)
   yamlRead<std::string>(config["ros"], "ros_send_packet_topic", 
       ros_send_topic, "rslidar_packets");
 
+  size_t ros_queue_length;
+  yamlRead<size_t>(config["ros"], "ros_queue_length", ros_queue_length, 100);
+
   static int node_index = 0;
   std::stringstream node_name;
   node_name << "rslidar_packets_destination_" << node_index++;
 
   node_ptr_.reset(new rclcpp::Node(node_name.str()));
-  pkt_pub_ = node_ptr_->create_publisher<rslidar_msg::msg::RslidarPacket>(ros_send_topic, 100);
+  pkt_pub_ = node_ptr_->create_publisher<rslidar_msg::msg::RslidarPacket>(ros_send_topic, ros_queue_length);
 }
 
 inline void DestinationPacketRos::sendPacket(const Packet& msg)

src/source/source_pointcloud_ros.hpp

@@ -464,20 +464,24 @@ inline void DestinationPointCloudRos::init(const YAML::Node& config)
   yamlRead<std::string>(config["ros"], 
       "ros_send_point_cloud_topic", ros_send_topic, "rslidar_points");
 
+  size_t ros_queue_length;
+  yamlRead<size_t>(config["ros"], "ros_queue_length", ros_queue_length, 100);
 
   static int node_index = 0;
   std::stringstream node_name;
   node_name << "rslidar_points_destination_" << node_index++;
 
   node_ptr_.reset(new rclcpp::Node(node_name.str()));
-  pub_ = node_ptr_->create_publisher<sensor_msgs::msg::PointCloud2>(ros_send_topic, 100);
+
+  pub_ = node_ptr_->create_publisher<sensor_msgs::msg::PointCloud2>(ros_send_topic, ros_queue_length);
 
 #ifdef ENABLE_IMU_DATA_PARSE
   std::string ros_send_imu_data_topic;
   yamlRead<std::string>(config["ros"], 
       "ros_send_imu_data_topic", ros_send_imu_data_topic, "rslidar_imu_data");
   imu_pub_ = node_ptr_->create_publisher<sensor_msgs::msg::Imu>(ros_send_imu_data_topic, 1000);
 #endif
+
 }
 
 inline void DestinationPointCloudRos::sendPointCloud(const LidarPointCloudMsg& msg)