refactor(topic_state_monitor): rework parameter

.github/CODEOWNERS

@@ -6,6 +6,7 @@ common/autoware_grid_map_utils/** [email protected]
 common/autoware_interpolation/** [email protected] [email protected]
 common/autoware_kalman_filter/** [email protected] [email protected] [email protected]
 common/autoware_motion_utils/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
+common/autoware_object_recognition_utils/** [email protected] [email protected] [email protected]
 common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** [email protected]
 common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** [email protected]
 common/autoware_path_distance_calculator/** [email protected]
@@ -24,7 +25,6 @@ common/fake_test_node/** [email protected] [email protected] shumpei.wakabay
 common/global_parameter_loader/** [email protected]
 common/glog_component/** [email protected]
 common/goal_distance_calculator/** [email protected]
-common/object_recognition_utils/** [email protected] [email protected] [email protected]
 common/osqp_interface/** [email protected] [email protected] [email protected] [email protected]
 common/polar_grid/** [email protected]
 common/qp_interface/** [email protected] [email protected] [email protected] [email protected]

common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_shift.hpp

@@ -0,0 +1,71 @@
+// 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 AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_SHIFT_HPP_
+#define AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_SHIFT_HPP_
+
+namespace autoware::motion_utils
+{
+/**
+ * @brief Calculates the velocity required for shifting
+ * @param lateral lateral distance
+ * @param jerk lateral jerk
+ * @param longitudinal_distance longitudinal distance
+ * @return velocity
+ */
+double calc_feasible_velocity_from_jerk(
+  const double lateral, const double jerk, const double longitudinal_distance);
+
+/**
+ * @brief Calculates the lateral distance required for shifting
+ * @param longitudinal longitudinal distance
+ * @param jerk lateral jerk
+ * @param velocity velocity
+ * @return lateral distance
+ */
+double calc_lateral_dist_from_jerk(
+  const double longitudinal, const double jerk, const double velocity);
+
+/**
+ * @brief Calculates the lateral distance required for shifting
+ * @param lateral lateral distance
+ * @param jerk lateral jerk
+ * @param velocity velocity
+ * @return longitudinal distance
+ */
+double calc_longitudinal_dist_from_jerk(
+  const double lateral, const double jerk, const double velocity);
+
+/**
+ * @brief Calculates the total time required for shifting
+ * @param lateral lateral distance
+ * @param jerk lateral jerk
+ * @param acc lateral acceleration
+ * @return time
+ */
+double calc_shift_time_from_jerk(const double lateral, const double jerk, const double acc);
+
+/**
+ * @brief Calculates the required jerk from lateral/longitudinal distance
+ * @param lateral lateral distance
+ * @param longitudinal longitudinal distance
+ * @param velocity velocity
+ * @return jerk
+ */
+double calc_jerk_from_lat_lon_distance(
+  const double lateral, const double longitudinal, const double velocity);
+
+}  // namespace autoware::motion_utils
+
+#endif  // AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_SHIFT_HPP_

common/autoware_motion_utils/src/trajectory/path_shift.cpp

@@ -0,0 +1,105 @@
+// 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 "autoware/motion_utils/trajectory/path_shift.hpp"
+
+#include "autoware/motion_utils/trajectory/trajectory.hpp"
+
+namespace autoware::motion_utils
+{
+double calc_feasible_velocity_from_jerk(
+  const double lateral, const double jerk, const double longitudinal_distance)
+{
+  const double j = std::abs(jerk);
+  const double l = std::abs(lateral);
+  const double d = std::abs(longitudinal_distance);
+  if (j < 1.0e-8 || l < 1.0e-8) {
+    const std::string error_message(
+      std::string(__func__) + ": Failed to calculate velocity due to invalid arg");
+    RCLCPP_WARN(get_logger(), "%s", error_message.c_str());
+    return 1.0e10;
+  }
+  return d / (4.0 * std::pow(0.5 * l / j, 1.0 / 3.0));
+}
+
+double calc_lateral_dist_from_jerk(
+  const double longitudinal, const double jerk, const double velocity)
+{
+  const double j = std::abs(jerk);
+  const double d = std::abs(longitudinal);
+  const double v = std::abs(velocity);
+
+  if (j < 1.0e-8 || v < 1.0e-8) {
+    const std::string error_message(
+      std::string(__func__) + ": Failed to calculate lateral distance due to invalid arg");
+    RCLCPP_WARN(get_logger(), "%s", error_message.c_str());
+    return 1.0e10;
+  }
+  return 2.0 * std::pow(d / (4.0 * v), 3.0) * j;
+}
+
+double calc_longitudinal_dist_from_jerk(
+  const double lateral, const double jerk, const double velocity)
+{
+  const double j = std::abs(jerk);
+  const double l = std::abs(lateral);
+  const double v = std::abs(velocity);
+  if (j < 1.0e-8) {
+    const std::string error_message(
+      std::string(__func__) + ": Failed to calculate longitudinal distance due to invalid arg");
+    RCLCPP_WARN(get_logger(), "%s", error_message.c_str());
+    return 1.0e10;
+  }
+  return 4.0 * std::pow(0.5 * l / j, 1.0 / 3.0) * v;
+}
+
+double calc_shift_time_from_jerk(const double lateral, const double jerk, const double acc)
+{
+  const double j = std::abs(jerk);
+  const double a = std::abs(acc);
+  const double l = std::abs(lateral);
+  if (j < 1.0e-8 || a < 1.0e-8) {
+    const std::string error_message(
+      std::string(__func__) + ": Failed to calculate shift time due to invalid arg");
+    RCLCPP_WARN(get_logger(), "%s", error_message.c_str());
+    return 1.0e10;  // TODO(Horibe) maybe invalid arg?
+  }
+
+  // time with constant jerk
+  double tj = a / j;
+
+  // time with constant acceleration (zero jerk)
+  double ta = (std::sqrt(a * a + 4.0 * j * j * l / a) - 3.0 * a) / (2.0 * j);
+
+  if (ta < 0.0) {
+    // it will not hit the acceleration limit this time
+    tj = std::pow(l / (2.0 * j), 1.0 / 3.0);
+    ta = 0.0;
+  }
+
+  const double t_total = 4.0 * tj + 2.0 * ta;
+  return t_total;
+}
+
+double calc_jerk_from_lat_lon_distance(
+  const double lateral, const double longitudinal, const double velocity)
+{
+  constexpr double ep = 1.0e-3;
+  const double lat = std::abs(lateral);
+  const double lon = std::max(std::abs(longitudinal), ep);
+  const double v = std::abs(velocity);
+  return 0.5 * lat * std::pow(4.0 * v / lon, 3);
+}
+
+}  // namespace autoware::motion_utils

common/autoware_motion_utils/test/src/trajectory/test_path_shift.cpp

@@ -0,0 +1,163 @@
+// 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 "autoware/motion_utils/trajectory/path_shift.hpp"
+
+#include <gtest/gtest.h>
+
+TEST(path_shift_test, calc_feasible_velocity_from_jerk)
+{
+  using autoware::motion_utils::calc_feasible_velocity_from_jerk;
+
+  double longitudinal_distance = 0.0;
+  double lateral_distance = 0.0;
+  double lateral_jerk = 0.0;
+
+  // Condition: zero lateral jerk
+  EXPECT_DOUBLE_EQ(
+    calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance),
+    1.0e10);
+
+  // Condition: zero lateral distance
+  lateral_jerk = 1.0;
+  EXPECT_DOUBLE_EQ(
+    calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance),
+    1.0e10);
+
+  // Condition: zero longitudinal distance
+  lateral_distance = 2.0;
+  EXPECT_DOUBLE_EQ(
+    calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance), 0.0);
+
+  // Condition: random condition
+  longitudinal_distance = 50.0;
+  EXPECT_DOUBLE_EQ(
+    calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance), 12.5);
+}
+
+TEST(path_shift_test, calc_lateral_dist_from_jerk)
+{
+  using autoware::motion_utils::calc_lateral_dist_from_jerk;
+
+  double longitudinal_distance = 0.0;
+  double lateral_jerk = 0.0;
+  double velocity = 0.0;
+
+  // Condition: zero lateral jerk
+  EXPECT_DOUBLE_EQ(
+    calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 1.0e10);
+
+  // Condition: zero velocity
+  lateral_jerk = 2.0;
+  EXPECT_DOUBLE_EQ(
+    calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 1.0e10);
+
+  // Condition: zero longitudinal distance
+  velocity = 10.0;
+  EXPECT_DOUBLE_EQ(calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 0.0);
+
+  // Condition: random condition
+  longitudinal_distance = 100.0;
+  EXPECT_DOUBLE_EQ(
+    calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 62.5);
+}
+
+TEST(path_shift_test, calc_longitudinal_dist_from_jerk)
+{
+  using autoware::motion_utils::calc_longitudinal_dist_from_jerk;
+
+  double lateral_distance = 0.0;
+  double lateral_jerk = 0.0;
+  double velocity = 0.0;
+
+  // Condition: zero lateral jerk
+  EXPECT_DOUBLE_EQ(
+    calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 1.0e10);
+
+  // Condition: zero lateral distance
+  lateral_jerk = -1.0;
+  velocity = 10.0;
+  EXPECT_DOUBLE_EQ(calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 0.0);
+
+  // Condition: zero velocity
+  velocity = 0.0;
+  lateral_distance = 54.0;
+  EXPECT_DOUBLE_EQ(calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 0.0);
+
+  // Condition: random
+  velocity = 8.0;
+  EXPECT_DOUBLE_EQ(
+    calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 96.0);
+}
+
+TEST(path_shift_test, calc_shift_time_from_jerk)
+{
+  constexpr double epsilon = 1e-6;
+
+  using autoware::motion_utils::calc_shift_time_from_jerk;
+
+  double lateral_distance = 0.0;
+  double lateral_jerk = 0.0;
+  double lateral_acceleration = 0.0;
+
+  // Condition: zero lateral jerk
+  EXPECT_DOUBLE_EQ(
+    calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 1.0e10);
+
+  // Condition: zero lateral acceleration
+  lateral_jerk = -2.0;
+  EXPECT_DOUBLE_EQ(
+    calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 1.0e10);
+
+  // Condition: zero lateral distance
+  lateral_acceleration = -4.0;
+  EXPECT_DOUBLE_EQ(
+    calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 0.0);
+
+  // Condition: random (TODO: use DOUBLE_EQ in future. currently not precise enough)
+  lateral_distance = 80.0;
+  EXPECT_NEAR(
+    calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 11.16515139,
+    epsilon);
+}
+
+TEST(path_shift_test, calc_jerk_from_lat_lon_distance)
+{
+  using autoware::motion_utils::calc_jerk_from_lat_lon_distance;
+
+  double lateral_distance = 0.0;
+  double longitudinal_distance = 100.0;
+  double velocity = 10.0;
+
+  // Condition: zero lateral distance
+  EXPECT_DOUBLE_EQ(
+    calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 0.0);
+
+  // Condition: zero velocity
+  lateral_distance = 5.0;
+  velocity = 0.0;
+  EXPECT_DOUBLE_EQ(
+    calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 0.0);
+
+  // Condition: zero longitudinal distance
+  longitudinal_distance = 0.0;
+  velocity = 10.0;
+  EXPECT_DOUBLE_EQ(
+    calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 1.6 * 1e14);
+
+  // Condition: random
+  longitudinal_distance = 100.0;
+  EXPECT_DOUBLE_EQ(
+    calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 0.16);
+}

common/object_recognition_utils/CMakeLists.txt → common/autoware_object_recognition_utils/CMakeLists.txt

@@ -1,12 +1,12 @@
 cmake_minimum_required(VERSION 3.14)
-project(object_recognition_utils)
+project(autoware_object_recognition_utils)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
 find_package(Boost REQUIRED)
 
-ament_auto_add_library(object_recognition_utils SHARED
+ament_auto_add_library(${PROJECT_NAME} SHARED
   src/predicted_path_utils.cpp
   src/conversion.cpp
 )
@@ -19,7 +19,7 @@ if(BUILD_TESTING)
   ament_add_ros_isolated_gtest(test_object_recognition_utils ${test_files})
 
   target_link_libraries(test_object_recognition_utils
-    object_recognition_utils
+    ${PROJECT_NAME}
   )
 endif()
 

common/object_recognition_utils/README.md → common/autoware_object_recognition_utils/README.md

@@ -1,4 +1,4 @@
-# object_recognition_utils
+# autoware_object_recognition_utils
 
 ## Purpose