feat(operation_mode_transition_manager): add package to manage vehicl…

…e autonomous mode change (#1246)

* add engage_transition_manager

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* rename to operation mode transition manager

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix precommit

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix cpplint

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix topic name & vehicle_info

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* update launch

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* update default param

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* add allow_autonomous_in_stopped

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix typo

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix precommit

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

control/operation_mode_transition_manager/CMakeLists.txt

@@ -0,0 +1,46 @@
+cmake_minimum_required(VERSION 3.14)
+project(operation_mode_transition_manager)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(operation_mode_transition_manager_node SHARED
+  src/operation_mode_transition_manager.cpp
+  src/state.cpp
+  src/data.cpp
+)
+
+rclcpp_components_register_node(operation_mode_transition_manager_node
+  PLUGIN "operation_mode_transition_manager::OperationModeTransitionManager"
+  EXECUTABLE operation_mode_transition_manager_exe
+)
+
+rosidl_generate_interfaces(
+  ${PROJECT_NAME}
+  "msg/OperationModeTransitionManagerDebug.msg"
+  DEPENDENCIES builtin_interfaces
+)
+
+# to use same package defined message
+if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0)
+    rosidl_target_interfaces(operation_mode_transition_manager_node
+    ${PROJECT_NAME} "rosidl_typesupport_cpp")
+else()
+    rosidl_get_typesupport_target(
+            cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
+    target_link_libraries(operation_mode_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/operation_mode_transition_manager/README.md

@@ -0,0 +1 @@
+# operation_mode_transition_manager

control/operation_mode_transition_manager/config/engage_transition_manager.param.yaml

@@ -0,0 +1,18 @@
+/**:
+  ros__parameters:
+    frequency_hz: 10.0
+    engage_acceptable_limits:
+      allow_autonomous_in_stopped: true  # no check if the velocity is zero, always allowed.
+      dist_threshold: 1.5
+      yaw_threshold: 0.524
+      speed_upper_threshold: 10.0
+      speed_lower_threshold: -10.0
+      acc_threshold: 1.5
+      lateral_acc_threshold: 1.0
+      lateral_acc_diff_threshold: 0.5
+    stable_check:
+      duration: 0.1
+      dist_threshold: 1.5
+      speed_upper_threshold: 2.0
+      speed_lower_threshold: -2.0
+      yaw_threshold: 0.262

control/operation_mode_transition_manager/include/operation_mode_transition_manager/data.hpp

@@ -0,0 +1,101 @@
+// 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 OPERATION_MODE_TRANSITION_MANAGER__DATA_HPP_
+#define OPERATION_MODE_TRANSITION_MANAGER__DATA_HPP_
+
+#include "operation_mode_transition_manager/msg/operation_mode_transition_manager_debug.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+#include <vehicle_info_util/vehicle_info_util.hpp>
+
+#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
+#include <autoware_auto_planning_msgs/msg/trajectory.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/is_autonomous_available.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 <string>
+
+namespace operation_mode_transition_manager
+{
+
+using nav_msgs::msg::Odometry;
+
+using autoware_auto_control_msgs::msg::AckermannControlCommand;
+using autoware_auto_planning_msgs::msg::Trajectory;
+using autoware_auto_vehicle_msgs::msg::ControlModeReport;
+using operation_mode_transition_manager::msg::OperationModeTransitionManagerDebug;
+using tier4_system_msgs::msg::IsAutonomousAvailable;
+using tier4_system_msgs::msg::OperationMode;
+using tier4_system_msgs::srv::OperationModeRequest;
+using tier4_vehicle_msgs::msg::ControlMode;
+using tier4_vehicle_msgs::srv::ControlModeRequest;
+
+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;
+  OperationMode current_gate_operation_mode;
+  vehicle_info_util::VehicleInfo vehicle_info;
+};
+
+struct EngageAcceptableParam
+{
+  bool allow_autonomous_in_stopped = true;
+  double dist_threshold = 2.0;
+  double speed_upper_threshold = 10.0;
+  double speed_lower_threshold = -10.0;
+  double yaw_threshold = 0.785;
+  double acc_threshold = 2.0;
+  double lateral_acc_threshold = 2.0;
+  double lateral_acc_diff_threshold = 1.0;
+};
+
+struct StableCheckParam
+{
+  double duration = 3.0;
+  double dist_threshold = 0.5;
+  double speed_upper_threshold = 3.0;
+  double speed_lower_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 operation_mode_transition_manager
+
+#endif  // OPERATION_MODE_TRANSITION_MANAGER__DATA_HPP_

control/operation_mode_transition_manager/include/operation_mode_transition_manager/operation_mode_transition_manager.hpp

@@ -0,0 +1,86 @@
+// 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 OPERATION_MODE_TRANSITION_MANAGER__OPERATION_MODE_TRANSITION_MANAGER_HPP_
+#define OPERATION_MODE_TRANSITION_MANAGER__OPERATION_MODE_TRANSITION_MANAGER_HPP_
+
+#include <operation_mode_transition_manager/data.hpp>
+#include <operation_mode_transition_manager/msg/operation_mode_transition_manager_debug.hpp>
+#include <operation_mode_transition_manager/state.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <vehicle_info_util/vehicle_info_util.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/is_autonomous_available.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 <memory>
+#include <utility>
+
+namespace operation_mode_transition_manager
+{
+
+class OperationModeTransitionManager : public rclcpp::Node
+{
+public:
+  explicit OperationModeTransitionManager(const rclcpp::NodeOptions & options);
+  ~OperationModeTransitionManager() = default;
+
+private:
+  rclcpp::Publisher<OperationMode>::SharedPtr pub_operation_mode_;
+  rclcpp::Publisher<IsAutonomousAvailable>::SharedPtr pub_auto_available_;
+  rclcpp::Publisher<OperationModeTransitionManagerDebug>::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::Subscription<OperationMode>::SharedPtr sub_gate_operation_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> operation_mode_transition_manager_;
+
+  std::shared_ptr<Data> data_;
+
+  State updateState(const std::shared_ptr<Data> data);
+  State getCurrentState() { return operation_mode_transition_manager_->getCurrentState(); }
+
+  EngageAcceptableParam engage_acceptable_param_;
+  StableCheckParam stable_check_param_;
+
+  bool hasDangerAcceleration();
+  std::pair<bool, bool> hasDangerLateralAcceleration();
+  bool checkEngageAvailable();
+
+  void publishData();
+
+  // update information
+  void onTimer();
+
+  void onOperationModeRequest(
+    const OperationModeRequest::Request::SharedPtr request,
+    const OperationModeRequest::Response::SharedPtr response);
+
+  mutable OperationModeTransitionManagerDebug debug_info_;
+};
+
+}  // namespace operation_mode_transition_manager
+
+#endif  // OPERATION_MODE_TRANSITION_MANAGER__OPERATION_MODE_TRANSITION_MANAGER_HPP_

control/operation_mode_transition_manager/include/operation_mode_transition_manager/state.hpp

@@ -0,0 +1,121 @@
+// 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 OPERATION_MODE_TRANSITION_MANAGER__STATE_HPP_
+#define OPERATION_MODE_TRANSITION_MANAGER__STATE_HPP_
+
+#include <operation_mode_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>
+
+#include <memory>
+
+namespace operation_mode_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 std::shared_ptr<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_;
+  std::shared_ptr<Data> data_;
+  StableCheckParam stable_check_param_;
+
+  State defaultUpdateOnManual();
+  bool sendAutonomousModeRequest();
+  bool sendManualModeRequest();
+};
+
+class StopState : public EngageStateBase
+{
+public:
+  explicit StopState(rclcpp::Node * node) : EngageStateBase(State::STOP, node) {}
+  State update() override { return defaultUpdateOnManual(); }
+};
+
+class RemoteOperatorState : public EngageStateBase
+{
+public:
+  explicit RemoteOperatorState(rclcpp::Node * node) : EngageStateBase(State::REMOTE_OPERATOR, node)
+  {
+  }
+  State update() override { return defaultUpdateOnManual(); }
+};
+
+class ManualDirectState : public EngageStateBase
+{
+public:
+  explicit ManualDirectState(rclcpp::Node * node) : EngageStateBase(State::MANUAL_DIRECT, node) {}
+  State update() override { return defaultUpdateOnManual(); }
+};
+
+class LocalOperatorState : public EngageStateBase
+{
+public:
+  explicit LocalOperatorState(rclcpp::Node * node) : EngageStateBase(State::LOCAL_OPERATOR, node) {}
+  State update() override { return defaultUpdateOnManual(); }
+};
+
+class TransitionToAutoState : public EngageStateBase
+{
+public:
+  explicit TransitionToAutoState(rclcpp::Node * node)
+  : EngageStateBase(State::TRANSITION_TO_AUTO, node)
+  {
+    transition_requested_time_ = clock_->now();
+  };
+  State update() override;
+
+private:
+  std::shared_ptr<rclcpp::Time> stable_start_time_;
+  bool checkSystemStable();
+
+  // return true when MANUAL mode is detected after AUTO transition is done.
+  bool checkVehicleOverride();
+
+  bool checkTransitionTimeout() const;
+
+  bool is_vehicle_mode_change_done_ = false;  // set to true when the mode changed to Auto.
+  bool is_control_mode_request_send_ = false;
+  rclcpp::Time transition_requested_time_;
+};
+
+class AutonomousState : public EngageStateBase
+{
+public:
+  explicit AutonomousState(rclcpp::Node * node) : EngageStateBase(State::AUTONOMOUS, node) {}
+  State update() override;
+};
+
+}  // namespace operation_mode_transition_manager
+
+#endif  // OPERATION_MODE_TRANSITION_MANAGER__STATE_HPP_