diff --git a/control/autoware_vehicle_cmd_gate/README.md b/control/autoware_vehicle_cmd_gate/README.md index e46db3c06cfeb..336c1acbe9bae 100644 --- a/control/autoware_vehicle_cmd_gate/README.md +++ b/control/autoware_vehicle_cmd_gate/README.md @@ -84,6 +84,13 @@ This functionality for starting/stopping was embedded in the source code but was ## Assumptions / Known limits +### External Emergency Heartbeat + The parameter `check_external_emergency_heartbeat` (true by default) enables an emergency stop request from external modules. This feature requires a `~/input/external_emergency_stop_heartbeat` topic for health monitoring of the external module, and the vehicle_cmd_gate module will not start without the topic. The `check_external_emergency_heartbeat` parameter must be false when the "external emergency stop" function is not used. + +### Commands on Mode changes + +Output commands' topics: `turn_indicators_cmd`, `hazard_light` and `gear_cmd` are selected based on `gate_mode`. +However, to ensure the continuity of commands, these commands will not change until the topics of new input commands arrive, even if a mode change occurs. diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index dd6e2f0f54aea..b26a2630ed994 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -53,6 +53,10 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) using std::placeholders::_2; using std::placeholders::_3; + prev_turn_indicator_ = nullptr; + prev_hazard_light_ = nullptr; + prev_gear_ = nullptr; + rclcpp::QoS durable_qos{1}; durable_qos.transient_local(); @@ -352,6 +356,23 @@ void VehicleCmdGate::onEmergencyCtrlCmd(Control::ConstSharedPtr msg) } } +// check the continuity of topics +template +T VehicleCmdGate::getContinuousTopic( + const std::shared_ptr & prev_topic, const T & current_topic, const std::string & topic_name) +{ + if ((rclcpp::Time(current_topic.stamp) - rclcpp::Time(prev_topic->stamp)).seconds() >= 0.0) { + return current_topic; + } else { + if (topic_name != "") { + RCLCPP_INFO( + get_logger(), + "The operation mode is changed, but the %s is not received yet:", topic_name.c_str()); + } + return *prev_topic; + } +} + void VehicleCmdGate::onTimer() { // Subscriber for auto @@ -447,6 +468,8 @@ void VehicleCmdGate::onTimer() if (!is_engaged_) { turn_indicator.command = TurnIndicatorsCommand::NO_COMMAND; hazard_light.command = HazardLightsCommand::NO_COMMAND; + turn_indicator.stamp = this->now(); + hazard_light.stamp = this->now(); } } else if (current_gate_mode_.data == GateMode::EXTERNAL) { turn_indicator = remote_commands_.turn_indicator; @@ -457,10 +480,40 @@ void VehicleCmdGate::onTimer() } } - // Publish topics - turn_indicator_cmd_pub_->publish(turn_indicator); - hazard_light_cmd_pub_->publish(hazard_light); - gear_cmd_pub_->publish(gear); + // Publish Turn Indicators, Hazard Lights and Gear Command + if (prev_turn_indicator_ != nullptr) { + *prev_turn_indicator_ = + getContinuousTopic(prev_turn_indicator_, turn_indicator, "TurnIndicatorsCommand"); + turn_indicator_cmd_pub_->publish(*prev_turn_indicator_); + } else { + if (msg_auto_command_turn_indicator || msg_remote_command_turn_indicator) { + prev_turn_indicator_ = std::make_shared(turn_indicator); + } + turn_indicator_cmd_pub_->publish(turn_indicator); + } + + if (prev_hazard_light_ != nullptr) { + *prev_hazard_light_ = + getContinuousTopic(prev_hazard_light_, hazard_light, "HazardLightsCommand"); + hazard_light_cmd_pub_->publish(*prev_hazard_light_); + } else { + if ( + msg_auto_command_hazard_light || msg_remote_command_hazard_light || + msg_emergency_command_hazard_light) { + prev_hazard_light_ = std::make_shared(hazard_light); + } + hazard_light_cmd_pub_->publish(hazard_light); + } + + if (prev_gear_ != nullptr) { + *prev_gear_ = getContinuousTopic(prev_gear_, gear, "GearCommand"); + gear_cmd_pub_->publish(*prev_gear_); + } else { + if (msg_auto_command_gear || msg_remote_command_gear || msg_emergency_command_gear) { + prev_gear_ = std::make_shared(gear); + } + gear_cmd_pub_->publish(gear); + } } void VehicleCmdGate::publishControlCommands(const Commands & commands) diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 42e28d633d16b..279bc6036bd34 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -50,6 +50,7 @@ #include #include +#include #include namespace autoware::vehicle_cmd_gate @@ -183,6 +184,11 @@ class VehicleCmdGate : public rclcpp::Node this, "input/emergency/gear_cmd"}; void onEmergencyCtrlCmd(Control::ConstSharedPtr msg); + // Previous Turn Indicators, Hazard Lights and Gear + TurnIndicatorsCommand::SharedPtr prev_turn_indicator_; + HazardLightsCommand::SharedPtr prev_hazard_light_; + GearCommand::SharedPtr prev_gear_; + // Parameter bool use_emergency_handling_; bool check_external_emergency_heartbeat_; @@ -232,6 +238,11 @@ class VehicleCmdGate : public rclcpp::Node void checkExternalEmergencyStop(diagnostic_updater::DiagnosticStatusWrapper & stat); + template + T getContinuousTopic( + const std::shared_ptr & prev_topic, const T & current_topic, + const std::string & topic_name = ""); + // Algorithm Control prev_control_cmd_; Control createStopControlCmd() const;