Merge pull request autowarefoundation#816 from tier4/sync-upstream

chore: sync upstream

common/tier4_planning_rviz_plugin/include/path/display_base.hpp

@@ -506,6 +506,10 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>
       vehicle_footprint_info_ = std::make_shared<VehicleFootprintInfo>(
         vehicle_info_->vehicle_length_m, vehicle_info_->vehicle_width_m,
         vehicle_info_->rear_overhang_m);
+
+      property_vehicle_length_.setValue(vehicle_info_->vehicle_length_m);
+      property_vehicle_width_.setValue(vehicle_info_->vehicle_width_m);
+      property_rear_overhang_.setValue(vehicle_info_->rear_overhang_m);
     } else {
       const float length{property_vehicle_length_.getFloat()};
       const float width{property_vehicle_width_.getFloat()};

control/operation_mode_transition_manager/README.md

@@ -25,7 +25,27 @@ There is also an `In Transition` state that occurs during each mode transitions.
 
 ## Design
 
-<!-- ## Assumptions / Known limits -->
+A rough design of the relationship between `operation_mode_transition_manager`` and the other nodes is shown below.
+
+![transition_rough_structure](image/transition_rough_structure.drawio.svg)
+
+A more detailed structure is below.
+
+![transition_detailed_structure](image/transition_detailed_structure.drawio.svg)
+
+Here we see that `operation_mode_transition_manager` has multiple state transitions as follows
+
+- **AUTOWARE ENABLED <---> DISABLED**
+  - **ENABLED**: the vehicle is controlled by Autoware.
+  - **DISABLED**: the vehicle is out of Autoware control, expecting the e.g. manual driving.
+- **AUTOWARE ENABLED <---> AUTO/LOCAL/REMOTE/NONE**
+  - **AUTO**: the vehicle is controlled by Autoware, with the autonomous control command calculated by the planning/control component.
+  - **LOCAL**: the vehicle is controlled by Autoware, with the locally connected operator, e.g. joystick controller.
+  - **REMOTE**: the vehicle is controlled by Autoware, with the remotely connected operator.
+  - **NONE**: the vehicle is not controlled by any operator.
+- **IN TRANSITION <---> COMPLETED**
+  - **IN TRANSITION**: the mode listed above is in the transition process, expecting the former operator to have a responsibility to confirm the transition is completed.
+  - **COMPLETED**: the mode transition is completed.
 
 ## Inputs / Outputs / API
 

localization/ndt_scan_matcher/README.md

@@ -64,6 +64,7 @@ One optional function is regularization. Please see the regularization chapter i
 | `max_iterations`                        | int    | The number of iterations required to calculate alignment                                        |
 | `converged_param_type`                  | int    | The type of indicators for scan matching score (0: TP, 1: NVTL)                                 |
 | `converged_param_transform_probability` | double | Threshold for deciding whether to trust the estimation result                                   |
+| `lidar_topic_timeout_sec`               | double | Tolerance of timestamp difference between current time and sensor pointcloud                    |
 | `num_threads`                           | int    | Number of threads used for parallel computing                                                   |
 
 (TP: Transform Probability, NVTL: Nearest Voxel Transform Probability)

localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml

@@ -37,6 +37,9 @@
     # The number of particles to estimate initial pose
     initial_estimate_particles_num: 100
 
+    # Tolerance of timestamp difference between current time and sensor pointcloud. [sec]
+    lidar_topic_timeout_sec: 1.0
+
     # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec]
     initial_pose_timeout_sec: 1.0
 

localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp

@@ -179,6 +179,7 @@ class NDTScanMatcher : public rclcpp::Node
   double converged_param_nearest_voxel_transformation_likelihood_;
 
   int initial_estimate_particles_num_;
+  double lidar_topic_timeout_sec_;
   double initial_pose_timeout_sec_;
   double initial_pose_distance_tolerance_m_;
   float inversion_vector_threshold_;

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

@@ -91,6 +91,7 @@ NDTScanMatcher::NDTScanMatcher()
   converged_param_transform_probability_(4.5),
   converged_param_nearest_voxel_transformation_likelihood_(2.3),
   initial_estimate_particles_num_(100),
+  lidar_topic_timeout_sec_(1.0),
   initial_pose_timeout_sec_(1.0),
   initial_pose_distance_tolerance_m_(10.0),
   inversion_vector_threshold_(-0.9),
@@ -141,6 +142,9 @@ NDTScanMatcher::NDTScanMatcher()
     "converged_param_nearest_voxel_transformation_likelihood",
     converged_param_nearest_voxel_transformation_likelihood_);
 
+  lidar_topic_timeout_sec_ =
+    this->declare_parameter("lidar_topic_timeout_sec", lidar_topic_timeout_sec_);
+
   initial_pose_timeout_sec_ =
     this->declare_parameter("initial_pose_timeout_sec", initial_pose_timeout_sec_);
 
@@ -268,6 +272,12 @@ void NDTScanMatcher::timer_diagnostic()
       diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
       diag_status_msg.message += "Initializing State. ";
     }
+    if (
+      state_ptr_->count("lidar_topic_delay_time_sec") &&
+      std::stod((*state_ptr_)["lidar_topic_delay_time_sec"]) > lidar_topic_timeout_sec_) {
+      diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+      diag_status_msg.message += "lidar_topic_delay_time_sec exceed limit. ";
+    }
     if (
       state_ptr_->count("skipping_publish_num") &&
       std::stoi((*state_ptr_)["skipping_publish_num"]) > 1 &&
@@ -347,11 +357,29 @@ void NDTScanMatcher::callback_sensor_points(
     return;
   }
 
+  const rclcpp::Time sensor_ros_time = sensor_points_msg_in_sensor_frame->header.stamp;
+  const double lidar_topic_delay_time_sec = (this->now() - sensor_ros_time).seconds();
+  (*state_ptr_)["lidar_topic_delay_time_sec"] = std::to_string(lidar_topic_delay_time_sec);
+
+  if (lidar_topic_delay_time_sec > lidar_topic_timeout_sec_) {
+    RCLCPP_WARN(
+      this->get_logger(),
+      "The LiDAR topic is experiencing latency. The delay time is %lf[sec] (the tolerance is "
+      "%lf[sec])",
+      lidar_topic_delay_time_sec, lidar_topic_timeout_sec_);
+
+    // If the delay time of the LiDAR topic exceeds the delay compensation time of ekf_localizer,
+    // even if further processing continues, the estimated result will be rejected by ekf_localizer.
+    // Therefore, it would be acceptable to exit the function here.
+    // However, for now, we will continue the processing as it is.
+
+    // return;
+  }
+
   // mutex ndt_ptr_
   std::lock_guard<std::mutex> lock(ndt_ptr_mtx_);
 
   const auto exe_start_time = std::chrono::system_clock::now();
-  const rclcpp::Time sensor_ros_time = sensor_points_msg_in_sensor_frame->header.stamp;
 
   // preprocess input pointcloud
   pcl::shared_ptr<pcl::PointCloud<PointSource>> sensor_points_in_sensor_frame(

perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp

@@ -20,11 +20,9 @@
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
-#include <motion_utils/motion_utils.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/ros/transform_listener.hpp>
 #include <tier4_autoware_utils/system/stop_watch.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>

perception/map_based_prediction/src/map_based_prediction_node.cpp

@@ -16,7 +16,10 @@
 
 #include <interpolation/linear_interpolation.hpp>
 #include <motion_utils/resample/resample.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+#include <tier4_autoware_utils/math/constants.hpp>
+#include <tier4_autoware_utils/math/normalization.hpp>
+#include <tier4_autoware_utils/ros/uuid_helper.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 

perception/map_based_prediction/src/path_generator.cpp

@@ -15,8 +15,8 @@
 #include "map_based_prediction/path_generator.hpp"
 
 #include <interpolation/spline_interpolation.hpp>
-#include <motion_utils/motion_utils.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <motion_utils/trajectory/trajectory.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
 
 #include <algorithm>
 

planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp

@@ -18,6 +18,7 @@
 #include "behavior_path_planner/parameters.hpp"
 #include "behavior_path_planner/turn_signal_decider.hpp"
 #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp"
+#include "motion_utils/trajectory/trajectory.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 #include <route_handler/route_handler.hpp>

planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp

@@ -17,9 +17,8 @@
 #include "behavior_path_planner/data_manager.hpp"
 #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp"
 #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp"
-#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
 
-#include <tier4_autoware_utils/ros/marker_helper.hpp>
+#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
 
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_perception_msgs/msg/predicted_path.hpp>

planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp

@@ -19,6 +19,7 @@
 #include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp"
 #include "behavior_path_planner/scene_module/scene_module_visitor.hpp"
 #include "behavior_path_planner/utils/lane_following/module_data.hpp"
+#include "tier4_autoware_utils/system/stop_watch.hpp"
 
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <rclcpp/rclcpp.hpp>

planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp

@@ -22,10 +22,15 @@
 #include <behavior_path_planner/steering_factor_interface.hpp>
 #include <behavior_path_planner/turn_signal_decider.hpp>
 #include <magic_enum.hpp>
+#include <motion_utils/marker/marker_helper.hpp>
+#include <motion_utils/trajectory/path_with_lane_id.hpp>
+#include <motion_utils/trajectory/trajectory.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <route_handler/route_handler.hpp>
 #include <rtc_interface/rtc_interface.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
+#include <tier4_autoware_utils/ros/uuid_helper.hpp>
 
 #include <autoware_adapi_v1_msgs/msg/steering_factor_array.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp

@@ -20,10 +20,11 @@
 #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp"
 
 #include <rclcpp/rclcpp.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
 
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
 #include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
 
 #include <lanelet2_core/geometry/Lanelet.h>

planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp

@@ -22,11 +22,11 @@
 #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp"
 #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp"
 #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp"
-#include "motion_utils/motion_utils.hpp"
+#include "motion_utils/trajectory/trajectory.hpp"
 #include "object_recognition_utils/predicted_path_utils.hpp"
 
 #include <route_handler/route_handler.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
 
 #include <autoware_auto_perception_msgs/msg/object_classification.hpp>
 #include <autoware_auto_perception_msgs/msg/predicted_object.hpp>

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -19,7 +19,7 @@
 #include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp"
 #include "behavior_path_planner/utils/path_utils.hpp"
 
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <tier4_autoware_utils/ros/update_param.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
 #include <tier4_planning_msgs/msg/path_change_module_id.hpp>

planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp

@@ -16,6 +16,7 @@
 
 #include "behavior_path_planner/utils/path_utils.hpp"
 #include "behavior_path_planner/utils/utils.hpp"
+#include "motion_utils/trajectory/path_with_lane_id.hpp"
 
 #include <magic_enum.hpp>
 

planning/behavior_path_planner/src/marker_utils/utils.cpp

@@ -19,8 +19,9 @@
 #include "behavior_path_planner/utils/path_utils.hpp"
 #include "behavior_path_planner/utils/utils.hpp"
 
+#include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
-
+#include <tier4_autoware_utils/ros/uuid_helper.hpp>
 namespace marker_utils
 {
 using behavior_path_planner::ShiftLine;

planning/behavior_path_planner/src/planner_manager.cpp

@@ -16,6 +16,7 @@
 
 #include "behavior_path_planner/utils/path_utils.hpp"
 #include "behavior_path_planner/utils/utils.hpp"
+#include "tier4_autoware_utils/system/stop_watch.hpp"
 
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <magic_enum.hpp>

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -23,7 +23,10 @@
 
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+#include <tier4_autoware_utils/ros/marker_helper.hpp>
+#include <tier4_autoware_utils/ros/uuid_helper.hpp>
 
 #include <tier4_planning_msgs/msg/avoidance_debug_factor.hpp>
 #include <tier4_planning_msgs/msg/avoidance_debug_msg.hpp>

planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp

@@ -14,6 +14,9 @@
 
 #include "behavior_path_planner/scene_module/avoidance/manager.hpp"
 
+#include "tier4_autoware_utils/ros/parameter.hpp"
+#include "tier4_autoware_utils/ros/update_param.hpp"
+
 #include <rclcpp/rclcpp.hpp>
 
 #include <memory>

planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp

@@ -20,7 +20,7 @@
 
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
 
 #include <algorithm>
 #include <limits>

planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp

@@ -14,6 +14,8 @@
 
 #include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp"
 
+#include "tier4_autoware_utils/ros/update_param.hpp"
+
 #include <rclcpp/rclcpp.hpp>
 
 #include <memory>

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

@@ -21,13 +21,12 @@
 #include "behavior_path_planner/utils/path_utils.hpp"
 #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp"
 #include "behavior_path_planner/utils/utils.hpp"
+#include "tier4_autoware_utils/math/unit_conversion.hpp"
 
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <magic_enum.hpp>
-#include <motion_utils/motion_utils.hpp>
 #include <rclcpp/rclcpp.hpp>
-#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <algorithm>
 #include <limits>

planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp

@@ -15,8 +15,8 @@
 #include "behavior_path_planner/scene_module/goal_planner/manager.hpp"
 
 #include "behavior_path_planner/utils/goal_planner/util.hpp"
+#include "tier4_autoware_utils/ros/update_param.hpp"
 
-#include <lanelet2_extension/utility/utilities.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <memory>

planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp

@@ -14,6 +14,9 @@
 
 #include "behavior_path_planner/scene_module/lane_change/manager.hpp"
 
+#include "tier4_autoware_utils/ros/parameter.hpp"
+#include "tier4_autoware_utils/ros/update_param.hpp"
+
 #include <rclcpp/rclcpp.hpp>
 
 #include <memory>

planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

@@ -23,6 +23,7 @@
 
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
+#include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
 
 #include <algorithm>
 #include <limits>

planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp

@@ -14,6 +14,8 @@
 
 #include "behavior_path_planner/scene_module/side_shift/manager.hpp"
 
+#include "tier4_autoware_utils/ros/update_param.hpp"
+
 #include <rclcpp/rclcpp.hpp>
 
 #include <memory>

planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp

@@ -14,6 +14,8 @@
 
 #include "behavior_path_planner/scene_module/start_planner/manager.hpp"
 
+#include "tier4_autoware_utils/ros/update_param.hpp"
+
 #include <rclcpp/rclcpp.hpp>
 
 #include <memory>