Merge branch 'hotfix/fix-ndt-matching-runtime-error-in-debug-mode' in…

…to 'master'

Fix ndt_matching's runtime error in debug mode

See merge request autowarefoundation/autoware.ai/core_perception!57

lidar_localizer/CMakeLists.txt

@@ -160,3 +160,16 @@ install(TARGETS
 install(DIRECTORY launch/
         DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
         PATTERN ".svn" EXCLUDE)
+
+if (CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+
+  add_rostest_gtest(test_launch_ndt_matching
+    test/test_launch_ndt_matching.test
+    test/src/test_launch_ndt_matching.cpp
+  )
+
+  target_link_libraries(test_launch_ndt_matching
+    ${catkin_LIBRARIES}
+  )
+endif ()

lidar_localizer/package.xml

@@ -28,4 +28,6 @@
   <depend>std_msgs</depend>
   <depend>tf</depend>
   <depend>velodyne_pointcloud</depend>
+
+  <test_depend>rostest</test_depend>
 </package>

lidar_localizer/test/src/test_launch_ndt_matching.cpp

@@ -0,0 +1,61 @@
+/*
+ * Copyright 2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * Authors: Kenji Miyake
+ */
+
+#include <gtest/gtest.h>
+
+#include <ros/ros.h>
+
+class TestSuite : public ::testing::Test
+{
+public:
+  TestSuite() = default;
+  ~TestSuite() = default;
+};
+
+bool isNodeFound()
+{
+  std::vector<std::string> node_names;
+  while (true)
+  {
+    // Get node names
+    ros::master::getNodes(node_names);
+
+    // Wait for ndt_matching node
+    const auto itr = std::find(std::begin(node_names), std::end(node_names), "/ndt_matching");
+
+    // Break if found
+    if (itr != std::end(node_names))
+    {
+      break;
+    }
+  }
+
+  return true;
+}
+
+TEST_F(TestSuite, launch_ndt_matching)
+{
+  ASSERT_TRUE(isNodeFound());
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "TestNode");
+  return RUN_ALL_TESTS();
+}

lidar_localizer/test/test_launch_ndt_matching.test

@@ -0,0 +1,12 @@
+<launch>
+  <param name="tf_x" value="1.2" />
+  <param name="tf_y" value="0.0" />
+  <param name="tf_z" value="2.0" />
+  <param name="tf_yaw" value="0.0" />
+  <param name="tf_pitch" value="0.0" />
+  <param name="tf_roll" value="0.0" />
+  <param name="localizer" value="velodyne" />
+
+  <node pkg="lidar_localizer" type="ndt_matching" name="ndt_matching" />
+  <test pkg="lidar_localizer" type="test_launch_ndt_matching" test-name="test_launch_ndt_matching" />
+</launch>

ndt_cpu/CMakeLists.txt

@@ -46,6 +46,13 @@ set(incs
         include/ndt_cpu/Octree.h
         )
 
+message(WARNING
+"Adding 'EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT' macro to prevent ndt_matching's runtime error in debug mode.
+  The bug reasons and solutions are written in http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html .
+  This workaround was discussed on https://gitlab.com/autowarefoundation/autoware.ai/core_perception/merge_requests/57 .")
+
+add_definitions(-DEIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT)
+
 add_library(ndt_cpu ${incs} ${srcs})
 
 target_link_libraries(ndt_cpu
@@ -63,4 +70,4 @@ install(TARGETS ndt_cpu
         ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
         LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
         RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-        )
+        )

ndt_cpu/README.md

@@ -0,0 +1,9 @@
+# ndt_cpu
+
+CPU version of NDT matching
+
+## Known issues
+
+Currently, this package contains a workaround for preventing runtime error in debug mode.  
+This should be fixed in the future release.  
+See [this MR](https://gitlab.com/autowarefoundation/autoware.ai/core_perception/merge_requests/57) for more details.