From d73a09a26317ebbe86f0a40662f90e6e850623ac Mon Sep 17 00:00:00 2001 From: SHtokuda <165623782+shtokuda@users.noreply.github.com> Date: Fri, 9 Aug 2024 19:38:06 +0900 Subject: [PATCH 1/2] feat(autoware_vehicle_cmd_gate): check the timestamp of input topics to avoid using old topics (#8084) * add prev_commands_ and check cmd's time stamp Signed-off-by: shtokuda * add timestamp when is_engaged is false Signed-off-by: shtokuda * style(pre-commit): autofix * add initialization for hazard_light timestamp in Commands Signed-off-by: shtokuda * style(pre-commit): autofix * update README.md Signed-off-by: shtokuda * style(pre-commit): autofix * fix typo Signed-off-by: shtokuda * fix(autoware_vehicle_cmd_gate): rename the function that checks the continuity of topics Signed-off-by: shtokuda Co-authored-by: Takamasa Horibe * style(pre-commit): autofix * feat(autoware_vehicle_cmd_gate): check continuity using shared_ptr Signed-off-by: shtokuda * feat(autoware_vehicle_cmd_gate): add INFO message for topics that are not receiving Signed-off-by: shtokuda * fix template function to pass build-and-test-differential Signed-off-by: shtokuda * fix(autoware_vehicle_cmd_gate): add #include according to pre-commit.ci Signed-off-by: shtokuda * fix(vehicle_cmd_gate) add underscores to member variable names for consistency Signed-off-by: shtokuda * style(pre-commit): autofix --------- Signed-off-by: shtokuda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Takamasa Horibe Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> --- control/autoware_vehicle_cmd_gate/README.md | 7 +++ .../src/vehicle_cmd_gate.cpp | 59 +++++++++++++++++-- .../src/vehicle_cmd_gate.hpp | 10 ++++ 3 files changed, 72 insertions(+), 4 deletions(-) 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..a02ab010e8cea 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,21 @@ 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 { + 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 +466,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 +478,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..3f74be72be9dc 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,10 @@ 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; From 60b733af0d11f4a9e883e5ec43964b02dfb738bb Mon Sep 17 00:00:00 2001 From: SHtokuda <165623782+shtokuda@users.noreply.github.com> Date: Fri, 16 Aug 2024 09:56:02 +0900 Subject: [PATCH 2/2] feat(autoware_vehicle_cmd_gate): accept same topic unless mode change occurs (#8479) * add prev_commands_ and check cmd's time stamp Signed-off-by: shtokuda * add timestamp when is_engaged is false Signed-off-by: shtokuda * style(pre-commit): autofix * add initialization for hazard_light timestamp in Commands Signed-off-by: shtokuda * style(pre-commit): autofix * update README.md Signed-off-by: shtokuda * style(pre-commit): autofix * fix typo Signed-off-by: shtokuda * fix(autoware_vehicle_cmd_gate): rename the function that checks the continuity of topics Signed-off-by: shtokuda Co-authored-by: Takamasa Horibe * style(pre-commit): autofix * feat(autoware_vehicle_cmd_gate): check continuity using shared_ptr Signed-off-by: shtokuda * feat(autoware_vehicle_cmd_gate): add INFO message for topics that are not receiving Signed-off-by: shtokuda * fix template function to pass build-and-test-differential Signed-off-by: shtokuda * fix(autoware_vehicle_cmd_gate): add #include according to pre-commit.ci Signed-off-by: shtokuda * fix(vehicle_cmd_gate) add underscores to member variable names for consistency Signed-off-by: shtokuda * style(pre-commit): autofix * feat(vehicle_cmd_gate): accept same topic unless mode change occurs Signed-off-by: shtokuda * feat(vehicle_cmd_gate): add default topic_name to getContinuousTopic function Signed-off-by: shtokuda --------- Signed-off-by: shtokuda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Takamasa Horibe Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> --- .../autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp | 10 ++++++---- .../autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp | 3 ++- 2 files changed, 8 insertions(+), 5 deletions(-) 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 a02ab010e8cea..b26a2630ed994 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -361,12 +361,14 @@ 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) { + if ((rclcpp::Time(current_topic.stamp) - rclcpp::Time(prev_topic->stamp)).seconds() >= 0.0) { return current_topic; } else { - RCLCPP_INFO( - get_logger(), - "The operation mode is changed, but the %s is not received yet:", topic_name.c_str()); + 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; } } 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 3f74be72be9dc..279bc6036bd34 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -240,7 +240,8 @@ class VehicleCmdGate : public rclcpp::Node template T getContinuousTopic( - const std::shared_ptr & prev_topic, const T & current_topic, const std::string & topic_name); + const std::shared_ptr & prev_topic, const T & current_topic, + const std::string & topic_name = ""); // Algorithm Control prev_control_cmd_;