Merge branch 'main' into change-merger-priority

common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

@@ -24,6 +24,7 @@
 #include <algorithm>
 #include <limits>
 #include <stdexcept>
+#include <utility>
 #include <vector>
 
 namespace motion_utils
@@ -580,6 +581,43 @@ double calcArcLength(const T & points)
   return calcSignedArcLength(points, 0, points.size() - 1);
 }
 
+template <class T>
+inline std::vector<double> calcCurvature(const T & points)
+{
+  std::vector<double> curvature_vec(points.size());
+
+  for (size_t i = 1; i < points.size() - 1; ++i) {
+    const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1));
+    const auto p2 = tier4_autoware_utils::getPoint(points.at(i));
+    const auto p3 = tier4_autoware_utils::getPoint(points.at(i + 1));
+    curvature_vec.at(i) = (tier4_autoware_utils::calcCurvature(p1, p2, p3));
+  }
+  curvature_vec.at(0) = curvature_vec.at(1);
+  curvature_vec.at(curvature_vec.size() - 1) = curvature_vec.at(curvature_vec.size() - 2);
+
+  return curvature_vec;
+}
+
+template <class T>
+inline std::vector<std::pair<double, double>> calcCurvatureAndArcLength(const T & points)
+{
+  // Note that arclength is for the segment, not the sum.
+  std::vector<std::pair<double, double>> curvature_arc_length_vec;
+  curvature_arc_length_vec.push_back(std::pair(0.0, 0.0));
+  for (size_t i = 1; i < points.size() - 1; ++i) {
+    const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1));
+    const auto p2 = tier4_autoware_utils::getPoint(points.at(i));
+    const auto p3 = tier4_autoware_utils::getPoint(points.at(i + 1));
+    const double curvature = tier4_autoware_utils::calcCurvature(p1, p2, p3);
+    const double arc_length = tier4_autoware_utils::calcDistance2d(points.at(i - 1), points.at(i)) +
+                              tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1));
+    curvature_arc_length_vec.push_back(std::pair(curvature, arc_length));
+  }
+  curvature_arc_length_vec.push_back(std::pair(0.0, 0.0));
+
+  return curvature_arc_length_vec;
+}
+
 /**
  * @brief Calculate distance to the forward stop point from the given src index
  */

common/tier4_autoware_utils/include/tier4_autoware_utils/ros/uuid_helper.hpp

@@ -0,0 +1,34 @@
+// Copyright 2022 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__ROS__UUID_HELPER_HPP_
+#define TIER4_AUTOWARE_UTILS__ROS__UUID_HELPER_HPP_
+
+#include <unique_identifier_msgs/msg/uuid.hpp>
+
+#include <string>
+
+namespace tier4_autoware_utils
+{
+inline std::string toHexString(const unique_identifier_msgs::msg::UUID & id)
+{
+  std::stringstream ss;
+  for (auto i = 0; i < 16; ++i) {
+    ss << std::hex << std::setfill('0') << std::setw(2) << +id.uuid[i];
+  }
+  return ss.str();
+}
+}  // namespace tier4_autoware_utils
+
+#endif  // TIER4_AUTOWARE_UTILS__ROS__UUID_HELPER_HPP_

common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp

@@ -33,6 +33,7 @@
 #include "tier4_autoware_utils/ros/self_pose_listener.hpp"
 #include "tier4_autoware_utils/ros/transform_listener.hpp"
 #include "tier4_autoware_utils/ros/update_param.hpp"
+#include "tier4_autoware_utils/ros/uuid_helper.hpp"
 #include "tier4_autoware_utils/ros/wait_for_param.hpp"
 #include "tier4_autoware_utils/system/stop_watch.hpp"
 

common/tier4_autoware_utils/package.xml

@@ -6,6 +6,8 @@
   <description>The tier4_autoware_utils package</description>
   <maintainer email="takamasa.horibe@tier4.jp">Takamasa Horibe</maintainer>
   <maintainer email="kenji.miyake@tier4.jp">Kenji Miyake</maintainer>
+  <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
+  <maintainer email="yutaka.shimizu@tier4.jp">Yutaka Shimizu</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
@@ -23,6 +25,7 @@
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tier4_debug_msgs</depend>
+  <depend>unique_identifier_msgs</depend>
   <depend>visualization_msgs</depend>
 
   <test_depend>ament_cmake_ros</test_depend>

control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml

@@ -3,6 +3,8 @@
     transition_timeout: 10.0
     frequency_hz: 10.0
     check_engage_condition: false # set false if you do not want to care about the engage condition.
+    nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index
+    nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index
     engage_acceptable_limits:
       allow_autonomous_in_stopped: true  # no check if the velocity is zero, always allowed.
       dist_threshold: 1.5

control/operation_mode_transition_manager/src/state.cpp

@@ -43,6 +43,10 @@ AutonomousMode::AutonomousMode(rclcpp::Node * node)
     "trajectory", 1, [this](const Trajectory::SharedPtr msg) { trajectory_ = *msg; });
 
   check_engage_condition_ = node->declare_parameter<bool>("check_engage_condition");
+  nearest_dist_deviation_threshold_ =
+    node->declare_parameter<double>("nearest_dist_deviation_threshold");
+  nearest_yaw_deviation_threshold_ =
+    node->declare_parameter<double>("nearest_yaw_deviation_threshold");
 
   // params for mode change available
   {
@@ -86,9 +90,6 @@ bool AutonomousMode::isModeChangeCompleted()
     return true;
   }
 
-  constexpr auto dist_max = 5.0;
-  constexpr auto yaw_max = M_PI_4;
-
   const auto current_speed = kinematics_.twist.twist.linear.x;
   const auto & param = engage_acceptable_param_;
 
@@ -107,8 +108,9 @@ bool AutonomousMode::isModeChangeCompleted()
     return unstable();
   }
 
-  const auto closest_idx =
-    findNearestIndex(trajectory_.points, kinematics_.pose.pose, dist_max, yaw_max);
+  const auto closest_idx = findNearestIndex(
+    trajectory_.points, kinematics_.pose.pose, nearest_dist_deviation_threshold_,
+    nearest_yaw_deviation_threshold_);
   if (!closest_idx) {
     RCLCPP_INFO(logger_, "Not stable yet: closest point not found");
     return unstable();
@@ -199,9 +201,6 @@ bool AutonomousMode::isModeChangeAvailable()
     return true;
   }
 
-  constexpr auto dist_max = 100.0;
-  constexpr auto yaw_max = M_PI_4;
-
   const auto current_speed = kinematics_.twist.twist.linear.x;
   const auto target_control_speed = control_cmd_.longitudinal.speed;
   const auto & param = engage_acceptable_param_;
@@ -213,8 +212,9 @@ bool AutonomousMode::isModeChangeAvailable()
     return false;
   }
 
-  const auto closest_idx =
-    findNearestIndex(trajectory_.points, kinematics_.pose.pose, dist_max, yaw_max);
+  const auto closest_idx = findNearestIndex(
+    trajectory_.points, kinematics_.pose.pose, nearest_dist_deviation_threshold_,
+    nearest_yaw_deviation_threshold_);
   if (!closest_idx) {
     RCLCPP_INFO(logger_, "Engage unavailable: closest point not found");
     debug_info_ = DebugInfo{};  // all false

control/operation_mode_transition_manager/src/state.hpp

@@ -72,7 +72,9 @@ class AutonomousMode : public ModeChangeBase
   rclcpp::Logger logger_;
   rclcpp::Clock::SharedPtr clock_;
 
-  bool check_engage_condition_ = true;  // if false, the vehicle is engaged without any checks.
+  bool check_engage_condition_ = true;       // if false, the vehicle is engaged without any checks.
+  double nearest_dist_deviation_threshold_;  // [m] for finding nearest index
+  double nearest_yaw_deviation_threshold_;   // [rad] for finding nearest index
   EngageAcceptableParam engage_acceptable_param_;
   StableCheckParam stable_check_param_;
   AckermannControlCommand control_cmd_;

control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp

@@ -33,7 +33,6 @@
 #include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp"
 #include "autoware_auto_control_msgs/msg/longitudinal_command.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
-#include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp"
 #include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp"
 #include "geometry_msgs/msg/accel_stamped.hpp"
 #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"

launch/tier4_control_launch/CMakeLists.txt

@@ -6,6 +6,5 @@ autoware_package()
 
 ament_auto_package(
   INSTALL_TO_SHARE
-  config
   launch
 )

launch/tier4_control_launch/README.md

@@ -12,9 +12,15 @@ Please see `<exec_depend>` in `package.xml`.
 
 You can include as follows in `*.launch.xml` to use `control.launch.py`.
 
+Note that you should provide parameter paths as `PACKAGE_param_path`. The list of parameter paths you should provide is written at the top of `planning.launch.xml`.
+
 ```xml
 <include file="$(find-pkg-share tier4_control_launch)/launch/control.launch.py">
   <!-- options for lateral_controller_mode: mpc_follower, pure_pursuit -->
+  <!-- Parameter files -->
+  <arg name="FOO_NODE_param_path" value="..."/>
+  <arg name="BAR_NODE_param_path" value="..."/>
+  ...
   <arg name="lateral_controller_mode" value="mpc_follower" />
 </include>
 ```