Merge branch 'beta/v0.19.1' into feat/crosswalk-keep-stopping-v0.19.1.1

Author: saka1-s

common/tier4_planning_rviz_plugin/include/path/display_base.hpp

@@ -315,6 +315,9 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>
         node->detachAllObjects();
         node->removeAndDestroyAllChildren();
         this->scene_manager_->destroySceneNode(node);
+
+        rviz_rendering::MovableText * text = velocity_texts_.at(i);
+        delete text;
       }
       velocity_texts_.resize(msg_ptr->points.size());
       velocity_text_nodes_.resize(msg_ptr->points.size());
@@ -338,6 +341,9 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>
         node->detachAllObjects();
         node->removeAndDestroyAllChildren();
         this->scene_manager_->destroySceneNode(node);
+
+        rviz_rendering::MovableText * text = slope_texts_.at(i);
+        delete text;
       }
       slope_texts_.resize(msg_ptr->points.size());
       slope_text_nodes_.resize(msg_ptr->points.size());

system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp

@@ -40,6 +40,7 @@
 
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <nav_msgs/msg/odometry.hpp>
+#include <std_srvs/srv/trigger.hpp>
 
 struct HazardLampPolicy
 {
@@ -105,6 +106,9 @@ class MrmHandler : public rclcpp::Node
     const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg);
   void onOperationModeState(
     const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg);
+  void onRecoverMrm(
+    const std_srvs::srv::Trigger::Request::SharedPtr,
+    const std_srvs::srv::Trigger::Response::SharedPtr response);
 
   // Publisher
 
@@ -135,6 +139,9 @@ class MrmHandler : public rclcpp::Node
   rclcpp::CallbackGroup::SharedPtr client_mrm_emergency_stop_group_;
   rclcpp::Client<tier4_system_msgs::srv::OperateMrm>::SharedPtr client_mrm_emergency_stop_;
 
+  // Services
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr service_recover_mrm_;
+
   bool requestMrmBehavior(
     const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior,
     RequestType request_type) const;

system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp

@@ -86,6 +86,12 @@ MrmHandler::MrmHandler() : Node("mrm_handler")
     "~/output/mrm/emergency_stop/operate", rmw_qos_profile_services_default,
     client_mrm_emergency_stop_group_);
 
+  // Services
+  service_recover_mrm_ = create_service<std_srvs::srv::Trigger>(
+    "/system/clear_mrm",
+    std::bind(&MrmHandler::onRecoverMrm, this, std::placeholders::_1, std::placeholders::_2),
+    rmw_qos_profile_services_default);
+
   // Initialize
   odom_ = std::make_shared<const nav_msgs::msg::Odometry>();
   control_mode_ = std::make_shared<const autoware_auto_vehicle_msgs::msg::ControlModeReport>();
@@ -619,3 +625,15 @@ bool MrmHandler::isArrivedAtGoal()
 
   return operation_mode_state_->mode == OperationModeState::STOP;
 }
+
+void MrmHandler::onRecoverMrm(
+  const std_srvs::srv::Trigger::Request::SharedPtr,
+  const std_srvs::srv::Trigger::Response::SharedPtr response)
+{
+  if (!param_.is_mrm_recoverable) {
+    is_mrm_holding_ = false;
+    response->success = true;
+  } else {
+    response->success = false;
+  }
+}