fix(pointcloud_preprocessor): fix distortion corrector unit test (aut…

…owarefoundation#7833) * fix: use pointcloud iterator instead of memcpy, remove reinterpret cast Signed-off-by: vividf <[email protected]> * chore: set default debugging parameter to false Signed-off-by: vividf <[email protected]> * style(pre-commit): autofix * chore: run precommit Signed-off-by: vividf <[email protected]> * chore: fix TIER IV name Signed-off-by: vividf <[email protected]> * chore: changed public variables to protected and add getters Signed-off-by: vividf <[email protected]> * chore: add tolerance Signed-off-by: vividf <[email protected]> * feat: add two tests for pure linear and rotation Signed-off-by: vividf <[email protected]> * chore: change naming, fix tolerance value Signed-off-by: vividf <[email protected]> * chore: add comment for quat Signed-off-by: vividf <[email protected]> * fix: remove node_->get_clock() and use self-defined timestamp Signed-off-by: vividf <[email protected]> * chore: remove redundant parameters Signed-off-by: vividf <[email protected]> * chore: fix spell error and add tests in cmake Signed-off-by: vividf <[email protected]> * chore: move all clock->now() to self-defined time Signed-off-by: vividf <[email protected]> * chore: change function names Signed-off-by: vividf <[email protected]> * chore: remove irrelevant variable Signed-off-by: vividf <[email protected]> * chore: fix variables naming Signed-off-by: vividf <[email protected]> * chore: change boolen naming: generate_points Signed-off-by: vividf <[email protected]> * chore: add assert to make sure ms is not larger than a second Signed-off-by: vividf <[email protected]> * chore: add note Signed-off-by: vividf <[email protected]> * chore: add unifore initialization and semantic meaning for magic number Signed-off-by: vividf <[email protected]> * chore: change vector to Eigen Signed-off-by: vividf <[email protected]> * chore: fix explanation Signed-off-by: vividf <[email protected]> * chore: fix more eigen Signed-off-by: vividf <[email protected]> * chore: fix more magic numbers Signed-off-by: vividf <[email protected]> * chore: add unit Signed-off-by: vividf <[email protected]> * fix: use assert from gtest Signed-off-by: vividf <[email protected]> --------- Signed-off-by: vividf <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Kenzo Lobos Tsunekawa <[email protected]>
tier4 · Sep 10, 2024 · 0c152ff · 0c152ff
1 parent 416609f
commit 0c152ff
Show file tree

Hide file tree

Showing 4 changed files with 945 additions and 1 deletion.
diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt
@@ -231,8 +231,12 @@ if(BUILD_TESTING)
   ament_add_gtest(test_utilities
     test/test_utilities.cpp
   )
+  ament_add_gtest(distortion_corrector_node_tests
+  test/test_distortion_corrector_node.cpp
+  )
 
   target_link_libraries(test_utilities pointcloud_preprocessor_filter)
+  target_link_libraries(distortion_corrector_node_tests pointcloud_preprocessor_filter)
 
 
 endif()
diff --git a/...reprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/...reprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
@@ -48,6 +48,11 @@ namespace pointcloud_preprocessor
 class DistortionCorrectorBase
 {
 public:
+  virtual bool pointcloud_transform_exists() = 0;
+  virtual bool pointcloud_transform_needed() = 0;
+  virtual std::deque<geometry_msgs::msg::TwistStamped> get_twist_queue() = 0;
+  virtual std::deque<geometry_msgs::msg::Vector3Stamped> get_angular_velocity_queue() = 0;
+
   virtual void processTwistMessage(
     const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) = 0;
   virtual void processIMUMessage(
@@ -96,6 +101,10 @@ class DistortionCorrector : public DistortionCorrectorBase
   : node_(node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_)
   {
   }
+  bool pointcloud_transform_exists();
+  bool pointcloud_transform_needed();
+  std::deque<geometry_msgs::msg::TwistStamped> get_twist_queue();
+  std::deque<geometry_msgs::msg::Vector3Stamped> get_angular_velocity_queue();
   void processTwistMessage(
     const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg) override;
 

diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -20,6 +20,31 @@
 
 namespace pointcloud_preprocessor
 {
+
+template <class T>
+bool DistortionCorrector<T>::pointcloud_transform_exists()
+{
+  return pointcloud_transform_exists_;
+}
+
+template <class T>
+bool DistortionCorrector<T>::pointcloud_transform_needed()
+{
+  return pointcloud_transform_needed_;
+}
+
+template <class T>
+std::deque<geometry_msgs::msg::TwistStamped> DistortionCorrector<T>::get_twist_queue()
+{
+  return twist_queue_;
+}
+
+template <class T>
+std::deque<geometry_msgs::msg::Vector3Stamped> DistortionCorrector<T>::get_angular_velocity_queue()
+{
+  return angular_velocity_queue_;
+}
+
 template <class T>
 void DistortionCorrector<T>::processTwistMessage(
   const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_msg)
@@ -349,7 +374,9 @@ inline void DistortionCorrector2D::undistortPointImplementation(
   theta_ += w * time_offset;
   baselink_quat_.setValue(
     0, 0, autoware::universe_utils::sin(theta_ * 0.5f),
-    autoware::universe_utils::cos(theta_ * 0.5f));  // baselink_quat.setRPY(0.0, 0.0, theta);
+    autoware::universe_utils::cos(
+      theta_ *
+      0.5f));  // baselink_quat.setRPY(0.0, 0.0, theta); (Note that the value is slightly different)
   const float dis = v * time_offset;
   x_ += dis * autoware::universe_utils::cos(theta_);
   y_ += dis * autoware::universe_utils::sin(theta_);