feat(vehicle_cmd_gate): add moderate_stop_interface to make vehicle s…

…top (autowarefoundation#3790) * feat(vehicle_cmd_gate): add moderate_stop_interface to make vehicle stop Signed-off-by: Berkay Karaman <[email protected]> Signed-off-by: Berkay Karaman <[email protected]> * move filter place Signed-off-by: Berkay Karaman <[email protected]> --------- Signed-off-by: Berkay Karaman <[email protected]>
Interplai · May 29, 2023 · 4856894 · 4856894
1 parent db5aba3
commit 4856894
Show file tree

Hide file tree

Showing 8 changed files with 162 additions and 0 deletions.
diff --git a/common/component_interface_specs/include/component_interface_specs/control.hpp b/common/component_interface_specs/include/component_interface_specs/control.hpp
@@ -19,7 +19,9 @@
 
 #include <tier4_control_msgs/msg/is_paused.hpp>
 #include <tier4_control_msgs/msg/is_start_requested.hpp>
+#include <tier4_control_msgs/msg/is_stopped.hpp>
 #include <tier4_control_msgs/srv/set_pause.hpp>
+#include <tier4_control_msgs/srv/set_stop.hpp>
 
 namespace control_interface
 {
@@ -48,6 +50,21 @@ struct IsStartRequested
   static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
 };
 
+struct SetStop
+{
+  using Service = tier4_control_msgs::srv::SetStop;
+  static constexpr char name[] = "/control/vehicle_cmd_gate/set_stop";
+};
+
+struct IsStopped
+{
+  using Message = tier4_control_msgs::msg::IsStopped;
+  static constexpr char name[] = "/control/vehicle_cmd_gate/is_stopped";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
 }  // namespace control_interface
 
 #endif  // COMPONENT_INTERFACE_SPECS__CONTROL_HPP_
diff --git a/control/vehicle_cmd_gate/CMakeLists.txt b/control/vehicle_cmd_gate/CMakeLists.txt
@@ -8,6 +8,7 @@ ament_auto_add_library(vehicle_cmd_gate_node SHARED
   src/vehicle_cmd_gate.cpp
   src/vehicle_cmd_filter.cpp
   src/pause_interface.cpp
+  src/moderate_stop_interface.cpp
 )
 
 rclcpp_components_register_node(vehicle_cmd_gate_node

diff --git a/control/vehicle_cmd_gate/README.md b/control/vehicle_cmd_gate/README.md
@@ -52,6 +52,7 @@
 | `external_emergency_stop_heartbeat_timeout` | double | timeout for external emergency                                              |
 | `stop_hold_acceleration`                    | double | longitudinal acceleration cmd when vehicle should stop                      |
 | `emergency_acceleration`                    | double | longitudinal acceleration cmd when vehicle stop with emergency              |
+| `moderate_stop_service_acceleration`        | double | longitudinal acceleration cmd when vehicle stop with moderate stop service  |
 | `nominal.vel_lim`                           | double | limit of longitudinal velocity (activated in AUTONOMOUS operation mode)     |
 | `nominal.lon_acc_lim`                       | double | limit of longitudinal acceleration (activated in AUTONOMOUS operation mode) |
 | `nominal.lon_jerk_lim`                      | double | limit of longitudinal jerk (activated in AUTONOMOUS operation mode)         |

diff --git a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml
@@ -8,6 +8,7 @@
     external_emergency_stop_heartbeat_timeout: 0.0
     stop_hold_acceleration: -1.5
     emergency_acceleration: -2.4
+    moderate_stop_service_acceleration: -1.5
     stopped_state_entry_duration_time: 0.1
     nominal:
       vel_lim: 25.0

diff --git a/control/vehicle_cmd_gate/src/moderate_stop_interface.cpp b/control/vehicle_cmd_gate/src/moderate_stop_interface.cpp
@@ -0,0 +1,69 @@
+// Copyright 2023 LeoDrive, A.Ş.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "moderate_stop_interface.hpp"
+
+namespace vehicle_cmd_gate
+{
+
+ModerateStopInterface::ModerateStopInterface(rclcpp::Node * node) : node_(node)
+{
+  const auto adaptor = component_interface_utils::NodeAdaptor(node);
+  adaptor.init_srv(srv_set_stop_, this, &ModerateStopInterface::on_stop_request);
+  adaptor.init_pub(pub_is_stopped_);
+
+  stop_state_.data = false;
+  stop_state_.requested_sources.clear();
+  publish();
+}
+
+bool ModerateStopInterface::is_stop_requested() const
+{
+  return stop_state_.data;
+}
+
+void ModerateStopInterface::publish()
+{
+  if (prev_stop_map_ != stop_map_) {
+    pub_is_stopped_->publish(stop_state_);
+    prev_stop_map_ = stop_map_;
+  }
+}
+
+void ModerateStopInterface::on_stop_request(
+  const SetStop::Service::Request::SharedPtr req, const SetStop::Service::Response::SharedPtr res)
+{
+  stop_map_[req->request_source] = req->stop;
+  update_stop_state();
+  res->status.success = true;
+}
+
+void ModerateStopInterface::update_stop_state()
+{
+  stop_state_.stamp = node_->now();
+  stop_state_.requested_sources.clear();
+  stop_state_.data = false;
+
+  if (stop_map_.empty()) {
+    return;
+  }
+  for (auto & itr : stop_map_) {
+    if (itr.second) {
+      stop_state_.data = true;
+      stop_state_.requested_sources.push_back(itr.first);
+    }
+  }
+}
+
+}  // namespace vehicle_cmd_gate
diff --git a/control/vehicle_cmd_gate/src/moderate_stop_interface.hpp b/control/vehicle_cmd_gate/src/moderate_stop_interface.hpp
@@ -0,0 +1,60 @@
+// Copyright 2023 LeoDrive, A.Ş.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MODERATE_STOP_INTERFACE_HPP_
+#define MODERATE_STOP_INTERFACE_HPP_
+
+#include <component_interface_specs/control.hpp>
+#include <component_interface_utils/rclcpp.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
+
+#include <string>
+#include <unordered_map>
+
+namespace vehicle_cmd_gate
+{
+
+class ModerateStopInterface
+{
+private:
+  using SetStop = control_interface::SetStop;
+  using IsStopped = control_interface::IsStopped;
+  using IsStartRequested = control_interface::IsStartRequested;
+
+public:
+  explicit ModerateStopInterface(rclcpp::Node * node);
+  bool is_stop_requested() const;
+  void publish();
+
+private:
+  IsStopped::Message stop_state_;
+  std::unordered_map<std::string, bool> stop_map_;
+  std::optional<std::unordered_map<std::string, bool>> prev_stop_map_;
+
+  rclcpp::Node * node_;
+  component_interface_utils::Service<SetStop>::SharedPtr srv_set_stop_;
+  component_interface_utils::Publisher<IsStopped>::SharedPtr pub_is_stopped_;
+
+  void on_stop_request(
+    const SetStop::Service::Request::SharedPtr req,
+    const SetStop::Service::Response::SharedPtr res);
+
+  void update_stop_state();
+};
+
+}  // namespace vehicle_cmd_gate
+
+#endif  // MODERATE_STOP_INTERFACE_HPP_
diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
@@ -143,6 +143,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
     declare_parameter<double>("external_emergency_stop_heartbeat_timeout");
   stop_hold_acceleration_ = declare_parameter<double>("stop_hold_acceleration");
   emergency_acceleration_ = declare_parameter<double>("emergency_acceleration");
+  moderate_stop_service_acceleration_ =
+    declare_parameter<double>("moderate_stop_service_acceleration");
 
   // Vehicle Parameter
   const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
@@ -196,6 +198,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
 
   // Pause interface
   pause_ = std::make_unique<PauseInterface>(this);
+  moderate_stop_interface_ = std::make_unique<ModerateStopInterface>(this);
 
   // Timer
   const auto update_period = 1.0 / declare_parameter<double>("update_rate");
@@ -378,6 +381,11 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands)
     filtered_commands.gear = commands.gear;  // tmp
   }
 
+  if (moderate_stop_interface_->is_stop_requested()) {  // if stop requested, stop the vehicle
+    filtered_commands.control.longitudinal.speed = 0.0;
+    filtered_commands.control.longitudinal.acceleration = moderate_stop_service_acceleration_;
+  }
+
   // Check emergency
   if (use_emergency_handling_ && is_system_emergency_) {
     RCLCPP_WARN_THROTTLE(
@@ -410,6 +418,7 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands)
   vehicle_cmd_emergency_pub_->publish(vehicle_cmd_emergency);
   control_cmd_pub_->publish(filtered_commands.control);
   pause_->publish();
+  moderate_stop_interface_->publish();
 
   // Save ControlCmd to steering angle when disengaged
   prev_control_cmd_ = filtered_commands.control;
@@ -474,6 +483,7 @@ void VehicleCmdGate::publishStatus()
   pub_external_emergency_->publish(external_emergency);
   operation_mode_pub_->publish(current_operation_mode_);
   pause_->publish();
+  moderate_stop_interface_->publish();
 }
 
 AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannControlCommand & in)

diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
@@ -15,6 +15,7 @@
 #ifndef VEHICLE_CMD_GATE_HPP_
 #define VEHICLE_CMD_GATE_HPP_
 
+#include "moderate_stop_interface.hpp"
 #include "pause_interface.hpp"
 #include "vehicle_cmd_filter.hpp"
 
@@ -160,6 +161,7 @@ class VehicleCmdGate : public rclcpp::Node
   double external_emergency_stop_heartbeat_timeout_;
   double stop_hold_acceleration_;
   double emergency_acceleration_;
+  double moderate_stop_service_acceleration_;
 
   // Service
   rclcpp::Service<EngageSrv>::SharedPtr srv_engage_;
@@ -216,6 +218,7 @@ class VehicleCmdGate : public rclcpp::Node
 
   // Pause interface for API
   std::unique_ptr<PauseInterface> pause_;
+  std::unique_ptr<ModerateStopInterface> moderate_stop_interface_;
 };
 
 }  // namespace vehicle_cmd_gate