diff --git a/common/component_interface_specs/include/component_interface_specs/system.hpp b/common/component_interface_specs/include/component_interface_specs/system.hpp index 270bfa0eab326..2c82632d237f3 100644 --- a/common/component_interface_specs/include/component_interface_specs/system.hpp +++ b/common/component_interface_specs/include/component_interface_specs/system.hpp @@ -17,8 +17,7 @@ #include -#include -#include +#include #include #include @@ -39,22 +38,13 @@ struct ChangeOperationMode struct OperationModeState { - using Message = tier4_system_msgs::msg::OperationModeState; + using Message = autoware_ad_api_msgs::msg::OperationModeState; static constexpr char name[] = "/system/operation_mode/state"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; }; -struct AutonomousModeAvailable -{ - using Message = tier4_system_msgs::msg::OperationModeAvailable; - static constexpr char name[] = "/system/operation_mode/autonomous_available"; - static constexpr size_t depth = 1; - static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; -}; - } // namespace system_interface #endif // COMPONENT_INTERFACE_SPECS__SYSTEM_HPP_ diff --git a/control/operation_mode_transition_manager/src/data.cpp b/control/operation_mode_transition_manager/src/data.cpp index 97817227efbf4..295fcc6a25242 100644 --- a/control/operation_mode_transition_manager/src/data.cpp +++ b/control/operation_mode_transition_manager/src/data.cpp @@ -52,23 +52,23 @@ std::optional toEnum(const ChangeOperationMode::Request & request return std::nullopt; } -OperationModeStateValue toMsg(const OperationMode mode) +OperationModeValue toMsg(const OperationMode mode) { const auto get_operation_mode = [](const OperationMode mode) { switch (mode) { case OperationMode::STOP: - return OperationModeStateValue::STOP; + return OperationModeValue::STOP; case OperationMode::AUTONOMOUS: - return OperationModeStateValue::AUTONOMOUS; + return OperationModeValue::AUTONOMOUS; case OperationMode::LOCAL: - return OperationModeStateValue::LOCAL; + return OperationModeValue::LOCAL; case OperationMode::REMOTE: - return OperationModeStateValue::REMOTE; + return OperationModeValue::REMOTE; } - return OperationModeStateValue::UNKNOWN; + return OperationModeValue::UNKNOWN; }; - OperationModeStateValue operation; + OperationModeValue operation; operation.mode = get_operation_mode(mode); return operation; } diff --git a/control/operation_mode_transition_manager/src/data.hpp b/control/operation_mode_transition_manager/src/data.hpp index d9115ab744389..fed178ff789a2 100644 --- a/control/operation_mode_transition_manager/src/data.hpp +++ b/control/operation_mode_transition_manager/src/data.hpp @@ -17,9 +17,9 @@ #include +#include #include #include -#include #include #include @@ -31,8 +31,8 @@ namespace operation_mode_transition_manager { using ServiceResponse = autoware_ad_api_msgs::srv::ChangeOperationMode::Response; -using OperationModeState = tier4_system_msgs::msg::OperationModeState; -using OperationModeStateValue = OperationModeState::_operation_type; +using OperationModeState = autoware_ad_api_msgs::msg::OperationModeState; +using OperationModeValue = OperationModeState::_operation_type; using ChangeOperationMode = tier4_system_msgs::srv::ChangeOperationMode; using ControlModeRequest = tier4_vehicle_msgs::srv::ControlModeRequest; using ControlModeReport = autoware_auto_vehicle_msgs::msg::ControlModeReport; @@ -79,7 +79,7 @@ struct StableCheckParam std::string toString(const std::optional mode); std::optional toEnum(const ChangeOperationMode::Request & request); -OperationModeStateValue toMsg(const OperationMode mode); +OperationModeValue toMsg(const OperationMode mode); } // namespace operation_mode_transition_manager diff --git a/control/operation_mode_transition_manager/src/node.cpp b/control/operation_mode_transition_manager/src/node.cpp index df52701a46304..6ab09cf1ad3b3 100644 --- a/control/operation_mode_transition_manager/src/node.cpp +++ b/control/operation_mode_transition_manager/src/node.cpp @@ -40,7 +40,6 @@ OperationModeTransitionManager::OperationModeTransitionManager(const rclcpp::Nod srv_autoware_control, this, &OperationModeTransitionManager::onChangeAutowareControl); node.init_srv(srv_operation_mode, this, &OperationModeTransitionManager::onChangeOperationMode); node.init_pub(pub_operation_mode_); - node.init_pub(pub_auto_mode_available_); } // timer @@ -54,7 +53,7 @@ OperationModeTransitionManager::OperationModeTransitionManager(const rclcpp::Nod transition_timeout_ = declare_parameter("transition_timeout"); current_mode_ = OperationMode::STOP; transition_ = nullptr; - gate_operation_mode_.operation.mode = OperationModeStateValue::UNKNOWN; + gate_operation_mode_.operation.mode = OperationModeValue::UNKNOWN; gate_operation_mode_.is_in_transition = false; control_mode_report_.mode = ControlModeReport::NO_COMMAND; @@ -206,23 +205,22 @@ void OperationModeTransitionManager::onTimer() void OperationModeTransitionManager::publishData() { + const bool current_control = control_mode_report_.mode == ControlModeReport::AUTONOMOUS; const auto time = now(); - OperationModeStateAPI::Message operation_mode_state; - operation_mode_state.operation = toMsg(current_mode_); - operation_mode_state.is_in_transition = transition_ ? true : false; - if (prev_pub_state_ != operation_mode_state) { - prev_pub_state_ = operation_mode_state; - operation_mode_state.stamp = time; - pub_operation_mode_->publish(operation_mode_state); - } - - AutoModeAvailableAPI::Message available; - available.available = modes_.at(OperationMode::AUTONOMOUS)->isModeChangeAvailable(); - if (prev_pub_available_ != available) { - prev_pub_available_ = available; - available.stamp = time; - pub_auto_mode_available_->publish(available); + OperationModeStateAPI::Message state; + state.operation = toMsg(current_mode_); + state.is_autoware_control_enabled = current_control; + state.is_in_transition = transition_ ? true : false; + state.change_to_stop = modes_.at(OperationMode::STOP)->isModeChangeAvailable(); + state.change_to_autonomous = modes_.at(OperationMode::AUTONOMOUS)->isModeChangeAvailable(); + state.change_to_local = modes_.at(OperationMode::LOCAL)->isModeChangeAvailable(); + state.change_to_remote = modes_.at(OperationMode::REMOTE)->isModeChangeAvailable(); + + if (prev_state_ != state) { + prev_state_ = state; + state.stamp = time; + pub_operation_mode_->publish(state); } ModeChangeBase::DebugInfo debug_info = modes_.at(OperationMode::AUTONOMOUS)->getDebugInfo(); diff --git a/control/operation_mode_transition_manager/src/node.hpp b/control/operation_mode_transition_manager/src/node.hpp index 15d671e2a2949..ffe10c8caa562 100644 --- a/control/operation_mode_transition_manager/src/node.hpp +++ b/control/operation_mode_transition_manager/src/node.hpp @@ -37,11 +37,9 @@ class OperationModeTransitionManager : public rclcpp::Node using ChangeAutowareControlAPI = system_interface::ChangeAutowareControl; using ChangeOperationModeAPI = system_interface::ChangeOperationMode; using OperationModeStateAPI = system_interface::OperationModeState; - using AutoModeAvailableAPI = system_interface::AutonomousModeAvailable; component_interface_utils::Service::SharedPtr srv_autoware_control; component_interface_utils::Service::SharedPtr srv_operation_mode; component_interface_utils::Publisher::SharedPtr pub_operation_mode_; - component_interface_utils::Publisher::SharedPtr pub_auto_mode_available_; void onChangeAutowareControl( const ChangeAutowareControlAPI::Service::Request::SharedPtr request, const ChangeAutowareControlAPI::Service::Response::SharedPtr response); @@ -70,8 +68,7 @@ class OperationModeTransitionManager : public rclcpp::Node OperationModeState gate_operation_mode_; ControlModeReport control_mode_report_; - std::optional prev_pub_state_; - std::optional prev_pub_available_; + std::optional prev_state_; }; } // namespace operation_mode_transition_manager diff --git a/control/vehicle_cmd_gate/README.md b/control/vehicle_cmd_gate/README.md index c0947465ac000..fc4692d585d08 100644 --- a/control/vehicle_cmd_gate/README.md +++ b/control/vehicle_cmd_gate/README.md @@ -26,7 +26,7 @@ | `~/input/emergency/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from emergency handler | | `~/input/emergency/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command from emergency handler | | `~/input/engage` | `autoware_auto_vehicle_msgs::msg::Engage` | engage signal | -| `~/input/operation_mode` | `tier4_system_msgs::msg::OperationModeState` | operation mode of Autoware | +| `~/input/operation_mode` | `autoware_ad_api_msgs::msg::OperationModeState` | operation mode of Autoware | ### Output diff --git a/control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_gate.hpp index 63f5cd67dcf82..bc8d44bb93ead 100644 --- a/control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/include/vehicle_cmd_gate/vehicle_cmd_gate.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -36,7 +37,6 @@ #include #include #include -#include #include #include @@ -44,6 +44,7 @@ namespace vehicle_cmd_gate { +using autoware_ad_api_msgs::msg::OperationModeState; using autoware_auto_control_msgs::msg::AckermannControlCommand; using autoware_auto_system_msgs::msg::EmergencyState; using autoware_auto_vehicle_msgs::msg::GearCommand; @@ -54,7 +55,6 @@ using tier4_control_msgs::msg::GateMode; using tier4_external_api_msgs::msg::Emergency; using tier4_external_api_msgs::msg::Heartbeat; using tier4_external_api_msgs::srv::SetEmergency; -using tier4_system_msgs::msg::OperationModeState; using tier4_vehicle_msgs::msg::VehicleEmergencyStamped; using diagnostic_msgs::msg::DiagnosticStatus;