diff --git a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml b/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml
index 644e937e0718d..48e7288150b4e 100644
--- a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml
+++ b/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml
@@ -26,6 +26,7 @@
+
diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
index e38299a6cfa6e..e4eadc1b69d8c 100644
--- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
+++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
@@ -61,7 +61,6 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
this->create_publisher("output/turn_indicators_cmd", durable_qos);
hazard_light_cmd_pub_ =
this->create_publisher("output/hazard_lights_cmd", durable_qos);
-
gate_mode_pub_ = this->create_publisher("output/gate_mode", durable_qos);
engage_pub_ = this->create_publisher("output/engage", durable_qos);
pub_external_emergency_ =
@@ -84,6 +83,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
[this](const OperationModeState::SharedPtr msg) { current_operation_mode_ = *msg; });
mrm_state_sub_ = this->create_subscription(
"input/mrm_state", 1, std::bind(&VehicleCmdGate::onMrmState, this, _1));
+ gear_status_sub_ = this->create_subscription(
+ "input/gear_status", 1, std::bind(&VehicleCmdGate::onGearStatus, this, _1));
// Subscriber for auto
auto_control_cmd_sub_ = this->create_subscription(
@@ -257,6 +258,8 @@ void VehicleCmdGate::onAutoHazardLightsCmd(HazardLightsCommand::ConstSharedPtr m
void VehicleCmdGate::onAutoShiftCmd(GearCommand::ConstSharedPtr msg) { auto_commands_.gear = *msg; }
+void VehicleCmdGate::onGearStatus(GearReport::ConstSharedPtr msg) { current_gear_ptr_ = msg; }
+
// for remote
void VehicleCmdGate::onRemoteCtrlCmd(AckermannControlCommand::ConstSharedPtr msg)
{
@@ -346,6 +349,15 @@ void VehicleCmdGate::onTimer()
return;
}
+ if (is_gate_mode_changed_) {
+ // If gate mode is external, is_engaged_ is always true
+ // While changing gate mode external to auto, the first is_engaged_ is always true for the first
+ // loop in this scope. So we need to wait for the second loop
+ // after gate mode is changed.
+ is_gate_mode_changed_ = false;
+ return;
+ }
+
// Select commands
TurnIndicatorsCommand turn_indicator;
HazardLightsCommand hazard_light;
@@ -362,6 +374,11 @@ void VehicleCmdGate::onTimer()
// Don't send turn signal when autoware is not engaged
if (!is_engaged_) {
+ if (!current_gear_ptr_) {
+ gear.command = GearCommand::NONE;
+ } else {
+ gear.command = current_gear_ptr_.get()->report;
+ }
turn_indicator.command = TurnIndicatorsCommand::NO_COMMAND;
hazard_light.command = HazardLightsCommand::NO_COMMAND;
}
@@ -564,7 +581,7 @@ void VehicleCmdGate::onGateMode(GateMode::ConstSharedPtr msg)
{
const auto prev_gate_mode = current_gate_mode_;
current_gate_mode_ = *msg;
-
+ is_gate_mode_changed_ = true;
if (current_gate_mode_.data != prev_gate_mode.data) {
RCLCPP_INFO(
get_logger(), "GateMode changed: %s -> %s", getGateModeName(prev_gate_mode.data),
diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
index 756e8e122b13f..6358a039bbbfb 100644
--- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
+++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
@@ -27,6 +27,7 @@
#include
#include
#include
+#include
#include
#include
#include
@@ -50,6 +51,7 @@ using autoware_adapi_v1_msgs::msg::MrmState;
using autoware_adapi_v1_msgs::msg::OperationModeState;
using autoware_auto_control_msgs::msg::AckermannControlCommand;
using autoware_auto_vehicle_msgs::msg::GearCommand;
+using autoware_auto_vehicle_msgs::msg::GearReport;
using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
using autoware_auto_vehicle_msgs::msg::SteeringReport;
using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
@@ -100,18 +102,22 @@ class VehicleCmdGate : public rclcpp::Node
rclcpp::Subscription::SharedPtr steer_sub_;
rclcpp::Subscription::SharedPtr operation_mode_sub_;
rclcpp::Subscription::SharedPtr mrm_state_sub_;
+ rclcpp::Subscription::SharedPtr gear_status_sub_;
void onGateMode(GateMode::ConstSharedPtr msg);
void onExternalEmergencyStopHeartbeat(Heartbeat::ConstSharedPtr msg);
void onSteering(SteeringReport::ConstSharedPtr msg);
void onMrmState(MrmState::ConstSharedPtr msg);
+ void onGearStatus(GearReport::ConstSharedPtr msg);
bool is_engaged_;
bool is_system_emergency_ = false;
bool is_external_emergency_stop_ = false;
+ bool is_gate_mode_changed_ = false;
double current_steer_ = 0;
GateMode current_gate_mode_;
MrmState current_mrm_state_;
+ GearReport::ConstSharedPtr current_gear_ptr_;
// Heartbeat
std::shared_ptr emergency_state_heartbeat_received_time_;
diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py
index d65614a39be7c..7abb41d8af294 100644
--- a/launch/tier4_control_launch/launch/control.launch.py
+++ b/launch/tier4_control_launch/launch/control.launch.py
@@ -147,6 +147,7 @@ def launch_setup(context, *args, **kwargs):
("input/emergency/hazard_lights_cmd", "/system/emergency/hazard_lights_cmd"),
("input/emergency/gear_cmd", "/system/emergency/gear_cmd"),
("input/mrm_state", "/system/fail_safe/mrm_state"),
+ ("input/gear_status", "/vehicle/status/gear_status"),
("output/vehicle_cmd_emergency", "/control/command/emergency_cmd"),
("output/control_cmd", "/control/command/control_cmd"),
("output/gear_cmd", "/control/command/gear_cmd"),