Type adaptation PCL

draco_point_cloud_transport/CMakeLists.txt

@@ -56,40 +56,6 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
-# TODO: Fix tests
-# if (CATKIN_ENABLE_TESTING)
-#   find_package(roslint REQUIRED)
-
-#   # catkin_lint - checks validity of package.xml and CMakeLists.txt
-#   # ROS buildfarm calls this without any environment and with empty rosdep cache,
-#   # so we have problems reading the list of packages from env
-#   # see https://github.com/ros-infrastructure/ros_buildfarm/issues/923
-#   if(DEFINED ENV{ROS_HOME})
-#     #catkin_lint: ignore_once env_var
-#     set(ROS_HOME "$ENV{ROS_HOME}")
-#   else()
-#     #catkin_lint: ignore_once env_var
-#     set(ROS_HOME "$ENV{HOME}/.ros")
-#   endif()
-#   #catkin_lint: ignore_once env_var
-#   if(DEFINED ENV{ROS_ROOT} AND EXISTS "${ROS_HOME}/rosdep/sources.cache")
-#     roslint_custom(catkin_lint "-W2" .)
-#   endif()
-
-#   # Roslint C++ - checks formatting and some other rules for C++ files
-
-#   file(GLOB_RECURSE ROSLINT_INCLUDE include/*.h include/*.hpp)
-#   file(GLOB_RECURSE ROSLINT_SRC src/*.cpp src/*.hpp src/*.h)
-#   #file(GLOB_RECURSE ROSLINT_TEST test/*.cpp)
-
-#   set(ROSLINT_CPP_OPTS "--extensions=h,hpp,hh,c,cpp,cc;--linelength=120;--filter=\
-#     -build/header_guard,-readability/namespace,-whitespace/braces,-runtime/references,\
-#     -build/c++11,-readability/nolint,-readability/todo,-legal/copyright,-build/namespaces")
-#   roslint_cpp(${ROSLINT_INCLUDE} ${ROSLINT_SRC})
-
-#   roslint_add_test()
-# endif()
-
 ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME})
 ament_export_dependencies(${dependencies})

draco_point_cloud_transport/README.md

@@ -42,9 +42,9 @@ By adjusting the **encode_speed** and **dedode_speed** parameters, one can adjus
  - "ny" - NORMAL
  - "nz" - NORMAL
  - all others are encoded as GENERIC
- 
+
  To specify custom quantization, one can either edit the list of recognized names or use **expert_quantization** and **expert_attribute_type** options.
- 
+
 ### Expert Quantization
 
 **Expert_quantization** option tell the encoder to use custom quantization values for point cloud attributes. Multiple POSITION attribute can therefore be encoded with varying quantization levels.
@@ -55,7 +55,7 @@ To set a quantization for a PointField entry "x" of point cloud which will be ad
 Example:
 
 ```bash
-$ rosparam set /base_topic/draco/attribute_mapping/quantization_bits/x 16
+ros2 param set <node name> /<base_topic>/draco/attribute_mapping/quantization_bits/x 16
 ```
 
 When using **expert_quantization**, user must specify the quantization bits for all PointField entries of point cloud.
@@ -70,11 +70,11 @@ To set a type for a PointField entry "x" of point cloud which will be advertised
 Example:
 
 ```bash
-$ rosparam set /base_topic/draco/attribute_mapping/attribute_type/x "'POSITION'"
+ros2 param set <node name> /<base_topic>/draco/attribute_mapping/attribute_type/x "'POSITION'"
 ```
 
 When using **expert_attribute_types**, user must specify the type for all PointField entries of point cloud. Accepted types are:
- - POSITION 
+ - POSITION
  - NORMAL
  - COLOR
  - TEX_COORD
@@ -83,7 +83,7 @@ When using **expert_attribute_types**, user must specify the type for all PointF
 When encoding rgb/rgba COLOR, user can specify to use the common rgba tweak of ROS (encoding rgba as 4 instances of 1 Byte instead of 1 instance of float32). To inform the encoder, that PointField entry "rgb" should be handled with the tweak, set parameter:
 
 ```bash
-$ rosparam set /base_topic/draco/attribute_mapping/rgba_tweak/rgb true
+ros2 param set <node name> /<base_topic>/draco/attribute_mapping/rgba_tweak/rgb true
 ```
 
 ## Subscriber

draco_point_cloud_transport/include/draco_point_cloud_transport/draco_publisher.hpp

@@ -54,6 +54,8 @@ class DracoPublisher
 public:
   std::string getTransportName() const override;
 
+  void declareParameters(const std::string & base_topic) override;
+
   TypedEncodeResult encodeTyped(const sensor_msgs::msg::PointCloud2 & raw) const override;
 
   static void registerPositionField(const std::string & field);

draco_point_cloud_transport/include/draco_point_cloud_transport/draco_subscriber.hpp

@@ -51,6 +51,8 @@ class DracoSubscriber
 public:
   std::string getTransportName() const override;
 
+  void declareParameters() override;
+
   DecodeResult decodeTyped(const point_cloud_interfaces::msg::CompressedPointCloud2 & compressed)
   const override;
 

draco_point_cloud_transport/src/draco_publisher.cpp

@@ -73,6 +73,132 @@ static std::unordered_map<std::string, draco::GeometryAttribute::Type> attribute
   {"normal_z", draco::GeometryAttribute::Type::NORMAL},
 };
 
+void DracoPublisher::declareParameters(const std::string & base_topic)
+{
+  declareParam<int>(std::string("encode_speed"), 7);
+  declareParam<int>(std::string("decode_speed"), 7);
+  declareParam<int>(std::string("encode_method"), 0);
+  declareParam<bool>(std::string("deduplicate"), true);
+  declareParam<bool>(std::string("force_quantization"), true);
+  declareParam<int>(std::string("quantization_POSITION"), 14);
+  declareParam<int>(std::string("quantization_NORMAL"), 14);
+  declareParam<int>(std::string("quantization_COLOR"), 14);
+  declareParam<int>(std::string("quantization_TEX_COORD"), 14);
+  declareParam<int>(std::string("quantization_GENERIC"), 14);
+  declareParam<bool>(std::string("expert_quantization"), true);
+  declareParam<bool>(std::string("expert_attribute_types"), true);
+
+  declareParam<std::string>(base_topic + "/attribute_mapping/attribute_type/x", "POSITION");
+  declareParam<std::string>(base_topic + "/attribute_mapping/attribute_type/y", "POSITION");
+  declareParam<std::string>(base_topic + "/attribute_mapping/attribute_type/z", "POSITION");
+  declareParam<int>(base_topic + "/attribute_mapping/quantization_bits/x", 16);
+  declareParam<int>(base_topic + "/attribute_mapping/quantization_bits/y", 16);
+  declareParam<int>(base_topic + "/attribute_mapping/quantization_bits/z", 16);
+  declareParam<int>(base_topic + "/attribute_mapping/quantization_bits/rgb", 16);
+  declareParam<bool>(base_topic + "/attribute_mapping/rgba_tweak/rgb", true);
+  declareParam<bool>(base_topic + "/attribute_mapping/rgba_tweak/rgba", false);
+
+  auto param_change_callback =
+    [this](std::vector<rclcpp::Parameter> parameters) -> rcl_interfaces::msg::SetParametersResult
+    {
+      auto result = rcl_interfaces::msg::SetParametersResult();
+      result.successful = true;
+      for (auto parameter : parameters) {
+        if (parameter.get_name() == "expert_quantization") {
+          config_.expert_quantization = parameter.as_bool();
+          return result;
+        } else if (parameter.get_name() == "force_quantization") {
+          config_.force_quantization = parameter.as_bool();
+          return result;
+        } else if (parameter.get_name() == "encode_speed") {
+          int value = parameter.as_int();
+          if (value >= 0 && value <= 10) {
+            config_.encode_speed = value;
+          } else {
+            RCLCPP_ERROR_STREAM(
+              getLogger(), "encode_speed value range should be between [0, 10] ");
+          }
+          return result;
+        } else if (parameter.get_name() == "decode_speed") {
+          int value = parameter.as_int();
+          if (value >= 0 && value <= 10) {
+            config_.decode_speed = value;
+          } else {
+            RCLCPP_ERROR_STREAM(
+              getLogger(), "decode_speed value range should be between [0, 10] ");
+          }
+          return result;
+        } else if (parameter.get_name() == "method_enum") {
+          int value = parameter.as_int();
+          if (value >= 0 && value <= 2) {
+            config_.method_enum = value;
+          } else {
+            RCLCPP_ERROR_STREAM(
+              getLogger(), "method_enum value range should be between [0, 2], "
+              "0 = auto, 1 = KD-tree, 2 = sequential ");
+          }
+          return result;
+        } else if (parameter.get_name() == "encode_method") {
+          config_.encode_method = parameter.as_int();
+          return result;
+        } else if (parameter.get_name() == "deduplicate") {
+          config_.deduplicate = parameter.as_bool();
+          return result;
+        } else if (parameter.get_name() == "quantization_POSITION") {
+          int value = parameter.as_int();
+          if (value >= 1 && value <= 31) {
+            config_.quantization_POSITION = value;
+          } else {
+            RCLCPP_ERROR_STREAM(
+              getLogger(), "quantization_POSITION value range should be between [1, 31] ");
+          }
+          return result;
+        } else if (parameter.get_name() == "quantization_NORMAL") {
+          int value = parameter.as_int();
+          if (value >= 1 && value <= 31) {
+            config_.quantization_NORMAL = value;
+          } else {
+            RCLCPP_ERROR_STREAM(
+              getLogger(), "quantization_NORMAL value range should be between [1, 31] ");
+          }
+          return result;
+        } else if (parameter.get_name() == "quantization_COLOR") {
+          int value = parameter.as_int();
+          if (value >= 1 && value <= 31) {
+            config_.quantization_COLOR = value;
+          } else {
+            RCLCPP_ERROR_STREAM(
+              getLogger(), "quantization_COLOR value range should be between [1, 31] ");
+          }
+          return result;
+        } else if (parameter.get_name() == "quantization_TEX_COORD") {
+          int value = parameter.as_int();
+          if (value >= 1 && value <= 31) {
+            config_.quantization_TEX_COORD = value;
+          } else {
+            RCLCPP_ERROR_STREAM(
+              getLogger(), "quantization_TEX_COORD value range should be between [1, 31] ");
+          }
+          return result;
+        } else if (parameter.get_name() == "quantization_GENERIC") {
+          int value = parameter.as_int();
+          if (value >= 1 && value <= 31) {
+            config_.quantization_GENERIC = value;
+          } else {
+            RCLCPP_ERROR_STREAM(
+              getLogger(), "quantization_GENERIC value range should be between [1, 31] ");
+          }
+          return result;
+        } else if (parameter.get_name() == "expert_attribute_types") {
+          config_.expert_attribute_types = parameter.as_bool();
+          return result;
+        }
+      }
+      return result;
+    };
+  setParamCallback(param_change_callback);
+}
+
 cras::expected<std::unique_ptr<draco::PointCloud>, std::string> DracoPublisher::convertPC2toDraco(
   const sensor_msgs::msg::PointCloud2 & PC2, const std::string & topic, bool deduplicate,
   bool expert_encoding) const
@@ -103,9 +229,8 @@ cras::expected<std::unique_ptr<draco::PointCloud>, std::string> DracoPublisher::
   for (const auto & field : PC2.fields) {
     if (expert_encoding) {  // find attribute type in user specified parameters
       rgba_tweak = false;
-
       if (getParam(
-          topic + "/draco/attribute_mapping/attribute_type/" + field.name,
+          topic + "/attribute_mapping/attribute_type/" + field.name,
           expert_attribute_data_type))
       {
         if (expert_attribute_data_type == "POSITION") {  // if data type is POSITION
@@ -114,7 +239,7 @@ cras::expected<std::unique_ptr<draco::PointCloud>, std::string> DracoPublisher::
           attribute_type = draco::GeometryAttribute::NORMAL;
         } else if (expert_attribute_data_type == "COLOR") {  // if data type is COLOR
           attribute_type = draco::GeometryAttribute::COLOR;
-          getParam(topic + "/draco/attribute_mapping/rgba_tweak/" + field.name, rgba_tweak);
+          getParam(topic + "/attribute_mapping/rgba_tweak/" + field.name, rgba_tweak);
         } else if (expert_attribute_data_type == "TEX_COORD") {  // if data type is TEX_COORD
           attribute_type = draco::GeometryAttribute::TEX_COORD;
         } else if (expert_attribute_data_type == "GENERIC") {  // if data type is GENERIC
@@ -131,7 +256,7 @@ cras::expected<std::unique_ptr<draco::PointCloud>, std::string> DracoPublisher::
           "Using regular type recognition instead.");
         RCLCPP_INFO_STREAM(
           getLogger(), "To set attribute type for " + field.name + " field entry, set " + topic +
-          "/draco/attribute_mapping/attribute_type/" + field.name);
+          "/attribute_mapping/attribute_type/" + field.name);
         expert_settings_ok = false;
       }
     }
@@ -280,7 +405,7 @@ DracoPublisher::TypedEncodeResult DracoPublisher::encodeTyped(
 
         for (const auto & field : rawDense.fields) {
           if (getParam(
-              base_topic_ + "/draco/attribute_mapping/quantization_bits/" + field.name,
+              base_topic_ + "/attribute_mapping/quantization_bits/" + field.name,
               attribute_quantization_bits))
           {
             expert_encoder.SetAttributeQuantization(att_id, attribute_quantization_bits);
@@ -290,7 +415,7 @@ DracoPublisher::TypedEncodeResult DracoPublisher::encodeTyped(
               " field entry. Using regular encoder instead.");
             RCLCPP_INFO_STREAM(
               getLogger(), "To set quantization for " + field.name + " field entry, set " +
-              base_topic_ + "/draco/attribute_mapping/quantization_bits/" + field.name);
+              base_topic_ + "/attribute_mapping/quantization_bits/" + field.name);
             expert_settings_ok = false;
           }
           att_id++;
@@ -314,7 +439,6 @@ DracoPublisher::TypedEncodeResult DracoPublisher::encodeTyped(
     }
   }
   // expert encoder end
-
   // regular encoder
   if ((!config_.expert_quantization) || (!expert_settings_ok)) {
     draco::Encoder encoder;

draco_point_cloud_transport/src/draco_subscriber.cpp

@@ -47,6 +47,41 @@
 
 namespace draco_point_cloud_transport
 {
+void DracoSubscriber::declareParameters()
+{
+  declareParam<bool>(std::string("SkipDequantizationPOSITION"), false);
+  declareParam<bool>(std::string("SkipDequantizationNORMAL"), false);
+  declareParam<bool>(std::string("SkipDequantizationCOLOR"), false);
+  declareParam<bool>(std::string("SkipDequantizationTEX_COORD"), false);
+  declareParam<bool>(std::string("SkipDequantizationGENERIC"), false);
+
+  auto param_change_callback =
+    [this](std::vector<rclcpp::Parameter> parameters) -> rcl_interfaces::msg::SetParametersResult
+    {
+      auto result = rcl_interfaces::msg::SetParametersResult();
+      result.successful = true;
+      for (auto parameter : parameters) {
+        if (parameter.get_name() == "SkipDequantizationPOSITION") {
+          config_.SkipDequantizationPOSITION = parameter.as_bool();
+          return result;
+        } else if (parameter.get_name() == "SkipDequantizationNORMAL") {
+          config_.SkipDequantizationNORMAL = parameter.as_bool();
+          return result;
+        } else if (parameter.get_name() == "SkipDequantizationCOLOR") {
+          config_.SkipDequantizationCOLOR = parameter.as_bool();
+          return result;
+        } else if (parameter.get_name() == "SkipDequantizationTEX_COORD") {
+          config_.SkipDequantizationTEX_COORD = parameter.as_bool();
+          return result;
+        } else if (parameter.get_name() == "SkipDequantizationGENERIC") {
+          config_.SkipDequantizationGENERIC = parameter.as_bool();
+          return result;
+        }
+      }
+      return result;
+    };
+  setParamCallback(param_change_callback);
+}
 
 //! Method for converting into sensor_msgs::msg::PointCloud2
 cras::expected<bool, std::string> convertDracoToPC2(