[pull] main from autowarefoundation:main

common/motion_utils/src/marker/marker_helper.cpp

@@ -143,7 +143,7 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::handleVirtualWall
   size_t id_to_delete = id;
   visualization_msgs::msg::MarkerArray wall_marker;
 
-  if (poses.size() == 0 || previous_virtual_wall_poses.empty()) {
+  if (poses.size() == 0) {
     return wall_marker;
   }
 

launch/tier4_control_launch/package.xml

@@ -5,6 +5,7 @@
   <version>0.1.0</version>
   <description>The tier4_control_launch package</description>
   <maintainer email="takamasa.horibe@tier4.jp">Takamasa Horibe</maintainer>
+  <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>

perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml

@@ -6,3 +6,9 @@
     min_cluster_size: 10
     max_cluster_size: 3000
     use_height: false
+    max_x: 70.0
+    min_x: -70.0
+    max_y: 70.0
+    min_y: -70.0
+    max_z: 4.5
+    min_z: -4.5

perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py

@@ -49,23 +49,85 @@ def load_composable_node_param(param_path):
         parameters=[load_composable_node_param("compare_map_param_path")],
     )
 
-    # set voxel grid filter as a component
-    use_map_voxel_grid_filter_component = ComposableNode(
+    # separate range of poincloud when map_filter used
+    use_map_short_range_crop_box_filter_component = ComposableNode(
         package="pointcloud_preprocessor",
         namespace=ns,
-        plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent",
-        name=AnonName("voxel_grid_filter"),
-        remappings=[("input", "map_filter/pointcloud"), ("output", "downsampled/pointcloud")],
-        parameters=[load_composable_node_param("voxel_grid_param_path")],
+        plugin="pointcloud_preprocessor::CropBoxFilterComponent",
+        name="short_distance_crop_box_range",
+        remappings=[
+            ("input", "map_filter/pointcloud"),
+            ("output", "short_range/pointcloud"),
+        ],
+        parameters=[
+            load_composable_node_param("voxel_grid_based_euclidean_param_path"),
+            {
+                "negative": False,
+            },
+        ],
     )
-    disuse_map_voxel_grid_filter_component = ComposableNode(
+
+    use_map_long_range_crop_box_filter_component = ComposableNode(
+        package="pointcloud_preprocessor",
+        namespace=ns,
+        plugin="pointcloud_preprocessor::CropBoxFilterComponent",
+        name="long_distance_crop_box_range",
+        remappings=[
+            ("input", LaunchConfiguration("input_pointcloud")),
+            ("output", "long_range/pointcloud"),
+        ],
+        parameters=[
+            load_composable_node_param("voxel_grid_based_euclidean_param_path"),
+            {
+                "negative": True,
+            },
+        ],
+    )
+
+    # disuse_map_voxel_grid_filter
+    disuse_map_short_range_crop_box_filter_component = ComposableNode(
+        package="pointcloud_preprocessor",
+        namespace=ns,
+        plugin="pointcloud_preprocessor::CropBoxFilterComponent",
+        name="short_distance_crop_box_range",
+        remappings=[
+            ("input", LaunchConfiguration("input_pointcloud")),
+            ("output", "short_range/pointcloud"),
+        ],
+        parameters=[
+            load_composable_node_param("voxel_grid_based_euclidean_param_path"),
+            {
+                "negative": False,
+            },
+        ],
+    )
+
+    disuse_map_long_range_crop_box_filter_component = ComposableNode(
+        package="pointcloud_preprocessor",
+        namespace=ns,
+        plugin="pointcloud_preprocessor::CropBoxFilterComponent",
+        name="long_distance_crop_box_range",
+        remappings=[
+            ("input", LaunchConfiguration("input_pointcloud")),
+            ("output", "long_range/pointcloud"),
+        ],
+        parameters=[
+            load_composable_node_param("voxel_grid_based_euclidean_param_path"),
+            {
+                "negative": True,
+            },
+        ],
+    )
+
+    # set voxel grid filter as a component
+    voxel_grid_filter_component = ComposableNode(
         package="pointcloud_preprocessor",
         namespace=ns,
         plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent",
         name=AnonName("voxel_grid_filter"),
         remappings=[
-            ("input", LaunchConfiguration("input_pointcloud")),
-            ("output", "downsampled/pointcloud"),
+            ("input", "short_range/pointcloud"),
+            ("output", "downsampled/short_range/pointcloud"),
         ],
         parameters=[load_composable_node_param("voxel_grid_param_path")],
     )
@@ -76,18 +138,37 @@ def load_composable_node_param(param_path):
         namespace=ns,
         plugin="pointcloud_preprocessor::VoxelGridOutlierFilterComponent",
         name="outlier_filter",
-        remappings=[("input", "downsampled/pointcloud"), ("output", "outlier_filter/pointcloud")],
+        remappings=[
+            ("input", "downsampled/short_range/pointcloud"),
+            ("output", "outlier_filter/pointcloud"),
+        ],
         parameters=[load_composable_node_param("outlier_param_path")],
     )
 
+    # concat with-outlier pointcloud and without-outlier pcl
+    downsample_concat_component = ComposableNode(
+        package="pointcloud_preprocessor",
+        namespace=ns,
+        plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent",
+        name="concat_downsampled_pcl",
+        remappings=[("output", "downsampled/concatenated/pointcloud")],
+        parameters=[
+            {
+                "input_topics": ["long_range/pointcloud", "outlier_filter/pointcloud"],
+                "output_frame": "base_link",
+            }
+        ],
+        extra_arguments=[{"use_intra_process_comms": True}],
+    )
+
     # set euclidean cluster as a component
     euclidean_cluster_component = ComposableNode(
         package=pkg,
         namespace=ns,
         plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode",
         name="euclidean_cluster",
         remappings=[
-            ("input", "outlier_filter/pointcloud"),
+            ("input", "downsampled/concatenated/pointcloud"),
             ("output", LaunchConfiguration("output_clusters")),
         ],
         parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")],
@@ -98,21 +179,30 @@ def load_composable_node_param(param_path):
         package="rclcpp_components",
         namespace=ns,
         executable="component_container",
-        composable_node_descriptions=[outlier_filter_component, euclidean_cluster_component],
+        composable_node_descriptions=[
+            voxel_grid_filter_component,
+            outlier_filter_component,
+            downsample_concat_component,
+            euclidean_cluster_component,
+        ],
         output="screen",
     )
 
     use_map_loader = LoadComposableNodes(
         composable_node_descriptions=[
             compare_map_filter_component,
-            use_map_voxel_grid_filter_component,
+            use_map_short_range_crop_box_filter_component,
+            use_map_long_range_crop_box_filter_component,
         ],
         target_container=container,
         condition=IfCondition(LaunchConfiguration("use_pointcloud_map")),
     )
 
     disuse_map_loader = LoadComposableNodes(
-        composable_node_descriptions=[disuse_map_voxel_grid_filter_component],
+        composable_node_descriptions=[
+            disuse_map_short_range_crop_box_filter_component,
+            disuse_map_long_range_crop_box_filter_component,
+        ],
         target_container=container,
         condition=UnlessCondition(LaunchConfiguration("use_pointcloud_map")),
     )

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

@@ -198,7 +198,7 @@ std::optional<LaneChangePath> constructCandidatePath(
   path_shifter.setLateralAccelerationLimit(std::abs(lane_change_param.lane_changing_lateral_acc));
 
   if (!path_shifter.generate(&shifted_path, offset_back)) {
-    RCLCPP_ERROR_STREAM(
+    RCLCPP_DEBUG(
       rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"),
       "failed to generate shifted path.");
   }

planning/behavior_velocity_planner/config/intersection.param.yaml

@@ -20,6 +20,7 @@
       use_stuck_stopline: false # stopline generate before the intersection lanelet when is_stuck
       assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning
       enable_front_car_decel_prediction: false # By default this feature is disabled
+      stop_overshoot_margin: 0.5 # [m] overshoot margin for stuck, collsion detection
 
     merge_from_private_road:
       stop_duration_sec: 1.0

planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp

@@ -93,6 +93,7 @@ class IntersectionModule : public SceneModuleInterface
     double
       assumed_front_car_decel;  //! the expected deceleration of front car when front car as well
     bool enable_front_car_decel_prediction;  //! flag for using above feature
+    double stop_overshoot_margin;            //! overshoot margin for stuck, collsion detection
   };
 
   IntersectionModule(

planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp

@@ -69,6 +69,7 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
   ip.assumed_front_car_decel = node.declare_parameter(ns + ".assumed_front_car_decel", 1.0);
   ip.enable_front_car_decel_prediction =
     node.declare_parameter(ns + ".enable_front_car_decel_prediction", false);
+  ip.stop_overshoot_margin = node.declare_parameter(ns + ".stop_overshoot_margin", 0.5);
 }
 
 MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node)

planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp

@@ -202,10 +202,9 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
     const double dist_stuck_stopline = motion_utils::calcSignedArcLength(
       path->points, path->points.at(stuck_line_idx).point.pose.position,
       path->points.at(closest_idx).point.pose.position);
-    const double eps = 1e-1;  // NOTE: check if sufficiently over the stuck stopline
     const bool is_over_stuck_stopline =
       util::isOverTargetIndex(*path, closest_idx, current_pose, stuck_line_idx) &&
-      dist_stuck_stopline > eps;
+      dist_stuck_stopline > planner_param_.stop_overshoot_margin;
     if (is_stuck && !is_over_stuck_stopline) {
       stop_line_idx_final = stuck_line_idx;
       pass_judge_line_idx_final = stuck_line_idx;

vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/accel_map.hpp

@@ -29,7 +29,7 @@ namespace raw_vehicle_cmd_converter
 class AccelMap
 {
 public:
-  bool readAccelMapFromCSV(const std::string & csv_path);
+  bool readAccelMapFromCSV(const std::string & csv_path, const bool validation = false);
   bool getThrottle(const double acc, const double vel, double & throttle) const;
   bool getAcceleration(const double throttle, const double vel, double & acc) const;
   std::vector<double> getVelIdx() const { return vel_index_; }

vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/brake_map.hpp

@@ -29,7 +29,7 @@ namespace raw_vehicle_cmd_converter
 class BrakeMap
 {
 public:
-  bool readBrakeMapFromCSV(const std::string & csv_path);
+  bool readBrakeMapFromCSV(const std::string & csv_path, const bool validation = false);
   bool getBrake(const double acc, const double vel, double & brake);
   bool getAcceleration(const double brake, const double vel, double & acc) const;
   std::vector<double> getVelIdx() const { return vel_index_; }

vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/steer_map.hpp

@@ -28,7 +28,7 @@ namespace raw_vehicle_cmd_converter
 class SteerMap
 {
 public:
-  bool readSteerMapFromCSV(const std::string & csv_path);
+  bool readSteerMapFromCSV(const std::string & csv_path, const bool validation = false);
   void getSteer(const double steer_rate, const double steer, double & output) const;
 
 private:

vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp

@@ -25,7 +25,7 @@ using namespace std::literals::chrono_literals;
 
 namespace raw_vehicle_cmd_converter
 {
-bool AccelMap::readAccelMapFromCSV(const std::string & csv_path)
+bool AccelMap::readAccelMapFromCSV(const std::string & csv_path, const bool validation)
 {
   CSVLoader csv(csv_path);
   std::vector<std::vector<std::string>> table;
@@ -38,7 +38,7 @@ bool AccelMap::readAccelMapFromCSV(const std::string & csv_path)
   vel_index_ = CSVLoader::getRowIndex(table);
   throttle_index_ = CSVLoader::getColumnIndex(table);
   accel_map_ = CSVLoader::getMap(table);
-  if (!CSVLoader::validateMap(accel_map_, true)) {
+  if (validation && !CSVLoader::validateMap(accel_map_, true)) {
     return false;
   }
   return true;

vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp

@@ -22,7 +22,7 @@
 
 namespace raw_vehicle_cmd_converter
 {
-bool BrakeMap::readBrakeMapFromCSV(const std::string & csv_path)
+bool BrakeMap::readBrakeMapFromCSV(const std::string & csv_path, const bool validation)
 {
   CSVLoader csv(csv_path);
   std::vector<std::vector<std::string>> table;
@@ -36,7 +36,7 @@ bool BrakeMap::readBrakeMapFromCSV(const std::string & csv_path)
   brake_index_ = CSVLoader::getColumnIndex(table);
   brake_map_ = CSVLoader::getMap(table);
   brake_index_rev_ = brake_index_;
-  if (!CSVLoader::validateMap(brake_map_, false)) {
+  if (validation && !CSVLoader::validateMap(brake_map_, false)) {
     return false;
   }
   std::reverse(std::begin(brake_index_rev_), std::end(brake_index_rev_));

vehicle/raw_vehicle_cmd_converter/src/node.cpp

@@ -42,17 +42,17 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode(
   use_steer_ff_ = declare_parameter("use_steer_ff", true);
   use_steer_fb_ = declare_parameter("use_steer_fb", true);
   if (convert_accel_cmd_) {
-    if (!accel_map_.readAccelMapFromCSV(csv_path_accel_map)) {
+    if (!accel_map_.readAccelMapFromCSV(csv_path_accel_map, true)) {
       throw std::invalid_argument("Accel map is invalid.");
     }
   }
   if (convert_brake_cmd_) {
-    if (!brake_map_.readBrakeMapFromCSV(csv_path_brake_map)) {
+    if (!brake_map_.readBrakeMapFromCSV(csv_path_brake_map, true)) {
       throw std::invalid_argument("Brake map is invalid.");
     }
   }
   if (convert_steer_cmd_) {
-    if (!steer_map_.readSteerMapFromCSV(csv_path_steer_map)) {
+    if (!steer_map_.readSteerMapFromCSV(csv_path_steer_map, true)) {
       throw std::invalid_argument("Steer map is invalid.");
     }
     const auto kp_steer{declare_parameter("steer_pid.kp", 150.0)};

vehicle/raw_vehicle_cmd_converter/src/steer_map.cpp

@@ -22,7 +22,7 @@
 namespace raw_vehicle_cmd_converter
 {
 
-bool SteerMap::readSteerMapFromCSV(const std::string & csv_path)
+bool SteerMap::readSteerMapFromCSV(const std::string & csv_path, const bool validation)
 {
   CSVLoader csv(csv_path);
   std::vector<std::vector<std::string>> table;
@@ -35,7 +35,7 @@ bool SteerMap::readSteerMapFromCSV(const std::string & csv_path)
   steer_index_ = CSVLoader::getRowIndex(table);
   output_index_ = CSVLoader::getColumnIndex(table);
   steer_map_ = CSVLoader::getMap(table);
-  if (!CSVLoader::validateMap(steer_map_, true)) {
+  if (validation && !CSVLoader::validateMap(steer_map_, true)) {
     return false;
   }
   return true;

vehicle/raw_vehicle_cmd_converter/test/test_raw_vehicle_cmd_converter.cpp

@@ -95,9 +95,9 @@ TEST(ConverterTests, LoadExampleMap)
   const auto data_path =
     ament_index_cpp::get_package_share_directory("raw_vehicle_cmd_converter") + "/data/default/";
   // for invalid path
-  EXPECT_TRUE(accel_map.readAccelMapFromCSV(data_path + "accel_map.csv"));
-  EXPECT_TRUE(brake_map.readBrakeMapFromCSV(data_path + "brake_map.csv"));
-  EXPECT_TRUE(steer_map.readSteerMapFromCSV(data_path + "steer_map.csv"));
+  EXPECT_TRUE(accel_map.readAccelMapFromCSV(data_path + "accel_map.csv", true));
+  EXPECT_TRUE(brake_map.readBrakeMapFromCSV(data_path + "brake_map.csv", true));
+  EXPECT_TRUE(steer_map.readSteerMapFromCSV(data_path + "steer_map.csv", true));
 }
 
 TEST(ConverterTests, LoadValidPath)
@@ -112,14 +112,14 @@ TEST(ConverterTests, LoadValidPath)
   EXPECT_TRUE(loadSteerMapData(steer_map));
 
   // for invalid path
-  EXPECT_FALSE(accel_map.readAccelMapFromCSV("invalid.csv"));
-  EXPECT_FALSE(brake_map.readBrakeMapFromCSV("invalid.csv"));
-  EXPECT_FALSE(steer_map.readSteerMapFromCSV("invalid.csv"));
+  EXPECT_FALSE(accel_map.readAccelMapFromCSV("invalid.csv", true));
+  EXPECT_FALSE(brake_map.readBrakeMapFromCSV("invalid.csv", true));
+  EXPECT_FALSE(steer_map.readSteerMapFromCSV("invalid.csv", true));
 
   // for invalid maps
-  EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_1col_map.csv"));
-  EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_inconsistent_rows_map.csv"));
-  EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_not_interpolatable.csv"));
+  EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_1col_map.csv", true));
+  EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_inconsistent_rows_map.csv", true));
+  EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_not_interpolatable.csv", true));
 }
 
 TEST(ConverterTests, AccelMapCalculation)