Merge branch 'tier4/main' into feat/robobus

tier4 · Dec 12, 2023 · 09d583a · 09d583a
2 parents ce3a580 + 59a7653
commit 09d583a
Show file tree

Hide file tree

Showing 473 changed files with 22,181 additions and 9,627 deletions.
diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
diff --git a/.github/workflows/spell-check-all.yaml b/.github/workflows/spell-check-all.yaml
@@ -2,6 +2,8 @@ name: spell-check-all
 
 on:
   workflow_dispatch:
+  schedule:
+    - cron: 0 0 * * *
 
 jobs:
   spell-check-all:

diff --git a/common/goal_distance_calculator/config/goal_distance_calculator.param.yaml b/common/goal_distance_calculator/config/goal_distance_calculator.param.yaml
@@ -1,3 +1,4 @@
 /**:
   ros__parameters:
     update_rate: 10.0
+    oneshot: false
diff --git a/common/goal_distance_calculator/launch/goal_distance_calculator.launch.xml b/common/goal_distance_calculator/launch/goal_distance_calculator.launch.xml
@@ -1,8 +1,6 @@
 <launch>
-  <arg name="oneshot" default="false"/>
   <arg name="config_file" default="$(find-pkg-share goal_distance_calculator)/config/goal_distance_calculator.param.yaml"/>
   <node pkg="goal_distance_calculator" exec="goal_distance_calculator_node" name="goal_distance_calculator" output="screen">
     <param from="$(var config_file)"/>
-    <param name="oneshot" value="$(var oneshot)"/>
   </node>
 </launch>
diff --git a/common/goal_distance_calculator/schema/goal_distance_calculator.schema.json b/common/goal_distance_calculator/schema/goal_distance_calculator.schema.json
@@ -0,0 +1,36 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for Goal Distance Calculator Node",
+  "type": "object",
+  "definitions": {
+    "goal_distance_calculator": {
+      "type": "object",
+      "properties": {
+        "update_rate": {
+          "type": "number",
+          "default": "10.0",
+          "exclusiveMinimum": 0,
+          "description": "Timer callback period. [Hz]"
+        },
+        "oneshot": {
+          "type": "boolean",
+          "default": "false",
+          "description": "Publish deviations just once or repeatedly."
+        }
+      },
+      "required": ["update_rate", "oneshot"]
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/goal_distance_calculator"
+        }
+      },
+      "required": ["ros__parameters"]
+    }
+  },
+  "required": ["/**"]
+}
diff --git a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp
@@ -40,8 +40,8 @@ GoalDistanceCalculatorNode::GoalDistanceCalculatorNode(const rclcpp::NodeOptions
   durable_qos.transient_local();
 
   // Node Parameter
-  node_param_.update_rate = declare_parameter("update_rate", 10.0);
-  node_param_.oneshot = declare_parameter("oneshot", true);
+  node_param_.update_rate = declare_parameter<double>("update_rate");
+  node_param_.oneshot = declare_parameter<bool>("oneshot");
 
   // Core
   goal_distance_calculator_ = std::make_unique<GoalDistanceCalculator>();

diff --git a/common/interpolation/test/src/test_spline_interpolation.cpp b/common/interpolation/test/src/test_spline_interpolation.cpp
@@ -222,16 +222,48 @@ TEST(spline_interpolation, splineByAkima)
 
 TEST(spline_interpolation, SplineInterpolation)
 {
-  // curve: query_keys is random
-  const std::vector<double> base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0};
-  const std::vector<double> base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0};
-  const std::vector<double> query_keys{0.0, 8.0, 18.0};
-  const std::vector<double> ans{-0.075611, 0.997242, 1.573258};
+  {
+    // curve: query_keys is random
+    const std::vector<double> base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0};
+    const std::vector<double> base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0};
+    const std::vector<double> query_keys{0.0, 8.0, 18.0};
+    const std::vector<double> ans{-0.075611, 0.997242, 1.573258};
+
+    SplineInterpolation s(base_keys, base_values);
+    const std::vector<double> query_values = s.getSplineInterpolatedValues(query_keys);
+
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {
+    // getSplineInterpolatedDiffValues
+    const std::vector<double> base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0};
+    const std::vector<double> base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0};
+    const std::vector<double> query_keys{0.0, 8.0, 12.0, 18.0};
+    const std::vector<double> ans{0.671301, 0.0509853, 0.209426, -0.253628};
 
-  SplineInterpolation s(base_keys, base_values);
-  const std::vector<double> query_values = s.getSplineInterpolatedValues(query_keys);
+    SplineInterpolation s(base_keys, base_values);
+    const std::vector<double> query_values = s.getSplineInterpolatedDiffValues(query_keys);
 
-  for (size_t i = 0; i < query_values.size(); ++i) {
-    EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {
+    // getSplineInterpolatedQuadDiffValues
+    const std::vector<double> base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0};
+    const std::vector<double> base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0};
+    const std::vector<double> query_keys{0.0, 8.0, 12.0, 18.0};
+    const std::vector<double> ans{-0.156582, 0.0440771, -0.0116873, -0.0495025};
+
+    SplineInterpolation s(base_keys, base_values);
+    const std::vector<double> query_values = s.getSplineInterpolatedQuadDiffValues(query_keys);
+
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
   }
 }
diff --git a/common/motion_utils/include/motion_utils/distance/distance.hpp b/common/motion_utils/include/motion_utils/distance/distance.hpp
@@ -15,17 +15,16 @@
 #ifndef MOTION_UTILS__DISTANCE__DISTANCE_HPP_
 #define MOTION_UTILS__DISTANCE__DISTANCE_HPP_
 
-#include <boost/optional.hpp>
-
 #include <algorithm>
 #include <cmath>
 #include <iostream>
+#include <optional>
 #include <tuple>
 #include <vector>
 
 namespace motion_utils
 {
-boost::optional<double> calcDecelDistWithJerkAndAccConstraints(
+std::optional<double> calcDecelDistWithJerkAndAccConstraints(
   const double current_vel, const double target_vel, const double current_acc, const double acc_min,
   const double jerk_acc, const double jerk_dec);
 

diff --git a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp
@@ -21,10 +21,9 @@
 #include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
 
-#include <boost/optional.hpp>
-
 #include <algorithm>
 #include <limits>
+#include <optional>
 #include <stdexcept>
 #include <vector>
 

diff --git a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp
@@ -18,12 +18,11 @@
 #include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp"
 #include <geometry_msgs/msg/point.hpp>
 
-#include <boost/optional.hpp>
-
+#include <optional>
 #include <utility>
 namespace motion_utils
 {
-boost::optional<std::pair<size_t, size_t>> getPathIndexRangeWithLaneId(
+std::optional<std::pair<size_t, size_t>> getPathIndexRangeWithLaneId(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id);
 
 size_t findNearestIndexFromLaneId(