feat(vehicle_cmd_gate): add an option for using raw external commands (…

…autowarefoundation#4325) * clean_commit Signed-off-by: ismetatabay <ismet@leodrive.ai> * move enable_cmd_limit_filter argument to param file Signed-off-by: ismetatabay <ismet@leodrive.ai> --------- Signed-off-by: ismetatabay <ismet@leodrive.ai>
kminoda · Aug 23, 2023 · c0b6e10 · c0b6e10
1 parent f7cafb1
commit c0b6e10
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 3 deletions.
diff --git a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml
@@ -5,6 +5,7 @@
     use_emergency_handling: false
     check_external_emergency_heartbeat: false
     use_start_request: false
+    enable_cmd_limit_filter: true
     external_emergency_stop_heartbeat_timeout: 0.0
     stop_hold_acceleration: -1.5
     emergency_acceleration: -2.4

diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
@@ -150,6 +150,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
   moderate_stop_service_acceleration_ =
     declare_parameter<double>("moderate_stop_service_acceleration");
   stop_check_duration_ = declare_parameter<double>("stop_check_duration");
+  enable_cmd_limit_filter_ = declare_parameter<bool>("enable_cmd_limit_filter");
 
   // Vehicle Parameter
   const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
@@ -411,9 +412,11 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands)
     filtered_commands.control.longitudinal.acceleration = stop_hold_acceleration_;
   }
 
-  // Apply limit filtering
-  filtered_commands.control = filterControlCommand(filtered_commands.control);
-
+  // Check if command filtering option is enable
+  if (enable_cmd_limit_filter_) {
+    // Apply limit filtering
+    filtered_commands.control = filterControlCommand(filtered_commands.control);
+  }
   // tmp: Publish vehicle emergency status
   VehicleEmergencyStamped vehicle_cmd_emergency;
   vehicle_cmd_emergency.emergency = (use_emergency_handling_ && is_system_emergency_);

diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
@@ -164,6 +164,7 @@ class VehicleCmdGate : public rclcpp::Node
   double stop_hold_acceleration_;
   double emergency_acceleration_;
   double moderate_stop_service_acceleration_;
+  bool enable_cmd_limit_filter_;
 
   // Service
   rclcpp::Service<EngageSrv>::SharedPtr srv_engage_;