Merge pull request #660 from tier4/sync-upstream

chore: sync upstream
tier4 · Jul 12, 2023 · 3df8b10 · 3df8b10
2 parents 7382681 + 6bf539b
commit 3df8b10
Show file tree

Hide file tree

Showing 53 changed files with 817 additions and 667 deletions.
diff --git a/common/autoware_ad_api_specs/README.md b/common/autoware_ad_api_specs/README.md
@@ -0,0 +1,3 @@
+# autoware_adapi_specs
+
+This package is a specification of Autoware AD API.
diff --git a/common/autoware_auto_common/package.xml b/common/autoware_auto_common/package.xml
@@ -5,6 +5,9 @@
   <version>1.0.0</version>
   <description>Miscellaneous helper functions</description>
   <maintainer email="opensource@apex.ai">Apex.AI, Inc.</maintainer>
+  <maintainer email="tomoya.kimura@tier4.jp">Tomoya Kimura</maintainer>
+  <maintainer email="shumpei.wakabayashi@tier4.jp">Shumpei Wakabayashi</maintainer>
+  <maintainer email="satoshi.ota@tier4.jp">Satoshi Ota</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>

diff --git a/common/autoware_auto_tf2/package.xml b/common/autoware_auto_tf2/package.xml
@@ -5,6 +5,9 @@
   <version>1.0.0</version>
   <description>Transform related utilities for different msg types</description>
   <maintainer email="jit.ray.c@gmail.com">Jit Ray Chowdhury</maintainer>
+  <maintainer email="tomoya.kimura@tier4.jp">Tomoya Kimura</maintainer>
+  <maintainer email="shumpei.wakabayashi@tier4.jp">Shumpei Wakabayashi</maintainer>
+  <maintainer email="satoshi.ota@tier4.jp">Satoshi Ota</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>

diff --git a/common/autoware_testing/package.xml b/common/autoware_testing/package.xml
@@ -5,6 +5,9 @@
   <version>0.1.0</version>
   <description>Tools for handling standard tests based on ros_testing</description>
   <maintainer email="adam.dabrowski@robotec.ai">Adam Dabrowski</maintainer>
+  <maintainer email="tomoya.kimura@tier4.jp">Tomoya Kimura</maintainer>
+  <maintainer email="shumpei.wakabayashi@tier4.jp">Shumpei Wakabayashi</maintainer>
+  <maintainer email="satoshi.ota@tier4.jp">Satoshi Ota</maintainer>
   <license>Apache 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>

diff --git a/common/component_interface_specs/README.md b/common/component_interface_specs/README.md
@@ -0,0 +1,3 @@
+# component_interface_specs
+
+This package is a specification of component interfaces.
diff --git a/common/component_interface_tools/README.md b/common/component_interface_tools/README.md
@@ -0,0 +1,7 @@
+# component_interface_tools
+
+This package provides the following tools for component interface.
+
+## service_log_checker
+
+Monitor the service log of component_interface_utils and display if the response status is an error.
diff --git a/common/fake_test_node/package.xml b/common/fake_test_node/package.xml
@@ -5,6 +5,9 @@
   <version>1.0.0</version>
   <description>A fake node that we can use in the integration-like cpp tests.</description>
   <maintainer email="opensource@apex.ai">Apex.AI, Inc.</maintainer>
+  <maintainer email="tomoya.kimura@tier4.jp">Tomoya Kimura</maintainer>
+  <maintainer email="shumpei.wakabayashi@tier4.jp">Shumpei Wakabayashi</maintainer>
+  <maintainer email="satoshi.ota@tier4.jp">Satoshi Ota</maintainer>
   <license>Apache 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>

diff --git a/common/tier4_api_utils/README.md b/common/tier4_api_utils/README.md
@@ -0,0 +1,4 @@
+# tier4_api_utils
+
+This is an old implementation of a class that logs when calling a service.
+Please use [component_interface_utils](../component_interface_utils/README.md) instead.
diff --git a/common/time_utils/package.xml b/common/time_utils/package.xml
@@ -5,6 +5,8 @@
   <version>1.0.0</version>
   <description>Simple conversion methods to/from std::chrono to simplify algorithm development</description>
   <maintainer email="christopherj.ho@gmail.com">Christopher Ho</maintainer>
+  <maintainer email="tomoya.kimura@tier4.jp">Tomoya Kimura</maintainer>
+  <maintainer email="shumpei.wakabayashi@tier4.jp">Shumpei Wakabayashi</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>

diff --git a/control/operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug.msg b/control/operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug.msg
@@ -1,8 +1,9 @@
 builtin_interfaces/Time stamp
 
 # states
-string previous_state
-string current_state
+string status
+bool in_autoware_control
+bool in_transition
 
 # flags for engage permission
 bool is_all_ok

diff --git a/control/operation_mode_transition_manager/src/node.cpp b/control/operation_mode_transition_manager/src/node.cpp
@@ -270,10 +270,19 @@ void OperationModeTransitionManager::publishData()
     pub_operation_mode_->publish(state);
   }
 
+  const auto status_str = [&]() {
+    if (!current_control) return "DISENGAGE (autoware mode = " + toString(current_mode_) + ")";
+    if (transition_)
+      return toString(current_mode_) + " (in transition from " + toString(transition_->previous) +
+             ")";
+    return toString(current_mode_);
+  }();
+
   ModeChangeBase::DebugInfo debug_info = modes_.at(OperationMode::AUTONOMOUS)->getDebugInfo();
   debug_info.stamp = time;
-  debug_info.current_state = toString(current_mode_);
-  debug_info.previous_state = transition_ ? toString(transition_->previous) : "NONE";
+  debug_info.status = status_str;
+  debug_info.in_autoware_control = current_control;
+  debug_info.in_transition = transition_ ? true : false;
   pub_debug_info_->publish(debug_info);
 }
 

diff --git a/control/pure_pursuit/README.md b/control/pure_pursuit/README.md
@@ -0,0 +1,19 @@
+# Pure Pursuit Controller
+
+The Pure Pursuit Controller module calculates the steering angle for tracking a desired trajectory using the pure pursuit algorithm. This is used as a lateral controller plugin in the `trajectory_follower_node`.
+
+## Inputs
+
+Set the following from the [controller_node](../trajectory_follower_node/README.md)
+
+- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow.
+- `nav_msgs/Odometry`: current ego pose and velocity information
+
+## Outputs
+
+Return LateralOutput which contains the following to the controller node
+
+- `autoware_auto_control_msgs/AckermannLateralCommand`: target steering angle
+- LateralSyncData
+  - steer angle convergence
+- `autoware_auto_planning_msgs/Trajectory`: predicted path for ego vehicle
diff --git a/control/shift_decider/include/shift_decider/shift_decider.hpp b/control/shift_decider/include/shift_decider/shift_decider.hpp
@@ -50,6 +50,7 @@ class ShiftDecider : public rclcpp::Node
   autoware_auto_system_msgs::msg::AutowareState::SharedPtr autoware_state_;
   autoware_auto_vehicle_msgs::msg::GearCommand shift_cmd_;
   autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr current_gear_ptr_;
+  uint8_t prev_shift_command = autoware_auto_vehicle_msgs::msg::GearCommand::PARK;
 
   bool park_on_goal_;
 };

diff --git a/control/shift_decider/src/shift_decider.cpp b/control/shift_decider/src/shift_decider.cpp
@@ -83,7 +83,7 @@ void ShiftDecider::updateCurrentShiftCmd()
     } else if (control_cmd_->longitudinal.speed < -vel_threshold) {
       shift_cmd_.command = GearCommand::REVERSE;
     } else {
-      shift_cmd_.command = current_gear_ptr_->report;
+      shift_cmd_.command = prev_shift_command;
     }
   } else {
     if (
@@ -95,6 +95,7 @@ void ShiftDecider::updateCurrentShiftCmd()
       shift_cmd_.command = current_gear_ptr_->report;
     }
   }
+  prev_shift_command = shift_cmd_.command;
 }
 
 void ShiftDecider::initTimer(double period_s)

diff --git a/evaluator/kinematic_evaluator/package.xml b/evaluator/kinematic_evaluator/package.xml
@@ -5,6 +5,12 @@
   <description>kinematic evaluator ROS 2 node</description>
 
   <maintainer email="dominik.jargot@robotec.ai">Dominik Jargot</maintainer>
+  <maintainer email="tomoya.kimura@tier4.jp">Tomoya Kimura</maintainer>
+  <maintainer email="shumpei.wakabayashi@tier4.jp">Shumpei Wakabayashi</maintainer>
+  <maintainer email="takamasa.horibe@tier4.jp">Takamasa Horibe</maintainer>
+  <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
+  <maintainer email="fumiya.watanabe@tier4.jp">Fumiya Watanabe</maintainer>
+  <maintainer email="satoshi.ota@tier4.jp">Satoshi Ota</maintainer>
 
   <license>Apache License 2.0</license>
 

diff --git a/evaluator/localization_evaluator/package.xml b/evaluator/localization_evaluator/package.xml
@@ -5,6 +5,7 @@
   <description>localization evaluator ROS 2 node</description>
 
   <maintainer email="dominik.jargot@robotec.ai">Dominik Jargot</maintainer>
+  <maintainer email="koji.minoda@tier4.jp">Koji Minoda</maintainer>
 
   <license>Apache License 2.0</license>
 

diff --git a/...aunch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/...aunch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py
@@ -114,7 +114,6 @@ def launch_setup(context, *args, **kwargs):
                 "lane_change.use_all_predicted_path": LaunchConfiguration(
                     "use_experimental_lane_change_function"
                 ),
-                "bt_tree_config_path": LaunchConfiguration("behavior_path_planner_tree_param_path"),
             },
         ],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],

diff --git a/...unch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/...unch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml
@@ -20,7 +20,6 @@
     <param from="$(find-pkg-share tier4_planning_launch)/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml"/>
     <param from="$(find-pkg-share tier4_planning_launch)/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml"/>
     <param from="$(var vehicle_param_file)"/>
-    <param name="bt_tree_config_path" value="$(find-pkg-share behavior_path_planner)/config/behavior_path_planner_tree.xml"/>
   </node>
 
   <group>

diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
@@ -20,6 +20,7 @@
       enable_yield_maneuver_during_shifting: false
       disable_path_update: false
       use_hatched_road_markings: false
+      use_intersection_areas: false
 
       # for debug
       publish_debug_marker: false

diff --git a/planning/behavior_path_planner/config/behavior_path_planner_tree.xml b/planning/behavior_path_planner/config/behavior_path_planner_tree.xml
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp
@@ -92,6 +92,7 @@ struct DrivableAreaInfo
   std::vector<DrivableLanes> drivable_lanes{};
   std::vector<Obstacle> obstacles{};  // obstacles to extract from the drivable area
   bool enable_expanding_hatched_road_markings{false};
+  bool enable_expanding_intersection_areas{false};
 
   // temporary only for pull over's freespace planning
   double drivable_margin{0.0};

diff --git a/...havior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/...havior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp
@@ -196,8 +196,8 @@ class LaneChangeBase
   virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0;
 
   virtual PathWithLaneId getPrepareSegment(
-    const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current,
-    const double backward_path_length, const double prepare_length) const = 0;
+    const lanelet::ConstLanelets & current_lanes, const double backward_path_length,
+    const double prepare_length) const = 0;
 
   virtual bool getLaneChangePaths(
     const lanelet::ConstLanelets & original_lanelets,

diff --git a/...g/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/...g/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp
@@ -96,8 +96,8 @@ class NormalLaneChange : public LaneChangeBase
   int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override;
 
   PathWithLaneId getPrepareSegment(
-    const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current,
-    const double backward_path_length, const double prepare_length) const override;
+    const lanelet::ConstLanelets & current_lanes, const double backward_path_length,
+    const double prepare_length) const override;
 
   bool getLaneChangePaths(
     const lanelet::ConstLanelets & original_lanelets,

diff --git a/...planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/...planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp
@@ -149,6 +149,12 @@ class StartPlannerModule : public SceneModuleInterface
   bool isStopped();
   bool hasFinishedCurrentPath();
 
+  // check if the goal is located behind the ego in the same route segment.
+  bool IsGoalBehindOfEgoInSameRouteSegment() const;
+
+  // generate BehaviorPathOutput with stopping path.
+  BehaviorModuleOutput generateStopOutput() const;
+
   void setDebugData() const;
 };
 }  // namespace behavior_path_planner

diff --git a/...vior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/...vior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp
@@ -108,6 +108,9 @@ struct AvoidanceParameters
   // use hatched road markings for avoidance
   bool use_hatched_road_markings{false};
 
+  // use intersection area for avoidance
+  bool use_intersection_areas{false};
+
   // constrains
   bool use_constraints_for_decel{false};
 

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp
@@ -124,8 +124,9 @@ void filterTargetObjects(
 double extendToRoadShoulderDistanceWithPolygon(
   const std::shared_ptr<route_handler::RouteHandler> & rh,
   const lanelet::ConstLineString3d & target_line, const double to_road_shoulder_distance,
-  const geometry_msgs::msg::Point & overhang_pos,
-  const lanelet::BasicPoint3d & overhang_basic_pose);
+  const lanelet::ConstLanelet & overhang_lanelet, const geometry_msgs::msg::Point & overhang_pos,
+  const lanelet::BasicPoint3d & overhang_basic_pose, const bool use_hatched_road_markings,
+  const bool use_intersection_areas);
 
 void fillAdditionalInfoFromPoint(const AvoidancePlanningData & data, AvoidLineArray & lines);
 

diff --git a/..._path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/..._path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp
@@ -27,6 +27,40 @@
 namespace behavior_path_planner
 {
 
+struct PoseWithPolygon
+{
+  Pose pose;
+  Polygon2d poly;
+
+  PoseWithPolygon(const Pose & pose, const Polygon2d & poly) : pose(pose), poly(poly) {}
+};
+
+struct PoseWithPolygonStamped : public PoseWithPolygon
+{
+  double time;
+
+  PoseWithPolygonStamped(const double time, const Pose & pose, const Polygon2d & poly)
+  : PoseWithPolygon(pose, poly), time(time)
+  {
+  }
+};
+
+struct PredictedPathWithPolygon
+{
+  float confidence;
+  std::vector<PoseWithPolygonStamped> path;
+};
+
+struct ExtendedPredictedObject
+{
+  unique_identifier_msgs::msg::UUID uuid;
+  geometry_msgs::msg::PoseWithCovariance initial_pose;
+  geometry_msgs::msg::TwistWithCovariance initial_twist;
+  geometry_msgs::msg::AccelWithCovariance initial_acceleration;
+  autoware_auto_perception_msgs::msg::Shape shape;
+  std::vector<PredictedPathWithPolygon> predicted_paths;
+};
+
 struct LaneChangeCancelParameters
 {
   bool enable_on_prepare_phase{true};
@@ -108,6 +142,13 @@ struct LaneChangeTargetObjectIndices
   std::vector<size_t> other_lane{};
 };
 
+struct LaneChangeTargetObjects
+{
+  std::vector<ExtendedPredictedObject> current_lane{};
+  std::vector<ExtendedPredictedObject> target_lane{};
+  std::vector<ExtendedPredictedObject> other_lane{};
+};
+
 enum class LaneChangeModuleType {
   NORMAL = 0,
   EXTERNAL_REQUEST,
Original file line number	Diff line number	Diff line change
		@@ -0,0 +1,3 @@
		# autoware_adapi_specs

		This package is a specification of Autoware AD API.
Original file line number	Diff line number	Diff line change
		@@ -0,0 +1,3 @@
		# component_interface_specs

		This package is a specification of component interfaces.