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)