diff --git a/common/motion_utils/src/marker/marker_helper.cpp b/common/motion_utils/src/marker/marker_helper.cpp index 60e5f09bbea89..ab4dc623c3125 100644 --- a/common/motion_utils/src/marker/marker_helper.cpp +++ b/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; } diff --git a/launch/tier4_control_launch/package.xml b/launch/tier4_control_launch/package.xml index bcb705d6b885e..108e43915b26f 100644 --- a/launch/tier4_control_launch/package.xml +++ b/launch/tier4_control_launch/package.xml @@ -5,6 +5,7 @@ 0.1.0 The tier4_control_launch package Takamasa Horibe + Takayuki Murooka Apache License 2.0 ament_cmake_auto diff --git a/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml b/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml index 27ba5a8178fcf..1ff1e7c8ab55e 100644 --- a/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml +++ b/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 diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py index 2cfc5142f6fd8..f523245092564 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py +++ b/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,10 +138,29 @@ 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, @@ -87,7 +168,7 @@ def load_composable_node_param(param_path): 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")), ) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 85454cc449e3e..1059a1187684c 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -198,7 +198,7 @@ std::optional 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."); } diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml index a769848497680..9ed153bb1c3d4 100644 --- a/planning/behavior_velocity_planner/config/intersection.param.yaml +++ b/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 diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 98589555bd7aa..7ac5c25ce38e1 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/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( diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index a8af4938b1bf0..8f4ee1c91cee2 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/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) diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index bbe6955c2a601..ed44be0508ee5 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/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; diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/accel_map.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/accel_map.hpp index fba69efdbc921..3bdb71a11f744 100644 --- a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/accel_map.hpp +++ b/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 getVelIdx() const { return vel_index_; } diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/brake_map.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/brake_map.hpp index 8d4d35545254c..6dd5ab94c5d06 100644 --- a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/brake_map.hpp +++ b/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 getVelIdx() const { return vel_index_; } diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/steer_map.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/steer_map.hpp index 5bdfdef639d20..7e4a3084d0223 100644 --- a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/steer_map.hpp +++ b/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: diff --git a/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp b/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp index 9ebb4bdbb5df7..42c63b152e4f4 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp +++ b/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> 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; diff --git a/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp b/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp index 154a0fc455a0a..68d89474f3ca6 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp +++ b/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> 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_)); diff --git a/vehicle/raw_vehicle_cmd_converter/src/node.cpp b/vehicle/raw_vehicle_cmd_converter/src/node.cpp index 007cce0b66cff..fb0bd00794c52 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/node.cpp +++ b/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)}; diff --git a/vehicle/raw_vehicle_cmd_converter/src/steer_map.cpp b/vehicle/raw_vehicle_cmd_converter/src/steer_map.cpp index 73a6641dd424d..e050a1beba3ae 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/steer_map.cpp +++ b/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> 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; diff --git a/vehicle/raw_vehicle_cmd_converter/test/test_raw_vehicle_cmd_converter.cpp b/vehicle/raw_vehicle_cmd_converter/test/test_raw_vehicle_cmd_converter.cpp index 3c8eba3072126..9f035b303e1a9 100644 --- a/vehicle/raw_vehicle_cmd_converter/test/test_raw_vehicle_cmd_converter.cpp +++ b/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)