Merge pull request #708 from tier4/sync-upstream

chore: sync upstream

common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp

@@ -166,6 +166,10 @@ QGroupBox * AutowareStatePanel::makeLocalizationGroup()
   localization_label_ptr_->setStyleSheet("border:1px solid black;");
   grid->addWidget(localization_label_ptr_, 0, 0);
 
+  init_by_gnss_button_ptr_ = new QPushButton("Init by GNSS");
+  connect(init_by_gnss_button_ptr_, SIGNAL(clicked()), SLOT(onClickInitByGnss()));
+  grid->addWidget(init_by_gnss_button_ptr_, 1, 0);
+
   group->setLayout(grid);
   return group;
 }
@@ -249,6 +253,9 @@ void AutowareStatePanel::onInitialize()
     "/api/localization/initialization_state", rclcpp::QoS{1}.transient_local(),
     std::bind(&AutowareStatePanel::onLocalization, this, _1));
 
+  client_init_by_gnss_ =
+    raw_node_->create_client<InitializeLocalization>("/api/localization/initialize");
+
   // Motion
   sub_motion_ = raw_node_->create_subscription<MotionState>(
     "/api/motion/state", rclcpp::QoS{1}.transient_local(),
@@ -587,6 +594,11 @@ void AutowareStatePanel::onClickClearRoute()
   callServiceWithoutResponse<ClearRoute>(client_clear_route_);
 }
 
+void AutowareStatePanel::onClickInitByGnss()
+{
+  callServiceWithoutResponse<InitializeLocalization>(client_init_by_gnss_);
+}
+
 void AutowareStatePanel::onClickAcceptStart()
 {
   callServiceWithoutResponse<AcceptStart>(client_accept_start_);

common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp

@@ -34,6 +34,7 @@
 #include <autoware_adapi_v1_msgs/srv/accept_start.hpp>
 #include <autoware_adapi_v1_msgs/srv/change_operation_mode.hpp>
 #include <autoware_adapi_v1_msgs/srv/clear_route.hpp>
+#include <autoware_adapi_v1_msgs/srv/initialize_localization.hpp>
 #include <autoware_auto_vehicle_msgs/msg/gear_report.hpp>
 #include <tier4_external_api_msgs/msg/emergency.hpp>
 #include <tier4_external_api_msgs/srv/set_emergency.hpp>
@@ -51,6 +52,7 @@ class AutowareStatePanel : public rviz_common::Panel
   using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute;
   using LocalizationInitializationState =
     autoware_adapi_v1_msgs::msg::LocalizationInitializationState;
+  using InitializeLocalization = autoware_adapi_v1_msgs::srv::InitializeLocalization;
   using MotionState = autoware_adapi_v1_msgs::msg::MotionState;
   using AcceptStart = autoware_adapi_v1_msgs::srv::AcceptStart;
   using MRMState = autoware_adapi_v1_msgs::msg::MrmState;
@@ -69,6 +71,7 @@ public Q_SLOTS:  // NOLINT for Qt
   void onClickAutowareControl();
   void onClickDirectControl();
   void onClickClearRoute();
+  void onClickInitByGnss();
   void onClickAcceptStart();
   void onClickVelocityLimit();
   void onClickEmergencyButton();
@@ -130,7 +133,10 @@ public Q_SLOTS:  // NOLINT for Qt
 
   // Localization
   QLabel * localization_label_ptr_{nullptr};
+  QPushButton * init_by_gnss_button_ptr_{nullptr};
+
   rclcpp::Subscription<LocalizationInitializationState>::SharedPtr sub_localization_;
+  rclcpp::Client<InitializeLocalization>::SharedPtr client_init_by_gnss_;
 
   void onLocalization(const LocalizationInitializationState::ConstSharedPtr msg);
 

planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp

@@ -105,9 +105,9 @@ class PlannerManager
    */
   void registerSceneModuleManager(const SceneModuleManagerPtr & manager_ptr)
   {
-    RCLCPP_INFO(logger_, "register %s module", manager_ptr->getModuleName().c_str());
+    RCLCPP_INFO(logger_, "register %s module", manager_ptr->name().c_str());
     manager_ptrs_.push_back(manager_ptr);
-    processing_time_.emplace(manager_ptr->getModuleName(), 0.0);
+    processing_time_.emplace(manager_ptr->name(), 0.0);
   }
 
   /**
@@ -301,8 +301,9 @@ class PlannerManager
    */
   void deleteExpiredModules(SceneModulePtr & module_ptr) const
   {
-    const auto manager = getManager(module_ptr);
-    manager->deleteModules(module_ptr);
+    module_ptr->onExit();
+    module_ptr->publishRTCStatus();
+    module_ptr.reset();
   }
 
   /**
@@ -348,41 +349,21 @@ class PlannerManager
     candidate_module_ptrs_.clear();
   }
 
-  /**
-   * @brief stop and remove not RUNNING modules in candidate_module_ptrs_.
-   */
-  void clearNotRunningCandidateModules()
-  {
-    const auto it = std::remove_if(
-      candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [this](auto & m) {
-        if (m->getCurrentStatus() != ModuleStatus::RUNNING) {
-          deleteExpiredModules(m);
-          return true;
-        }
-        return false;
-      });
-    candidate_module_ptrs_.erase(it, candidate_module_ptrs_.end());
-  }
-
-  /**
-   * @brief check if there is any RUNNING module in candidate_module_ptrs_.
-   */
-  bool hasAnyRunningCandidateModule()
-  {
-    return std::any_of(candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [](auto & m) {
-      return m->getCurrentStatus() == ModuleStatus::RUNNING;
-    });
-  }
-
   /**
    * @brief get current root lanelet. the lanelet is used for reference path generation.
    * @param planner data.
    * @return root lanelet.
    */
-  lanelet::ConstLanelet updateRootLanelet(const std::shared_ptr<PlannerData> & data) const
+  lanelet::ConstLanelet updateRootLanelet(
+    const std::shared_ptr<PlannerData> & data, bool success_lane_change = false) const
   {
     lanelet::ConstLanelet ret{};
-    data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret);
+    if (success_lane_change) {
+      data->route_handler->getClosestPreferredLaneletWithinRoute(
+        data->self_odometry->pose.pose, &ret);
+    } else {
+      data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret);
+    }
     RCLCPP_DEBUG(logger_, "update start lanelet. id:%ld", ret.id());
     return ret;
   }
@@ -396,7 +377,7 @@ class PlannerManager
   {
     const auto itr = std::find_if(
       manager_ptrs_.begin(), manager_ptrs_.end(),
-      [&module_ptr](const auto & m) { return m->getModuleName() == module_ptr->name(); });
+      [&module_ptr](const auto & m) { return m->name() == module_ptr->name(); });
 
     if (itr == manager_ptrs_.end()) {
       throw std::domain_error("unknown manager name.");

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp

@@ -35,9 +35,9 @@ class AvoidanceModuleManager : public SceneModuleManagerInterface
   AvoidanceModuleManager(
     rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config);
 
-  std::shared_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
+  std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
   {
-    return std::make_shared<AvoidanceModule>(name_, *node_, parameters_, rtc_interface_ptr_map_);
+    return std::make_unique<AvoidanceModule>(name_, *node_, parameters_, rtc_interface_ptr_map_);
   }
 
   void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp

@@ -34,9 +34,9 @@ class DynamicAvoidanceModuleManager : public SceneModuleManagerInterface
   DynamicAvoidanceModuleManager(
     rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config);
 
-  std::shared_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
+  std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
   {
-    return std::make_shared<DynamicAvoidanceModule>(
+    return std::make_unique<DynamicAvoidanceModule>(
       name_, *node_, parameters_, rtc_interface_ptr_map_);
   }
 

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp

@@ -33,9 +33,9 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface
   GoalPlannerModuleManager(
     rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config);
 
-  std::shared_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
+  std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
   {
-    return std::make_shared<GoalPlannerModule>(name_, *node_, parameters_, rtc_interface_ptr_map_);
+    return std::make_unique<GoalPlannerModule>(name_, *node_, parameters_, rtc_interface_ptr_map_);
   }
 
   void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp

@@ -37,7 +37,7 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface
     rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config,
     const Direction direction, const LaneChangeModuleType type);
 
-  std::shared_ptr<SceneModuleInterface> createNewSceneModuleInstance() override;
+  std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override;
 
   void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;
 
@@ -55,7 +55,7 @@ class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager
   AvoidanceByLaneChangeModuleManager(
     rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config);
 
-  std::shared_ptr<SceneModuleInterface> createNewSceneModuleInstance() override;
+  std::unique_ptr<SceneModuleInterface> createNewSceneModuleInstance() override;
 
   // void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;
 

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp

@@ -104,6 +104,10 @@ class NormalLaneChange : public LaneChangeBase
 
   int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override;
 
+  std::vector<double> sampleLongitudinalAccValues(
+    const lanelet::ConstLanelets & current_lanes,
+    const lanelet::ConstLanelets & target_lanes) const;
+
   double calcPrepareDuration(
     const lanelet::ConstLanelets & current_lanes,
     const lanelet::ConstLanelets & target_lanes) const;