diff --git a/common/autoware_adapi_specs/include/autoware/adapi_specs/vehicle.hpp b/common/autoware_adapi_specs/include/autoware/adapi_specs/vehicle.hpp
index a7568d54b5e1a..dc907b6610cde 100644
--- 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
index ddbf103878953..f2782e313e785 100644
--- 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
new file mode 100644
index 0000000000000..3a00beeb623d4
--- /dev/null
+++ b/system/autoware_default_adapi/launch/test_default_adapi.launch.xml
@@ -0,0 +1,5 @@
+
+
+
+
+
diff --git a/system/autoware_default_adapi/src/interface.cpp b/system/autoware_default_adapi/src/interface.cpp
index fb924f06f42ea..6535dcfca30fd 100644
--- 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
index 4eaa98376b147..93f31ff693344 100644
--- 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
index 10eed606a5f6b..f37b8a978a235 100644
--- 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 cli_clear_route_;
Cli cli_operation_mode_;
Sub 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
index 23588bdf4597a..d3ad5d8ddccee 100644
--- 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("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);
}
-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);
+ // 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
index 50312a38a4cb7..a75232a2b2c74 100644
--- 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
+#include
#include
+#include
#include
#include
@@ -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 srv_command_;
- Srv srv_layout_;
- Pub pub_status_;
- Cli cli_command_;
- Cli cli_layout_;
- Sub sub_status_;
- std::optional status_;
+ Srv srv_command_;
+ Srv srv_layout_;
+ Pub pub_status_;
+ Cli cli_command_;
+ Cli cli_layout_;
+ Sub sub_status_;
+ std::optional status_;
+
+ bool check_autoware_control_;
+ bool is_autoware_control_;
+ bool is_stop_mode_;
+ Sub 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
index c91889149cb0e..8db70ca5cba9a 100644
--- 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)