feat(autoware_default_adapi): release adapi v1.6.0 (#9704)

* feat: reject clearing route during autonomous mode Signed-off-by: Takagi, Isamu <[email protected]> * feat: modify check and relay door service Signed-off-by: Takagi, Isamu <[email protected]> * fix door condition Signed-off-by: Takagi, Isamu <[email protected]> * fix error and add option Signed-off-by: Takagi, Isamu <[email protected]> * update v1.6.0 Signed-off-by: Takagi, Isamu <[email protected]> --------- Signed-off-by: Takagi, Isamu <[email protected]>
autowarefoundation · Dec 25, 2024 · e0531b8 · e0531b8
1 parent d2a9531
commit e0531b8
Show file tree

Hide file tree

Showing 9 changed files with 90 additions and 20 deletions.
diff --git a/common/autoware_adapi_specs/include/autoware/adapi_specs/vehicle.hpp b/common/autoware_adapi_specs/include/autoware/adapi_specs/vehicle.hpp
@@ -41,7 +41,7 @@ struct VehicleStatus
   using Message = autoware_adapi_v1_msgs::msg::VehicleStatus;
   static constexpr char name[] = "/api/vehicle/status";
   static constexpr size_t depth = 1;
-  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
   static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
 };
 

diff --git a/system/autoware_default_adapi/config/default_adapi.param.yaml b/system/autoware_default_adapi/config/default_adapi.param.yaml
@@ -6,3 +6,7 @@
   ros__parameters:
     require_accept_start: false
     stop_check_duration: 1.0
+
+/adapi/node/vehicle_door:
+  ros__parameters:
+    check_autoware_control: true
diff --git a/system/autoware_default_adapi/launch/test_default_adapi.launch.xml b/system/autoware_default_adapi/launch/test_default_adapi.launch.xml
@@ -0,0 +1,5 @@
+<launch>
+  <arg name="vehicle_model" default="sample_vehicle"/>
+  <include file="$(find-pkg-share autoware_global_parameter_loader)/launch/global_params.launch.py"/>
+  <include file="$(find-pkg-share autoware_default_adapi)/launch/default_adapi.launch.py"/>
+</launch>
diff --git a/system/autoware_default_adapi/src/interface.cpp b/system/autoware_default_adapi/src/interface.cpp
@@ -21,7 +21,7 @@ InterfaceNode::InterfaceNode(const rclcpp::NodeOptions & options) : Node("interf
 {
   const auto on_interface_version = [](auto, auto res) {
     res->major = 1;
-    res->minor = 5;
+    res->minor = 6;
     res->patch = 0;
   };
 

diff --git a/system/autoware_default_adapi/src/routing.cpp b/system/autoware_default_adapi/src/routing.cpp
@@ -68,6 +68,7 @@ RoutingNode::RoutingNode(const rclcpp::NodeOptions & options) : Node("routing",
   adaptor.init_cli(cli_operation_mode_, group_cli_);
   adaptor.init_sub(sub_operation_mode_, this, &RoutingNode::on_operation_mode);
 
+  is_autoware_control_ = false;
   is_auto_mode_ = false;
   state_.state = State::Message::UNKNOWN;
 }
@@ -85,6 +86,7 @@ void RoutingNode::change_stop_mode()
 
 void RoutingNode::on_operation_mode(const OperationModeState::Message::ConstSharedPtr msg)
 {
+  is_autoware_control_ = msg->is_autoware_control_enabled;
   is_auto_mode_ = msg->mode == OperationModeState::Message::AUTONOMOUS;
 }
 
@@ -119,7 +121,14 @@ void RoutingNode::on_clear_route(
   const autoware::adapi_specs::routing::ClearRoute::Service::Request::SharedPtr req,
   const autoware::adapi_specs::routing::ClearRoute::Service::Response::SharedPtr res)
 {
-  change_stop_mode();
+  // For safety, do not clear the route while it is in use.
+  // https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/routing/clear_route/
+  if (is_auto_mode_ && is_autoware_control_) {
+    res->status.success = false;
+    res->status.code = ResponseStatus::UNKNOWN;
+    res->status.message = "The route cannot be cleared while it is in use.";
+    return;
+  }
   res->status = conversion::convert_call(cli_clear_route_, req);
 }
 

diff --git a/system/autoware_default_adapi/src/routing.hpp b/system/autoware_default_adapi/src/routing.hpp
@@ -52,6 +52,7 @@ class RoutingNode : public rclcpp::Node
   Cli<autoware::component_interface_specs::planning::ClearRoute> cli_clear_route_;
   Cli<autoware::component_interface_specs::system::ChangeOperationMode> cli_operation_mode_;
   Sub<autoware::component_interface_specs::system::OperationModeState> sub_operation_mode_;
+  bool is_autoware_control_;
   bool is_auto_mode_;
   State::Message state_;
 

diff --git a/system/autoware_default_adapi/src/vehicle_door.cpp b/system/autoware_default_adapi/src/vehicle_door.cpp
@@ -24,18 +24,50 @@ VehicleDoorNode::VehicleDoorNode(const rclcpp::NodeOptions & options)
 {
   const auto adaptor = autoware::component_interface_utils::NodeAdaptor(this);
   group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-  adaptor.relay_service(cli_command_, srv_command_, group_cli_, 3.0);
-  adaptor.relay_service(cli_layout_, srv_layout_, group_cli_, 3.0);
+  adaptor.relay_service(cli_layout_, srv_layout_, group_cli_);
+  adaptor.init_cli(cli_command_, group_cli_);
+  adaptor.init_srv(srv_command_, this, &VehicleDoorNode::on_command);
   adaptor.init_pub(pub_status_);
   adaptor.init_sub(sub_status_, this, &VehicleDoorNode::on_status);
+  adaptor.init_sub(sub_operation_mode_, this, &VehicleDoorNode::on_operation_mode);
+
+  check_autoware_control_ = declare_parameter<bool>("check_autoware_control");
+  is_autoware_control_ = false;
+  is_stop_mode_ = false;
+}
+
+void VehicleDoorNode::on_operation_mode(const OperationModeState::Message::ConstSharedPtr msg)
+{
+  is_autoware_control_ = msg->is_autoware_control_enabled;
+  is_stop_mode_ = msg->mode == OperationModeState::Message::STOP;
+}
+void VehicleDoorNode::on_status(InternalDoorStatus::Message::ConstSharedPtr msg)
+{
+  utils::notify(pub_status_, status_, *msg, utils::ignore_stamp<InternalDoorStatus::Message>);
 }
 
-void VehicleDoorNode::on_status(
-  autoware::component_interface_specs::vehicle::DoorStatus::Message::ConstSharedPtr msg)
+void VehicleDoorNode::on_command(
+  const ExternalDoorCommand::Service::Request::SharedPtr req,
+  const ExternalDoorCommand::Service::Response::SharedPtr res)
 {
-  utils::notify(
-    pub_status_, status_, *msg,
-    utils::ignore_stamp<autoware::component_interface_specs::vehicle::DoorStatus::Message>);
+  // For safety, do not open the door if the vehicle is not stopped.
+  // https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/vehicle/doors/command/
+  if (!is_stop_mode_ || (!is_autoware_control_ && check_autoware_control_)) {
+    bool is_open = false;
+    for (const auto & door : req->doors) {
+      if (door.command == autoware_adapi_v1_msgs::msg::DoorCommand::OPEN) {
+        is_open = true;
+        break;
+      }
+    }
+    if (is_open) {
+      res->status.success = false;
+      res->status.code = autoware_adapi_v1_msgs::msg::ResponseStatus::UNKNOWN;
+      res->status.message = "Doors cannot be opened if the vehicle is not stopped.";
+      return;
+    }
+  }
+  autoware::component_interface_utils::status::copy(cli_command_->call(req), res);
 }
 
 }  // namespace autoware::default_adapi

diff --git a/system/autoware_default_adapi/src/vehicle_door.hpp b/system/autoware_default_adapi/src/vehicle_door.hpp
@@ -16,7 +16,9 @@
 #define VEHICLE_DOOR_HPP_
 
 #include <autoware/adapi_specs/vehicle.hpp>
+#include <autoware/component_interface_specs/system.hpp>
 #include <autoware/component_interface_specs/vehicle.hpp>
+#include <autoware/component_interface_utils/status.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <optional>
@@ -33,16 +35,33 @@ class VehicleDoorNode : public rclcpp::Node
   explicit VehicleDoorNode(const rclcpp::NodeOptions & options);
 
 private:
-  void on_status(
-    autoware::component_interface_specs::vehicle::DoorStatus::Message::ConstSharedPtr msg);
+  using OperationModeState = autoware::component_interface_specs::system::OperationModeState;
+  using InternalDoorStatus = autoware::component_interface_specs::vehicle::DoorStatus;
+  using InternalDoorLayout = autoware::component_interface_specs::vehicle::DoorLayout;
+  using InternalDoorCommand = autoware::component_interface_specs::vehicle::DoorCommand;
+  using ExternalDoorStatus = autoware::adapi_specs::vehicle::DoorStatus;
+  using ExternalDoorLayout = autoware::adapi_specs::vehicle::DoorLayout;
+  using ExternalDoorCommand = autoware::adapi_specs::vehicle::DoorCommand;
+
+  void on_operation_mode(const OperationModeState::Message::ConstSharedPtr msg);
+  void on_status(InternalDoorStatus::Message::ConstSharedPtr msg);
+  void on_command(
+    const ExternalDoorCommand::Service::Request::SharedPtr req,
+    const ExternalDoorCommand::Service::Response::SharedPtr res);
+
   rclcpp::CallbackGroup::SharedPtr group_cli_;
-  Srv<autoware::adapi_specs::vehicle::DoorCommand> srv_command_;
-  Srv<autoware::adapi_specs::vehicle::DoorLayout> srv_layout_;
-  Pub<autoware::adapi_specs::vehicle::DoorStatus> pub_status_;
-  Cli<autoware::component_interface_specs::vehicle::DoorCommand> cli_command_;
-  Cli<autoware::component_interface_specs::vehicle::DoorLayout> cli_layout_;
-  Sub<autoware::component_interface_specs::vehicle::DoorStatus> sub_status_;
-  std::optional<autoware::component_interface_specs::vehicle::DoorStatus::Message> status_;
+  Srv<ExternalDoorCommand> srv_command_;
+  Srv<ExternalDoorLayout> srv_layout_;
+  Pub<ExternalDoorStatus> pub_status_;
+  Cli<InternalDoorCommand> cli_command_;
+  Cli<InternalDoorLayout> cli_layout_;
+  Sub<InternalDoorStatus> sub_status_;
+  std::optional<InternalDoorStatus::Message> status_;
+
+  bool check_autoware_control_;
+  bool is_autoware_control_;
+  bool is_stop_mode_;
+  Sub<OperationModeState> sub_operation_mode_;
 };
 
 }  // namespace autoware::default_adapi

diff --git a/system/autoware_default_adapi/test/node/interface_version.py b/system/autoware_default_adapi/test/node/interface_version.py
@@ -30,7 +30,7 @@
 
 if response.major != 1:
     exit(1)
-if response.minor != 5:
+if response.minor != 6:
     exit(1)
 if response.patch != 0:
     exit(1)