revert commits c80ab70 and 58ba910

ctu-mrs · Sep 16, 2024 · 190d13d · 190d13d
1 parent 58ba910
commit 190d13d
Show file tree

Hide file tree

Showing 5 changed files with 17 additions and 75 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -34,7 +34,6 @@ set(CATKIN_DEPENDENCIES
   sensor_msgs
   std_msgs
   visualization_msgs
-  livox_ros_driver2
   )
 
 find_package(PCL REQUIRED COMPONENTS

diff --git a/include/PCLFiltration.h b/include/PCLFiltration.h
@@ -28,8 +28,6 @@
 #include <sensor_msgs/CameraInfo.h>
 #include <sensor_msgs/image_encodings.h>
 
-#include <livox_ros_driver2/CustomMsg.h>
-
 #include <geometry_msgs/Point.h>
 #include <geometry_msgs/PointStamped.h>
 #include <geometry_msgs/Transform.h>
@@ -169,18 +167,18 @@ class SensorDepthCamera {
 
   template <typename T, typename U>
   void convertDepthToCloud(const sensor_msgs::Image::ConstPtr& depth_msg, const sensor_msgs::Image::ConstPtr& intensity_msg, PC_I::Ptr& cloud_out,
-                           PC_I::Ptr& cloud_over_max_range_out, PC_I::Ptr& cloud_low_intensity, const bool return_removed_close = false,
-                           const bool return_removed_far = false, const bool replace_nans = false, const bool keep_ordered = false);
+                           PC_I::Ptr& cloud_over_max_range_out, PC_I::Ptr& cloud_low_intensity, const bool return_removed_close = false, const bool return_removed_far = false,
+                           const bool replace_nans = false, const bool keep_ordered = false);
 
   template <typename T, typename U>
   void convertDepthToCloudUnordered(const sensor_msgs::Image::ConstPtr& depth_msg, const sensor_msgs::Image::ConstPtr& intensity_msg, PC_I::Ptr& out_pc,
-                                    PC_I::Ptr& removed_pc, PC_I::Ptr& cloud_low_intensity, const bool return_removed_close = false,
-                                    const bool return_removed_far = false, const bool replace_nans = false);
+                                    PC_I::Ptr& removed_pc, PC_I::Ptr& cloud_low_intensity, const bool return_removed_close = false, const bool return_removed_far = false,
+                                    const bool replace_nans = false);
 
   template <typename T, typename U>
   void convertDepthToCloudOrdered(const sensor_msgs::Image::ConstPtr& depth_msg, const sensor_msgs::Image::ConstPtr& intensity_msg, PC_I::Ptr& out_pc,
-                                  PC_I::Ptr& removed_pc, PC_I::Ptr& cloud_low_intensity, const bool return_removed_close = false,
-                                  const bool return_removed_far = false, const bool replace_nans = false);
+                                  PC_I::Ptr& removed_pc, PC_I::Ptr& cloud_low_intensity, const bool return_removed_close = false, const bool return_removed_far = false,
+                                  const bool replace_nans = false);
 
   void imagePointToCloudPoint(const int x, const int y, const float depth, const float intensity, pt_XYZI& point);
 
@@ -286,8 +284,7 @@ class PCLFiltration : public nodelet::Nodelet {
 private:
   bool is_initialized = false;
 
-  mrs_lib::SubscribeHandler<sensor_msgs::PointCloud2>     _sub_lidar3d_ros;
-  mrs_lib::SubscribeHandler<livox_ros_driver2::CustomMsg> _sub_lidar3d_livox;
+  mrs_lib::SubscribeHandler<sensor_msgs::PointCloud2> _sub_lidar3d;
 
   ros::Publisher _pub_lidar3d;
   ros::Publisher _pub_lidar3d_over_max_range;
@@ -307,7 +304,6 @@ class PCLFiltration : public nodelet::Nodelet {
 
   /* 3D LIDAR */
   void        lidar3dCallback(const sensor_msgs::PointCloud2::ConstPtr msg);
-  void        lidarLivoxCallback(const livox_ros_driver2::CustomMsg::ConstPtr msg);
   std::string _lidar3d_name;
   float       _lidar3d_frequency;
   float       _lidar3d_vfov;

diff --git a/launch/pcl_filter.launch b/launch/pcl_filter.launch
@@ -7,10 +7,8 @@
   <arg name="custom_config" default="" />
   <arg name="node_name" default="pcl_filter" />
   <arg name="name_suffix" default="" />
-
-  <arg name="topic_3d_lidar_in"      default="os_cloud_nodelet/points" />
-  <arg name="topic_3d_lidar_type_in" default="sensor_msgs/PointCloud2" />
-  <arg name="topic_3d_lidar_out"     default="~points_processed" />
+  <arg name="topic_3d_lidar_in" default="os_cloud_nodelet/points" />
+  <arg name="topic_3d_lidar_out" default="~points_processed" />
 
   <arg     if="$(arg debug)" name="launch_prefix" value="debug_roslaunch" />
   <arg unless="$(arg debug)" name="launch_prefix" value="" />
@@ -32,8 +30,7 @@
     <node pkg="nodelet" type="nodelet" name="$(arg nodelet_name)" args="$(arg nodelet) mrs_pcl_tools/PCLFiltration $(arg nodelet_manager)" output="screen" launch-prefix="$(arg launch_prefix)">
 
       <!-- Parameters -->
-      <param name="uav_name"               type="string" value="$(arg UAV_NAME)" />
-      <param name="topic_3d_lidar_type_in" type="string" value="$(arg topic_3d_lidar_type_in)" />
+      <param name="uav_name" type="string" value="$(arg UAV_NAME)" />
 
       <rosparam file="$(find mrs_pcl_tools)/config/pcl_filter.yaml" />
       <rosparam if="$(eval not arg('custom_config') == '')" file="$(arg custom_config)" command="load" />

diff --git a/package.xml b/package.xml
@@ -24,7 +24,6 @@
   <depend>mrs_modules_msgs</depend>
   <depend>nodelet</depend>
   <depend>ouster_ros</depend>
-  <depend>livox_ros_driver2</depend>
   <depend>pcl_conversions</depend>
   <depend>pcl_ros</depend>
   <depend>rosconsole</depend>

diff --git a/src/PCLFiltration.cpp b/src/PCLFiltration.cpp
@@ -19,9 +19,8 @@ void PCLFiltration::onInit() {
   _common_handlers->param_loader = std::make_shared<mrs_lib::ParamLoader>(nh, "PCLFilter");
 
   // Transformer
-  const auto uav_name            = _common_handlers->param_loader->loadParam2<std::string>("uav_name");
-  const auto topic_3d_lidar_type = _common_handlers->param_loader->loadParam2<std::string>("topic_3d_lidar_type_in", "sensor_msgs/PointCloud2");
-  _common_handlers->transformer  = std::make_shared<mrs_lib::Transformer>("PCLFilter");
+  const auto uav_name           = _common_handlers->param_loader->loadParam2<std::string>("uav_name");
+  _common_handlers->transformer = std::make_shared<mrs_lib::Transformer>("PCLFilter");
   _common_handlers->transformer->setDefaultPrefix(uav_name);
   _common_handlers->transformer->setLookupTimeout(ros::Duration(0.05));
   _common_handlers->transformer->retryLookupNewest(false);
@@ -118,31 +117,22 @@ void PCLFiltration::onInit() {
   _common_handlers->param_loader->loadParam("rplidar/voxel_resolution", _rplidar_voxel_resolution, 0.0f);
 
   if (!_common_handlers->param_loader->loadedSuccessfully()) {
-    NODELET_ERROR("[PCLFiltration]: Some compulsory parameters were not loaded successfully. Shuting down.");
+    NODELET_ERROR("[PCLFiltration]: Some compulsory parameters were not loaded successfully, ending the node");
     ros::shutdown();
   }
 
   if (_lidar3d_republish) {
 
     if (_lidar3d_row_step <= 0 || _lidar3d_col_step <= 0) {
-      NODELET_ERROR("[PCLFiltration]: Downsampling row/col steps for 3D lidar must be >=1. Shuting down.");
+      NODELET_ERROR("[PCLFiltration]: Downsampling row/col steps for 3D lidar must be >=1, ending nodelet.");
       ros::shutdown();
     }
 
 
     mrs_lib::SubscribeHandlerOptions shopts(nh);
-    shopts.node_name          = "PCLFiltration";
-    shopts.no_message_timeout = ros::Duration(5.0);
-
-    if (topic_3d_lidar_type == "sensor_msgs/PointCloud2") {
-      _sub_lidar3d_ros = mrs_lib::SubscribeHandler<sensor_msgs::PointCloud2>(shopts, "lidar3d_in", &PCLFiltration::lidar3dCallback, this);
-    } else if (topic_3d_lidar_type == "livox_ros_driver2/CustomMsg") {
-      _sub_lidar3d_livox = mrs_lib::SubscribeHandler<livox_ros_driver2::CustomMsg>(shopts, "lidar3d_in", &PCLFiltration::lidarLivoxCallback, this);
-    } else {
-      NODELET_ERROR("[PCLFiltration]: Unknown type of 3D LiDAR msg: %s. Shuting down.", topic_3d_lidar_type.c_str());
-      ros::shutdown();
-    }
-
+    shopts.node_name            = "PCLFiltration";
+    shopts.no_message_timeout   = ros::Duration(5.0);
+    _sub_lidar3d                = mrs_lib::SubscribeHandler<sensor_msgs::PointCloud2>(shopts, "lidar3d_in", &PCLFiltration::lidar3dCallback, this);
     _pub_lidar3d                = nh.advertise<sensor_msgs::PointCloud2>("lidar3d_out", 1);
     _pub_lidar3d_over_max_range = nh.advertise<sensor_msgs::PointCloud2>("lidar3d_over_max_range_out", 1);
     if (_filter_removeBelowGround.used())
@@ -222,46 +212,7 @@ void PCLFiltration::lidar3dCallback(const sensor_msgs::PointCloud2::ConstPtr msg
 
   _common_handlers->diagnostics->publish(diag_msg);
 }
-//}
-
-/* lidarLivoxCallback() //{ */
-void PCLFiltration::lidarLivoxCallback(const livox_ros_driver2::CustomMsg::ConstPtr msg) {
-
-  if (!is_initialized || !_lidar3d_republish) {
-    return;
-  }
-
-  // FIXME: a lot of unneccessary conversions is happening here (doing this the ugly way due to HEAVY time constraints)
-
-  // Convert to pcl::PointXYZI point cloud
-  const PC_I::Ptr cloud = boost::make_shared<PC_I>();
-  cloud->reserve(msg->points.size());
-
-  pcl_conversions::toPCL(msg->header, cloud->header);
-
-  for (const auto& p_livox : msg->points) {
-    pt_XYZI p_xyzi;
-    p_xyzi.x         = p_livox.x;
-    p_xyzi.y         = p_livox.y;
-    p_xyzi.z         = p_livox.z;
-    p_xyzi.intensity = p_livox.reflectivity;
-
-    cloud->emplace_back(p_xyzi);
-  }
-
-  cloud->width  = cloud->size();
-  cloud->height = 1;
-
-  // Convert to sensor_msgs::PointCloud2 message type
-  const sensor_msgs::PointCloud2::Ptr cloud_ros = boost::make_shared<sensor_msgs::PointCloud2>();
-  pcl::toROSMsg(*cloud, *cloud_ros);
-
-  // Call standard method
-  lidar3dCallback(cloud_ros);
-}
-//}
 
-/* process_msg() //{ */
 template <typename PC>
 void PCLFiltration::process_msg(typename boost::shared_ptr<PC>& inout_pc_ptr) {