Merge branch 'autowarefoundation:main' into feat/add_perception_objects_pointcloud_publisher

Author: lexavtanke

control/trajectory_follower_node/package.xml

@@ -42,6 +42,7 @@
   <test_depend>autoware_lint_common</test_depend>
   <test_depend>autoware_testing</test_depend>
   <test_depend>fake_test_node</test_depend>
+  <test_depend>ros_testing</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml

@@ -23,7 +23,7 @@
   <!-- RTC controller -->
   <group>
     <push-ros-namespace namespace="autoware_api/external/rtc_controller"/>
-    <node_container pkg="rclcpp_components" exec="component_container_mt" name="container" namespace="">
+    <node_container pkg="rclcpp_components" exec="component_container_mt" name="container" namespace="" ros_args="--log-level autoware_api.external.rtc_controller.container:=warn">
       <composable_node pkg="autoware_iv_external_api_adaptor" plugin="external_api::RTCController" name="node"/>
     </node_container>
   </group>

launch/tier4_planning_launch/launch/planning.launch.xml

@@ -16,6 +16,8 @@
   <arg name="nearest_search_param_path"/>
   <!-- rtc -->
   <arg name="rtc_auto_mode_manager_param_path"/>
+  <!-- mission planner -->
+  <arg name="mission_planner_param_path"/>
   <!-- behavior path planner -->
   <arg name="side_shift_param_path"/>
   <arg name="avoidance_param_path"/>
@@ -58,7 +60,9 @@
     <!-- mission planning module -->
     <group>
       <push-ros-namespace namespace="mission_planning"/>
-      <include file="$(find-pkg-share tier4_planning_launch)/launch/mission_planning/mission_planning.launch.xml"/>
+      <include file="$(find-pkg-share tier4_planning_launch)/launch/mission_planning/mission_planning.launch.xml">
+        <arg name="mission_planner_param_path" value="$(var mission_planner_param_path)"/>
+      </include>
     </group>
 
     <!-- scenario planning module -->

planning/behavior_path_planner/CMakeLists.txt

@@ -15,6 +15,8 @@ set(common_src
   src/steering_factor_interface.cpp
   src/behavior_path_planner_node.cpp
   src/scene_module/scene_module_visitor.cpp
+  src/scene_module/pull_out/pull_out_module.cpp
+  src/scene_module/side_shift/side_shift_module.cpp
   src/turn_signal_decider.cpp
   src/util/avoidance/util.cpp
   src/util/lane_change/util.cpp
@@ -43,13 +45,11 @@ if(COMPILE_WITH_OLD_ARCHITECTURE)
   ament_auto_add_library(behavior_path_planner_node SHARED
     src/behavior_tree_manager.cpp
     src/scene_module/scene_module_bt_node_interface.cpp
-    src/scene_module/side_shift/side_shift_module.cpp
     src/scene_module/avoidance/avoidance_module.cpp
     src/scene_module/lane_following/lane_following_module.cpp
     src/scene_module/lane_change/external_request_lane_change_module.cpp
     src/scene_module/lane_change/lane_change_module.cpp
     src/scene_module/pull_over/pull_over_module.cpp
-    src/scene_module/pull_out/pull_out_module.cpp
     ${common_src}
   )
 
@@ -60,6 +60,8 @@ if(COMPILE_WITH_OLD_ARCHITECTURE)
 else()
   ament_auto_add_library(behavior_path_planner_node SHARED
     src/planner_manager.cpp
+    src/scene_module/pull_out/manager.cpp
+    src/scene_module/side_shift/manager.cpp
     ${common_src}
   )
 

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -29,6 +29,8 @@
 #include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp"
 #else
 #include "behavior_path_planner/planner_manager.hpp"
+#include "behavior_path_planner/scene_module/pull_out/manager.hpp"
+#include "behavior_path_planner/scene_module/side_shift/manager.hpp"
 #endif
 
 #include "behavior_path_planner/steering_factor_interface.hpp"
@@ -95,6 +97,7 @@ using rcl_interfaces::msg::SetParametersResult;
 using steering_factor_interface::SteeringFactorInterface;
 using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
 using tier4_planning_msgs::msg::LaneChangeDebugMsgArray;
+using tier4_planning_msgs::msg::LateralOffset;
 using tier4_planning_msgs::msg::Scenario;
 using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
@@ -113,6 +116,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   rclcpp::Subscription<PredictedObjects>::SharedPtr perception_subscriber_;
   rclcpp::Subscription<OccupancyGrid>::SharedPtr occupancy_grid_subscriber_;
   rclcpp::Subscription<OccupancyGrid>::SharedPtr costmap_subscriber_;
+  rclcpp::Subscription<LateralOffset>::SharedPtr lateral_offset_subscriber_;
   rclcpp::Subscription<OperationModeState>::SharedPtr operation_mode_subscriber_;
   rclcpp::Publisher<PathWithLaneId>::SharedPtr path_publisher_;
   rclcpp::Publisher<TurnIndicatorsCommand>::SharedPtr turn_signal_publisher_;
@@ -153,8 +157,10 @@ class BehaviorPathPlannerNode : public rclcpp::Node
 
   // parameters
   std::shared_ptr<AvoidanceParameters> avoidance_param_ptr_;
+  std::shared_ptr<SideShiftParameters> side_shift_param_ptr_;
   std::shared_ptr<LaneChangeParameters> lane_change_param_ptr_;
   std::shared_ptr<LaneFollowingParameters> lane_following_param_ptr_;
+  std::shared_ptr<PullOutParameters> pull_out_param_ptr_;
 
   BehaviorPathPlannerParameters getCommonParam();
 
@@ -178,6 +184,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   void onMap(const HADMapBin::ConstSharedPtr map_msg);
   void onRoute(const LaneletRoute::ConstSharedPtr route_msg);
   void onOperationMode(const OperationModeState::ConstSharedPtr msg);
+  void onLateralOffset(const LateralOffset::ConstSharedPtr msg);
   SetParametersResult onSetParam(const std::vector<rclcpp::Parameter> & parameters);
 
   /**

planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp

@@ -31,6 +31,7 @@
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <nav_msgs/msg/odometry.hpp>
+#include <tier4_planning_msgs/msg/lateral_offset.hpp>
 
 #include <lanelet2_core/geometry/Lanelet.h>
 
@@ -52,6 +53,7 @@ using geometry_msgs::msg::PoseStamped;
 using nav_msgs::msg::OccupancyGrid;
 using nav_msgs::msg::Odometry;
 using route_handler::RouteHandler;
+using tier4_planning_msgs::msg::LateralOffset;
 using PlanResult = PathWithLaneId::SharedPtr;
 
 struct BoolStamped
@@ -124,6 +126,7 @@ struct PlannerData
   PredictedObjects::ConstSharedPtr dynamic_object{};
   OccupancyGrid::ConstSharedPtr occupancy_grid{};
   OccupancyGrid::ConstSharedPtr costmap{};
+  LateralOffset::ConstSharedPtr lateral_offset{};
   OperationModeState::ConstSharedPtr operation_mode{};
   PathWithLaneId::SharedPtr reference_path{std::make_shared<PathWithLaneId>()};
   PathWithLaneId::SharedPtr prev_output_path{std::make_shared<PathWithLaneId>()};

planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp

@@ -30,6 +30,8 @@ struct BehaviorPathPlannerParameters
   bool verbose;
 
   ModuleConfigParameters config_avoidance;
+  ModuleConfigParameters config_pull_out;
+  ModuleConfigParameters config_side_shift;
 
   double backward_path_length;
   double forward_path_length;

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

@@ -0,0 +1,54 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// 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 BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__MANAGER_HPP_
+#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__MANAGER_HPP_
+
+#include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp"
+#include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace behavior_path_planner
+{
+
+class PullOutModuleManager : public SceneModuleManagerInterface
+{
+public:
+  PullOutModuleManager(
+    rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config,
+    const std::shared_ptr<PullOutParameters> & parameters);
+
+  std::shared_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
+  {
+    return std::make_shared<PullOutModule>(name_, *node_, parameters_, rtc_interface_);
+  }
+
+  void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;
+
+private:
+  std::shared_ptr<PullOutParameters> parameters_;
+
+  std::shared_ptr<RTCInterface> rtc_interface_;
+
+  std::vector<std::shared_ptr<PullOutModule>> registered_modules_;
+};
+
+}  // namespace behavior_path_planner
+
+#endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__MANAGER_HPP_

planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp

@@ -63,25 +63,38 @@ struct PullOutStatus
 class PullOutModule : public SceneModuleInterface
 {
 public:
+#ifdef USE_OLD_ARCHITECTURE
   PullOutModule(
-    const std::string & name, rclcpp::Node & node, const PullOutParameters & parameters);
+    const std::string & name, rclcpp::Node & node,
+    const std::shared_ptr<PullOutParameters> & parameters);
+#else
+  PullOutModule(
+    const std::string & name, rclcpp::Node & node,
+    const std::shared_ptr<PullOutParameters> & parameters,
+    const std::shared_ptr<RTCInterface> & rtc_interface);
+
+  void updateModuleParams(const std::shared_ptr<PullOutParameters> & parameters)
+  {
+    parameters_ = parameters;
+  }
+#endif
 
   BehaviorModuleOutput run() override;
 
   bool isExecutionRequested() const override;
   bool isExecutionReady() const override;
-  BT::NodeStatus updateState() override;
-  BT::NodeStatus getNodeStatusWhileWaitingApproval() const override
-  {
-    return BT::NodeStatus::SUCCESS;
-  }
+  ModuleStatus updateState() override;
+  ModuleStatus getNodeStatusWhileWaitingApproval() const override { return ModuleStatus::SUCCESS; }
   BehaviorModuleOutput plan() override;
   BehaviorModuleOutput planWaitingApproval() override;
   CandidateOutput planCandidate() const override;
   void onEntry() override;
   void onExit() override;
 
-  void setParameters(const PullOutParameters & parameters) { parameters_ = parameters; }
+  void setParameters(const std::shared_ptr<PullOutParameters> & parameters)
+  {
+    parameters_ = parameters;
+  }
   void resetStatus();
 
   void acceptVisitor(
@@ -90,7 +103,7 @@ class PullOutModule : public SceneModuleInterface
   }
 
 private:
-  PullOutParameters parameters_;
+  std::shared_ptr<PullOutParameters> parameters_;
   vehicle_info_util::VehicleInfo vehicle_info_;
 
   std::vector<std::shared_ptr<PullOutPlannerBase>> pull_out_planners_;

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

@@ -0,0 +1,53 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// 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 BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__MANAGER_HPP_
+#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__MANAGER_HPP_
+
+#include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp"
+#include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace behavior_path_planner
+{
+
+class SideShiftModuleManager : public SceneModuleManagerInterface
+{
+public:
+  SideShiftModuleManager(
+    rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config,
+    const std::shared_ptr<SideShiftParameters> & parameters);
+
+  std::shared_ptr<SceneModuleInterface> createNewSceneModuleInstance() override
+  {
+    return std::make_shared<SideShiftModule>(name_, *node_, parameters_);
+  }
+
+  void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;
+
+private:
+  std::shared_ptr<SideShiftParameters> parameters_;
+
+  std::vector<std::shared_ptr<SideShiftModule>> registered_modules_;
+};
+
+}  // namespace behavior_path_planner
+
+#endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__MANAGER_HPP_

planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp

@@ -40,21 +40,27 @@ class SideShiftModule : public SceneModuleInterface
 {
 public:
   SideShiftModule(
-    const std::string & name, rclcpp::Node & node, const SideShiftParameters & parameters);
+    const std::string & name, rclcpp::Node & node,
+    const std::shared_ptr<SideShiftParameters> & parameters);
 
   bool isExecutionRequested() const override;
   bool isExecutionReady() const override;
   bool isReadyForNextRequest(
     const double & min_request_time_sec, bool override_requests = false) const noexcept;
-  BT::NodeStatus updateState() override;
+  ModuleStatus updateState() override;
   void updateData() override;
   BehaviorModuleOutput plan() override;
   BehaviorModuleOutput planWaitingApproval() override;
   CandidateOutput planCandidate() const override;
   void onEntry() override;
   void onExit() override;
 
-  void setParameters(const SideShiftParameters & parameters);
+  void setParameters(const std::shared_ptr<SideShiftParameters> & parameters);
+
+  void updateModuleParams(const std::shared_ptr<SideShiftParameters> & parameters)
+  {
+    parameters_ = parameters;
+  }
 
   void acceptVisitor(
     [[maybe_unused]] const std::shared_ptr<SceneModuleVisitor> & visitor) const override
@@ -66,7 +72,9 @@ class SideShiftModule : public SceneModuleInterface
 
   void initVariables();
 
+#ifdef USE_OLD_ARCHITECTURE
   void onLateralOffset(const LateralOffset::ConstSharedPtr lateral_offset_msg);
+#endif
 
   // non-const methods
   void adjustDrivableArea(ShiftedPath * path) const;
@@ -82,9 +90,10 @@ class SideShiftModule : public SceneModuleInterface
 
   // member
   PathWithLaneId refined_path_{};
-  std::shared_ptr<PathWithLaneId> reference_path_{std::make_shared<PathWithLaneId>()};
+  PathWithLaneId reference_path_{};
+  PathWithLaneId prev_reference_{};
   lanelet::ConstLanelets current_lanelets_;
-  SideShiftParameters parameters_;
+  std::shared_ptr<SideShiftParameters> parameters_;
 
   // Requested lateral offset to shift the reference path.
   double requested_lateral_offset_{0.0};
@@ -112,6 +121,7 @@ class SideShiftModule : public SceneModuleInterface
   // NOTE: this function is ported from avoidance.
   Pose getUnshiftedEgoPose(const ShiftedPath & prev_path) const;
   inline Pose getEgoPose() const { return planner_data_->self_odometry->pose.pose; }
+  PathWithLaneId extendBackwardLength(const PathWithLaneId & original_path) const;
   PathWithLaneId calcCenterLinePath(
     const std::shared_ptr<const PlannerData> & planner_data, const Pose & pose) const;
 

planning/behavior_path_planner/package.xml

@@ -13,6 +13,7 @@
   <maintainer email="kosuke.takeuchi@tier4.jp">Kosuke Takeuchi</maintainer>
   <!-- side shift module -->
   <maintainer email="fumiya.watanabe@tier4.jp">Fumiya Watanabe</maintainer>
+  <maintainer email="kyoichi.sugahara@tier4.jp">Kyoichi Sugahara</maintainer>
   <!-- avoidance module -->
   <maintainer email="takamasa.horibe@tier4.jp">Takamasa Horibe</maintainer>
   <maintainer email="satoshi.ota@tier4.jp">Satoshi Ota</maintainer>