Merge branch 'main' into feature/multiple_candidate_paths

common/component_interface_tools/CMakeLists.txt

@@ -0,0 +1,7 @@
+cmake_minimum_required(VERSION 3.14)
+project(component_interface_tools)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+ament_auto_add_executable(service_log_checker src/service_log_checker.cpp)
+ament_auto_package(INSTALL_TO_SHARE launch)

common/component_interface_tools/launch/service_log_checker.launch.xml

@@ -0,0 +1,3 @@
+<launch>
+  <node pkg="component_interface_tools" exec="service_log_checker" name="service_log_checker"/>
+</launch>

common/component_interface_tools/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>component_interface_tools</name>
+  <version>0.1.0</version>
+  <description>The component_interface_tools package</description>
+  <maintainer email="isamu.takagi@tier4.jp">Takagi, Isamu</maintainer>
+  <maintainer email="makoto.yabuta@tier4.jp">yabuta</maintainer>
+  <maintainer email="kahhooi.tan@tier4.jp">Kah Hooi Tan</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <depend>diagnostic_updater</depend>
+  <depend>fmt</depend>
+  <depend>rclcpp</depend>
+  <depend>tier4_system_msgs</depend>
+  <depend>yaml_cpp_vendor</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

common/component_interface_tools/src/service_log_checker.cpp

@@ -0,0 +1,110 @@
+// 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.
+
+#include "service_log_checker.hpp"
+
+#include <yaml-cpp/yaml.h>
+
+#include <memory>
+#include <string>
+
+#define FMT_HEADER_ONLY
+#include <fmt/format.h>
+
+ServiceLogChecker::ServiceLogChecker() : Node("service_log_checker"), diagnostics_(this)
+{
+  sub_ = create_subscription<ServiceLog>(
+    "/service_log", 50, std::bind(&ServiceLogChecker::on_service_log, this, std::placeholders::_1));
+
+  diagnostics_.setHardwareID(get_name());
+  diagnostics_.add("response_status", this, &ServiceLogChecker::update_diagnostics);
+}
+
+void ServiceLogChecker::on_service_log(const ServiceLog::ConstSharedPtr msg)
+{
+  try {
+    // Ignore service request.
+    if (msg->type == ServiceLog::CLIENT_REQUEST || msg->type == ServiceLog::SERVER_REQUEST) {
+      return;
+    }
+
+    // Ignore service errors.
+    if (msg->type == ServiceLog::ERROR_UNREADY) {
+      return set_error(*msg, "not ready");
+    }
+    if (msg->type == ServiceLog::ERROR_TIMEOUT) {
+      return set_error(*msg, "timeout");
+    }
+
+    // Ignore version API because it doesn't have response status.
+    if (msg->name == "/api/interface/version") {
+      return;
+    }
+
+    // Parse response data.
+    const auto status = YAML::Load(msg->yaml)["status"];
+    if (!status) {
+      return set_error(*msg, "no response status");
+    }
+
+    // Check response status.
+    const auto success = status["success"].as<bool>();
+    if (!success) {
+      const auto message = status["message"].as<std::string>();
+      const auto code = status["code"].as<uint16_t>();
+      return set_error(*msg, fmt::format("status code {} '{}'", code, message));
+    }
+  } catch (const YAML::Exception & error) {
+    return set_error(*msg, fmt::format("invalid data: '{}'", error.what()));
+  }
+
+  set_success(*msg);
+}
+
+void ServiceLogChecker::set_success(const ServiceLog & msg)
+{
+  errors_.erase(fmt::format("{} ({})", msg.name, msg.node));
+}
+
+void ServiceLogChecker::set_error(const ServiceLog & msg, const std::string & log)
+{
+  errors_[fmt::format("{} ({})", msg.name, msg.node)] = log;
+  RCLCPP_ERROR_STREAM(get_logger(), fmt::format("{}: {} ({})", msg.name, log, msg.node));
+}
+
+void ServiceLogChecker::update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat)
+{
+  using diagnostic_msgs::msg::DiagnosticStatus;
+
+  for (const auto & error : errors_) {
+    stat.add(error.first, error.second);
+  }
+
+  if (errors_.empty()) {
+    stat.summary(DiagnosticStatus::OK, "OK");
+  } else {
+    stat.summary(DiagnosticStatus::ERROR, "ERROR");
+  }
+}
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::executors::SingleThreadedExecutor executor;
+  auto node = std::make_shared<ServiceLogChecker>();
+  executor.add_node(node);
+  executor.spin();
+  executor.remove_node(node);
+  rclcpp::shutdown();
+}

common/component_interface_tools/src/service_log_checker.hpp

@@ -0,0 +1,42 @@
+// 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 SERVICE_LOG_CHECKER_HPP_
+#define SERVICE_LOG_CHECKER_HPP_
+
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <tier4_system_msgs/msg/service_log.hpp>
+
+#include <string>
+#include <unordered_map>
+
+class ServiceLogChecker : public rclcpp::Node
+{
+public:
+  ServiceLogChecker();
+
+private:
+  using ServiceLog = tier4_system_msgs::msg::ServiceLog;
+  rclcpp::Subscription<ServiceLog>::SharedPtr sub_;
+  diagnostic_updater::Updater diagnostics_;
+  void on_service_log(const ServiceLog::ConstSharedPtr msg);
+  void set_success(const ServiceLog & msg);
+  void set_error(const ServiceLog & msg, const std::string & log);
+  void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat);
+  std::unordered_map<std::string, std::string> errors_;
+};
+
+#endif  // SERVICE_LOG_CHECKER_HPP_

common/component_interface_utils/CMakeLists.txt

@@ -3,5 +3,4 @@ project(component_interface_utils)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()
-ament_export_dependencies(tier4_system_msgs)
 ament_auto_package()

common/component_interface_utils/package.xml

@@ -13,6 +13,7 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
   <build_depend>autoware_cmake</build_depend>
+  <build_depend>tier4_system_msgs</build_depend>
 
   <depend>autoware_adapi_v1_msgs</depend>
   <depend>rclcpp</depend>

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>

control/trajectory_follower/design/mpc_lateral_controller-design.md

@@ -93,6 +93,7 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving.
 | path_smoothing_times                         | int    | number of times of applying path smoothing filter                                                                                                 | 1             |
 | curvature_smoothing_num_ref_steer            | double | index distance of points used in curvature calculation for reference steer command: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 35            |
 | curvature_smoothing_num_traj                 | double | index distance of points used in curvature calculation for trajectory: p(i-num), p(i), p(i+num). larger num makes less noisy values.              | 1             |
+| extend_trajectory_for_end_yaw_control        | bool   | trajectory extending flag for end yaw control.                                                                                                    | true          |
 | steering_lpf_cutoff_hz                       | double | cutoff frequency of lowpass filter for steering output command [hz]                                                                               | 3.0           |
 | admissible_position_error                    | double | stop vehicle when following position error is larger than this value [m].                                                                         | 5.0           |
 | admissible_yaw_error_rad                     | double | stop vehicle when following yaw angle error is larger than this value [rad].                                                                      | 1.57          |

control/trajectory_follower/include/trajectory_follower/mpc.hpp

@@ -400,7 +400,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
     const autoware_auto_planning_msgs::msg::Trajectory & trajectory_msg,
     const double traj_resample_dist, const bool enable_path_smoothing,
     const int path_filter_moving_ave_num, const int curvature_smoothing_num_traj,
-    const int curvature_smoothing_num_ref_steer);
+    const int curvature_smoothing_num_ref_steer, const bool extend_trajectory_for_end_yaw_control);
   /**
    * @brief set the vehicle model of this MPC
    */

control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp

@@ -97,6 +97,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController
   //!< @brief path resampling interval [m]
   double m_traj_resample_dist;
 
+  //!< @brief flag of traj extending for terminal yaw
+  bool m_extend_trajectory_for_end_yaw_control;
+
   /* parameters for stop state */
   double m_stop_state_entry_ego_speed;
   double m_stop_state_entry_target_speed;

control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp

@@ -179,6 +179,21 @@ TRAJECTORY_FOLLOWER_PUBLIC bool calcNearestPoseInterp(
 //  */
 TRAJECTORY_FOLLOWER_PUBLIC double calcStopDistance(
   const autoware_auto_planning_msgs::msg::Trajectory & current_trajectory, const int origin);
+
+/**
+ * @brief extend terminal points
+ * Note: The current MPC does not properly take into account the attitude angle at the end of the
+ * path. By extending the end of the path in the attitude direction, the MPC can consider the
+ * attitude angle well, resulting in improved control performance. If the trajectory is
+ * well-defined considering the end point attitude angle, this feature is not necessary.
+ * @param [in] terminal yaw
+ * @param [in] extend interval
+ * @param [in] flag of forward shift
+ * @param [out] extended trajectory
+ */
+void extendTrajectoryInYawDirection(
+  const double yaw, const double interval, const bool is_forward_shift, MPCTrajectory & traj);
+
 }  // namespace MPCUtils
 }  // namespace trajectory_follower
 }  // namespace control

control/trajectory_follower/src/mpc.cpp

@@ -180,7 +180,7 @@ void MPC::setReferenceTrajectory(
   const autoware_auto_planning_msgs::msg::Trajectory & trajectory_msg,
   const double traj_resample_dist, const bool enable_path_smoothing,
   const int path_filter_moving_ave_num, const int curvature_smoothing_num_traj,
-  const int curvature_smoothing_num_ref_steer)
+  const int curvature_smoothing_num_ref_steer, const bool extend_trajectory_for_end_yaw_control)
 {
   trajectory_follower::MPCTrajectory mpc_traj_raw;        // received raw trajectory
   trajectory_follower::MPCTrajectory mpc_traj_resampled;  // resampled trajectory
@@ -217,6 +217,17 @@ void MPC::setReferenceTrajectory(
     }
   }
 
+  /* extend terminal points
+   * Note: The current MPC does not properly take into account the attitude angle at the end of the
+   * path. By extending the end of the path in the attitude direction, the MPC can consider the
+   * attitude angle well, resulting in improved control performance. If the trajectory is
+   * well-defined considering the end point attitude angle, this feature is not necessary.
+   */
+  if (extend_trajectory_for_end_yaw_control) {
+    trajectory_follower::MPCUtils::extendTrajectoryInYawDirection(
+      mpc_traj_raw.yaw.back(), traj_resample_dist, m_is_forward_shift, mpc_traj_smoothed);
+  }
+
   /* calculate yaw angle */
   trajectory_follower::MPCUtils::calcTrajectoryYawFromXY(&mpc_traj_smoothed, m_is_forward_shift);
   trajectory_follower::MPCUtils::convertEulerAngleToMonotonic(&mpc_traj_smoothed.yaw);

control/trajectory_follower/src/mpc_lateral_controller.cpp

@@ -65,6 +65,8 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node}
   m_mpc.m_admissible_yaw_error_rad = node_->declare_parameter<double>("admissible_yaw_error_rad");
   m_mpc.m_use_steer_prediction = node_->declare_parameter<bool>("use_steer_prediction");
   m_mpc.m_param.steer_tau = node_->declare_parameter<double>("vehicle_model_steer_tau");
+  m_extend_trajectory_for_end_yaw_control =
+    node_->declare_parameter<bool>("extend_trajectory_for_end_yaw_control");
 
   /* stop state parameters */
   m_stop_state_entry_ego_speed = node_->declare_parameter<double>("stop_state_entry_ego_speed");
@@ -302,7 +304,8 @@ void MpcLateralController::setTrajectory(
 
   m_mpc.setReferenceTrajectory(
     *msg, m_traj_resample_dist, m_enable_path_smoothing, m_path_filter_moving_ave_num,
-    m_curvature_smoothing_num_traj, m_curvature_smoothing_num_ref_steer);
+    m_curvature_smoothing_num_traj, m_curvature_smoothing_num_ref_steer,
+    m_extend_trajectory_for_end_yaw_control);
 
   // update trajectory buffer to check the trajectory shape change.
   m_trajectory_buffer.push_back(*m_current_trajectory_ptr);

control/trajectory_follower/src/mpc_utils.cpp

@@ -265,10 +265,9 @@ bool calcMPCTrajectoryTime(MPCTrajectory & traj)
   traj.relative_time.clear();
   traj.relative_time.push_back(t);
   for (size_t i = 0; i < traj.x.size() - 1; ++i) {
-    const double dx = traj.x.at(i + 1) - traj.x.at(i);
-    const double dy = traj.y.at(i + 1) - traj.y.at(i);
-    const double dz = traj.z.at(i + 1) - traj.z.at(i);
-    const double dist = std::sqrt(dx * dx + dy * dy + dz * dz);
+    const double dist = std::hypot(
+      traj.x.at(i + 1) - traj.x.at(i), traj.y.at(i + 1) - traj.y.at(i),
+      traj.z.at(i + 1) - traj.z.at(i));
     const double v = std::max(std::fabs(traj.vx.at(i)), 0.1);
     t += (dist / v);
     traj.relative_time.push_back(t);
@@ -404,6 +403,31 @@ double calcStopDistance(
   return stop_dist;
 }
 
+void extendTrajectoryInYawDirection(
+  const double yaw, const double interval, const bool is_forward_shift, MPCTrajectory & traj)
+{
+  // set terminal yaw
+  traj.yaw.back() = yaw;
+
+  // get terminal pose
+  autoware_auto_planning_msgs::msg::Trajectory autoware_traj;
+  autoware::motion::control::trajectory_follower::MPCUtils::convertToAutowareTrajectory(
+    traj, autoware_traj);
+  auto extended_pose = autoware_traj.points.back().pose;
+
+  constexpr double extend_dist = 10.0;
+  constexpr double extend_vel = 10.0;
+  const double x_offset = is_forward_shift ? interval : -interval;
+  const double dt = interval / extend_vel;
+  const size_t num_extended_point = static_cast<size_t>(extend_dist / interval);
+  for (size_t i = 0; i < num_extended_point; ++i) {
+    extended_pose = tier4_autoware_utils::calcOffsetPose(extended_pose, x_offset, 0.0, 0.0);
+    traj.push_back(
+      extended_pose.position.x, extended_pose.position.y, extended_pose.position.z, traj.yaw.back(),
+      extend_vel, traj.k.back(), traj.smooth_k.back(), traj.relative_time.back() + dt);
+  }
+}
+
 }  // namespace MPCUtils
 }  // namespace trajectory_follower
 }  // namespace control