Merge pull request #205 from tier4/sync-upstream

chore: sync upstream

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/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/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