feat: enable vehicle engage on manual driving

.github/workflows/sync-upstream.yaml

@@ -0,0 +1,28 @@
+name: sync-upstream
+
+on:
+  schedule:
+    - cron: 0 0 * * *
+  workflow_dispatch:
+
+jobs:
+  sync-upstream:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Generate token
+        id: generate-token
+        uses: tibdex/github-app-token@v1
+        with:
+          app_id: ${{ secrets.APP_ID }}
+          private_key: ${{ secrets.PRIVATE_KEY }}
+
+      - name: Run sync-branches
+        uses: autowarefoundation/autoware-github-actions/sync-branches@v1
+        with:
+          token: ${{ steps.generate-token.outputs.token }}
+          base-branch: tier4/main
+          sync-pr-branch: sync-upstream
+          sync-target-repository: https://github.com/autowarefoundation/autoware.universe.git
+          sync-target-branch: main
+          pr-title: "chore: sync upstream"
+          auto-merge-method: merge

control/engage_transition_manager/CMakeLists.txt

@@ -0,0 +1,43 @@
+cmake_minimum_required(VERSION 3.14)
+project(engage_transition_manager)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(engage_transition_manager_node SHARED
+  src/engage_transition_manager.cpp
+  src/state.cpp
+  src/data.cpp
+)
+
+rclcpp_components_register_node(engage_transition_manager_node
+  PLUGIN "engage_transition_manager::EngageTransitionManager"
+  EXECUTABLE engage_transition_manager_exe
+)
+
+rosidl_generate_interfaces(
+  ${PROJECT_NAME}
+  "msg/EngageTransitionManagerDebug.msg"
+  DEPENDENCIES builtin_interfaces
+)
+
+if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0)
+    rosidl_target_interfaces(engage_transition_manager_node
+    ${PROJECT_NAME} "rosidl_typesupport_cpp")
+else()
+    rosidl_get_typesupport_target(
+            cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
+    target_link_libraries(engage_transition_manager_node "${cpp_typesupport_target}")
+endif()
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    launch
+    config
+)

control/engage_transition_manager/README.md

@@ -0,0 +1 @@
+# engage_transition_manager

control/engage_transition_manager/config/engage_transition_manager.param.yaml

@@ -0,0 +1,12 @@
+/**:
+  ros__parameters:
+    frequency_hz: 10.0
+    engage_acceptable_limits:
+      dist_threshold: 2.0
+      yaw_threshold: 0.785
+      speed_threshold: 10.0
+    stable_check:
+      duration: 3.0
+      dist_threshold: 0.5
+      speed_threshold: 3.0
+      yaw_threshold: 0.314

control/engage_transition_manager/include/engage_transition_manager/data.hpp

@@ -0,0 +1,91 @@
+// Copyright 2022 Autoware Foundation
+//
+// 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 ENGAGE_TRANSITION_MANAGER__DATA_HPP_
+#define ENGAGE_TRANSITION_MANAGER__DATA_HPP_
+
+#include "engage_transition_manager/msg/engage_transition_manager_debug.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
+#include <autoware_auto_vehicle_msgs/msg/control_mode_report.hpp>
+#include <autoware_auto_vehicle_msgs/srv/autonomy_mode_change.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <tier4_system_msgs/msg/operation_mode.hpp>
+#include <tier4_system_msgs/srv/operation_mode_request.hpp>
+#include <tier4_vehicle_msgs/msg/control_mode.hpp>
+#include <tier4_vehicle_msgs/srv/control_mode_request.hpp>
+#include <tier4_vehicle_msgs/msg/is_autonomous_available.hpp>
+
+namespace engage_transition_manager
+{
+
+using nav_msgs::msg::Odometry;
+
+using autoware_auto_planning_msgs::msg::Trajectory;
+using autoware_auto_control_msgs::msg::AckermannControlCommand;
+using autoware_auto_vehicle_msgs::msg::ControlModeReport;
+using tier4_system_msgs::msg::OperationMode;
+using tier4_system_msgs::srv::OperationModeRequest;
+using tier4_vehicle_msgs::msg::ControlMode;
+using tier4_vehicle_msgs::srv::ControlModeRequest;
+using tier4_vehicle_msgs::msg::IsAutonomousAvailable;
+using engage_transition_manager::msg::EngageTransitionManagerDebug;
+
+enum class State {
+  STOP = 0,
+  MANUAL_DIRECT,
+  REMOTE_OPERATOR,
+  LOCAL_OPERATOR,
+  TRANSITION_TO_AUTO,
+  AUTONOMOUS,
+};
+
+struct Data
+{
+  bool is_auto_available;
+  State requested_state;
+  Odometry kinematics;
+  Trajectory trajectory;
+  AckermannControlCommand control_cmd;
+  ControlModeReport current_control_mode;
+};
+
+struct EngageAcceptableParam
+{
+  double dist_threshold = 2.0;
+  double speed_threshold = 10.0;
+  double yaw_threshold = 0.785;
+  double large_acc_threshold = 2.0;
+};
+
+struct StableCheckParam
+{
+  double duration = 3.0;
+  double dist_threshold = 0.5;
+  double speed_threshold = 3.0;
+  double yaw_threshold = M_PI / 10.0;
+};
+
+uint8_t toMsg(const State s);
+State toEnum(const OperationMode s);
+std::string toStr(const State s);
+bool isManual(const State s);
+bool isAuto(const State s);
+
+}  // namespace engage_transition_manager
+
+#endif  // ENGAGE_TRANSITION_MANAGER__DATA_HPP_

control/engage_transition_manager/include/engage_transition_manager/engage_transition_manager.hpp

@@ -0,0 +1,90 @@
+// Copyright 2022 Autoware Foundation
+//
+// 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 ENGAGE_TRANSITION_MANAGER__ENGAGE_TRANSITION_MANAGER_HPP_
+#define ENGAGE_TRANSITION_MANAGER__ENGAGE_TRANSITION_MANAGER_HPP_
+
+#include "engage_transition_manager/msg/engage_transition_manager_debug.hpp"
+
+#include <engage_transition_manager/data.hpp>
+#include <engage_transition_manager/state.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <tier4_system_msgs/msg/operation_mode.hpp>
+#include <tier4_system_msgs/srv/operation_mode_request.hpp>
+#include <tier4_vehicle_msgs/msg/control_mode.hpp>
+#include <tier4_vehicle_msgs/srv/control_mode_request.hpp>
+#include <tier4_vehicle_msgs/msg/is_autonomous_available.hpp>
+
+namespace engage_transition_manager
+{
+
+// using nav_msgs::msg::Odometry;
+
+// using autoware_auto_planning_msgs::msg::Trajectory;
+// using autoware_auto_control_msgs::msg::AckermannControlCommand;
+// using autoware_auto_vehicle_msgs::msg::ControlModeReport;
+// using tier4_system_msgs::msg::OperationMode;
+// using tier4_system_msgs::srv::OperationModeRequest;
+// using tier4_vehicle_msgs::msg::IsAutonomousAvailable;
+// using engage_transition_manager::msg::EngageTransitionManagerDebug;
+
+class EngageTransitionManager : public rclcpp::Node
+{
+public:
+  explicit EngageTransitionManager(const rclcpp::NodeOptions & options);
+  ~EngageTransitionManager() = default;
+
+private:
+  rclcpp::Publisher<OperationMode>::SharedPtr pub_operation_mode_;
+  rclcpp::Publisher<IsAutonomousAvailable>::SharedPtr pub_auto_available_;
+  rclcpp::Publisher<EngageTransitionManagerDebug>::SharedPtr pub_debug_info_;
+  rclcpp::Subscription<Odometry>::SharedPtr sub_vehicle_kinematics_;
+  rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_;
+  rclcpp::Subscription<AckermannControlCommand>::SharedPtr sub_control_cmd_;
+  rclcpp::Subscription<ControlModeReport>::SharedPtr sub_control_mode_;
+  rclcpp::Service<OperationModeRequest>::SharedPtr srv_mode_change_server_;
+  rclcpp::Client<OperationModeRequest>::SharedPtr srv_mode_change_client_;
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  std::unique_ptr<EngageStateBase> engage_transition_manager_;
+
+  Data data_;
+
+  State updateState(const Data & data);
+  State getCurrentState() { return engage_transition_manager_->getCurrentState(); };
+
+  EngageAcceptableParam engage_acceptable_param_;
+  StableCheckParam stable_check_param_;
+
+  bool checkEngageAvailable();
+
+  void publishData();
+
+  // update information
+  void onTimer();
+
+  void onOperationModeRequest(
+    const OperationModeRequest::Request::SharedPtr request,
+    const OperationModeRequest::Response::SharedPtr response);
+
+  mutable EngageTransitionManagerDebug debug_info_;
+};
+
+}  // namespace engage_transition_manager
+
+#endif  // ENGAGE_TRANSITION_MANAGER__ENGAGE_TRANSITION_MANAGER_HPP_

control/engage_transition_manager/include/engage_transition_manager/state.hpp

@@ -0,0 +1,104 @@
+// Copyright 2022 Autoware Foundation
+//
+// 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 ENGAGE_TRANSITION_MANAGER__STATE_HPP_
+#define ENGAGE_TRANSITION_MANAGER__STATE_HPP_
+
+#include <engage_transition_manager/data.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <tier4_system_msgs/msg/operation_mode.hpp>
+#include <tier4_system_msgs/srv/operation_mode_request.hpp>
+#include <tier4_vehicle_msgs/srv/control_mode_request.hpp>
+
+namespace engage_transition_manager
+{
+class EngageStateBase
+{
+public:
+  EngageStateBase(const State state, rclcpp::Node * node);
+  ~EngageStateBase() = default;
+
+  virtual State update() = 0;
+
+  State getCurrentState() { return state_; };
+  void setData(const Data & data) { data_ = data; };
+  void setParam(const StableCheckParam & param) { stable_check_param_ = param; };
+
+protected:
+  rclcpp::Client<ControlModeRequest>::SharedPtr srv_mode_change_client_;
+
+  rclcpp::Logger logger_;
+  rclcpp::Clock::SharedPtr clock_;
+
+  State state_;
+  Data data_;
+  StableCheckParam stable_check_param_;
+
+  State defaultUpdateOnManual();
+  bool sendAutonomousModeRequest();
+  bool sendManualModeRequest();
+};
+
+class NoneState : public EngageStateBase
+{
+public:
+  NoneState(rclcpp::Node * node) : EngageStateBase(State::STOP, node){};
+  State update() override { return defaultUpdateOnManual(); };
+};
+
+class RemoteState : public EngageStateBase
+{
+public:
+  RemoteState(rclcpp::Node * node) : EngageStateBase(State::REMOTE_OPERATOR, node){};
+  State update() override { return defaultUpdateOnManual(); };
+};
+
+class DirectState : public EngageStateBase
+{
+public:
+  DirectState(rclcpp::Node * node) : EngageStateBase(State::MANUAL_DIRECT, node){};
+  State update() override { return defaultUpdateOnManual(); };
+};
+
+class LocalState : public EngageStateBase
+{
+public:
+  LocalState(rclcpp::Node * node) : EngageStateBase(State::LOCAL_OPERATOR, node){};
+  State update() override { return defaultUpdateOnManual(); };
+};
+
+class TransitionState : public EngageStateBase
+{
+public:
+  TransitionState(rclcpp::Node * node) : EngageStateBase(State::TRANSITION_TO_AUTO, node){};
+  State update() override;
+
+private:
+  std::unique_ptr<rclcpp::Time> stable_start_time_;
+  bool checkSystemStable();
+};
+
+class AutonomousState : public EngageStateBase
+{
+public:
+  AutonomousState(rclcpp::Node * node) : EngageStateBase(State::AUTONOMOUS, node){};
+  State update() override;
+};
+
+}  // namespace engage_transition_manager
+
+#endif  // ENGAGE_TRANSITION_MANAGER__STATE_HPP_