[pull] main from autowarefoundation:main

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/osqp_interface/CMakeLists.txt → common/autoware_osqp_interface/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.14)
-project(osqp_interface)
+project(autoware_osqp_interface)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()
@@ -17,9 +17,9 @@ set(OSQP_INTERFACE_LIB_SRC
 )
 
 set(OSQP_INTERFACE_LIB_HEADERS
-  include/osqp_interface/csc_matrix_conv.hpp
-  include/osqp_interface/osqp_interface.hpp
-  include/osqp_interface/visibility_control.hpp
+  include/autoware/osqp_interface/csc_matrix_conv.hpp
+  include/autoware/osqp_interface/osqp_interface.hpp
+  include/autoware/osqp_interface/visibility_control.hpp
 )
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
@@ -28,18 +28,18 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
 )
 target_compile_options(${PROJECT_NAME} PRIVATE -Wno-error=old-style-cast -Wno-error=useless-cast)
 
-target_include_directories(osqp_interface
+target_include_directories(${PROJECT_NAME}
   SYSTEM PUBLIC
     "${OSQP_INCLUDE_DIR}"
     "${EIGEN3_INCLUDE_DIR}"
 )
 
-ament_target_dependencies(osqp_interface
+ament_target_dependencies(${PROJECT_NAME}
   Eigen3
   osqp_vendor
 )
 
-# crucial so downstream package builds because osqp_interface exposes osqp.hpp
+# crucial so downstream package builds because autoware_osqp_interface exposes osqp.hpp
 ament_export_include_directories("${OSQP_INCLUDE_DIR}")
 # crucial so the linking order is correct in a downstream package: libosqp_interface.a should come before libosqp.a
 ament_export_libraries(osqp::osqp)

common/osqp_interface/design/osqp_interface-design.md → common/autoware_osqp_interface/design/osqp_interface-design.md

@@ -1,6 +1,6 @@
 # Interface for the OSQP library
 
-This is the design document for the `osqp_interface` package.
+This is the design document for the `autoware_osqp_interface` package.
 
 ## Purpose / Use cases