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"),