tmp: implement assert

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Author: takayuki5168

common/motion_utils/src/resample/resample.cpp

@@ -20,6 +20,7 @@
 #include "motion_utils/resample/resample_utils.hpp"
 #include "motion_utils/trajectory/trajectory.hpp"
 #include "tier4_autoware_utils/geometry/geometry.hpp"
+#include "tier4_autoware_utils/system/assert.hpp"
 
 namespace motion_utils
 {
@@ -28,6 +29,7 @@ std::vector<geometry_msgs::msg::Point> resamplePointVector(
   const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy,
   const bool use_lerp_for_z)
 {
+  ASSERT(false);
   // validate arguments
   if (!resample_utils::validate_arguments(points, resampled_arclength)) {
     return points;

common/tier4_autoware_utils/CMakeLists.txt

@@ -16,8 +16,14 @@ ament_auto_add_library(tier4_autoware_utils SHARED
   src/ros/marker_helper.cpp
   src/ros/logger_level_configure.cpp
   src/system/backtrace.cpp
+  src/system/assert.cpp
 )
 
+option(ENABLE_ASSERT_ABORT "Option to enable assert abort" ON)
+if(ENABLE_ASSERT_ABORT)
+  target_compile_definitions(tier4_autoware_utils PUBLIC ENABLE_ASSERT_ABORT)
+endif()
+
 if(BUILD_TESTING)
   find_package(ament_cmake_ros REQUIRED)
 

common/tier4_autoware_utils/include/tier4_autoware_utils/system/assert.hpp

@@ -0,0 +1,34 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// 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.
+
+#ifndef TIER4_AUTOWARE_UTILS__SYSTEM__ASSERT_HPP_
+#define TIER4_AUTOWARE_UTILS__SYSTEM__ASSERT_HPP_
+
+#ifdef ENABLE_ASSERT_ABORT
+
+#define ASSERT(expr)                                                                               \
+  printf(                                                                                          \
+    "[Assertion] %s() at %s:%d " #expr                                                             \
+    "--------------------------------------------------------------------------------------------" \
+    "---------------------------------------------------------",                                   \
+    __FUNCTION__, __FILE__, __LINE__);
+
+#else
+
+#define ASSERT(...)
+hogehoge
+
+#endif
+
+#endif  // TIER4_AUTOWARE_UTILS__SYSTEM__ASSERT_HPP_

common/tier4_autoware_utils/src/system/assert.cpp

@@ -0,0 +1,15 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// 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.
+
+#include "tier4_autoware_utils/system/assert.hpp"