From ac82f10e9a65f5f6fb906e40bf1f2c00f62834df Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 22 Dec 2022 17:49:16 +0900 Subject: [PATCH 01/92] fix(obstacle_avoidance_planner): deal with wider path interval (#1616) (#180) * fix(obstacle_avoidance_planner): deal with wider path interval Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka Co-authored-by: Yuma Nihei --- planning/obstacle_avoidance_planner/src/node.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 462ebbb9eca20..d3f26893b4241 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -1468,7 +1468,13 @@ ObstacleAvoidancePlanner::alignVelocity( auto fine_traj_points_with_vel = fine_traj_points_with_path_zero_vel; size_t prev_begin_idx = 0; for (size_t i = 0; i < fine_traj_points_with_vel.size(); ++i) { - const auto truncated_points = points_utils::clipForwardPoints(path_points, prev_begin_idx, 5.0); + auto truncated_points = points_utils::clipForwardPoints(path_points, prev_begin_idx, 5.0); + if (truncated_points.size() < 2) { + // NOTE: At least, two points must be contained in truncated_points + truncated_points = std::vector( + path_points.begin() + prev_begin_idx, + path_points.begin() + std::min(path_points.size(), prev_begin_idx + 2)); + } const auto & target_pose = fine_traj_points_with_vel[i].pose; const auto closest_seg_idx_optional = tier4_autoware_utils::findNearestSegmentIndex( From ec11dab438da0ec947e85bbbc63a5ccfe3c4b67a Mon Sep 17 00:00:00 2001 From: Yuma Nihei Date: Fri, 23 Dec 2022 16:31:31 +0900 Subject: [PATCH 02/92] fix(elevation_map_loader): remove and create map bagfile if it cannot be loaded (backport #1772) (#218) fix(elevation_map_loader): remove and create map bagfile if it cannot be loaded (#1772) * fix(elevation_map_loader): remove and create map bagfile if it cannot be loaded Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> * fix(elevation_map_loader): add dependencies Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> * fix(elevation_map_loader): use snake case for variable Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> Co-authored-by: Shintaro Tomie <58775300+Shin-kyoto@users.noreply.github.com> --- perception/elevation_map_loader/package.xml | 1 + .../src/elevation_map_loader_node.cpp | 22 +++++++++++++++++-- 2 files changed, 21 insertions(+), 2 deletions(-) diff --git a/perception/elevation_map_loader/package.xml b/perception/elevation_map_loader/package.xml index b60113c41c26f..1a727f591ffcd 100644 --- a/perception/elevation_map_loader/package.xml +++ b/perception/elevation_map_loader/package.xml @@ -19,6 +19,7 @@ pcl_conversions rclcpp rclcpp_components + rosbag2_storage_default_plugins tf2_geometry_msgs tf2_ros tier4_autoware_utils diff --git a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp index cac07dab64fa7..57f95eb7e1e89 100644 --- a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp +++ b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp @@ -44,6 +44,7 @@ #define EIGEN_MPL2_ONLY #include #include +#include ElevationMapLoaderNode::ElevationMapLoaderNode(const rclcpp::NodeOptions & options) : Node("elevation_map_loader", options) @@ -102,8 +103,25 @@ void ElevationMapLoaderNode::publish() RCLCPP_INFO( this->get_logger(), "Load elevation map from: %s", data_manager_.elevation_map_path_->c_str()); - grid_map::GridMapRosConverter::loadFromBag( - *data_manager_.elevation_map_path_, "elevation_map", elevation_map_); + + // Check if bag can be loaded + bool is_bag_loaded = false; + try { + is_bag_loaded = grid_map::GridMapRosConverter::loadFromBag( + *data_manager_.elevation_map_path_, "elevation_map", elevation_map_); + } catch (rosbag2_storage_plugins::SqliteException & e) { + is_bag_loaded = false; + } + if (!is_bag_loaded) { + // Delete directory including elevation map if bag is broken + RCLCPP_ERROR( + this->get_logger(), "Try to loading bag, but bag is broken. Remove %s", + data_manager_.elevation_map_path_->c_str()); + std::filesystem::remove_all(data_manager_.elevation_map_path_->c_str()); + // Create elevation map from pointcloud map if bag is broken + RCLCPP_INFO(this->get_logger(), "Create elevation map from pointcloud map "); + createElevationMap(); + } } elevation_map_.setFrameId(map_frame_); From eef08f976188bb864e4232c1988bd8a000add969 Mon Sep 17 00:00:00 2001 From: Yuma Nihei Date: Fri, 23 Dec 2022 18:26:34 +0900 Subject: [PATCH 03/92] feat(system_error_monitor): add heartbeat timeout for subscribed data (backport #1501) (#217) feat(system_error_monitor): add heartbeat timeout for subscribed data (#1501) * feat(system_error_monitor): add heartbeat timeout for subscribed data * fix: pre-commit * ci(pre-commit): autofix * docs: update README * ci(pre-commit): autofix * fix : not use Heartbeat Checker * fix: add stamp to autoware/state * fix: delete unused header * fix: add stamp for heartbeat * Revert "fix: add stamp to autoware/state" This reverts commit cff00c68899c29d136731081f1088f1b7a020880. Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Hiroki OTA Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- system/system_error_monitor/README.md | 13 ++--- .../system_error_monitor_core.hpp | 8 +++ .../launch/system_error_monitor.launch.xml | 2 + .../system_error_monitor_node.launch.xml | 2 + .../src/system_error_monitor_core.cpp | 49 +++++++++++++++++++ 5 files changed, 68 insertions(+), 6 deletions(-) diff --git a/system/system_error_monitor/README.md b/system/system_error_monitor/README.md index d1bfbdc95488d..a62d4b0f00576 100644 --- a/system/system_error_monitor/README.md +++ b/system/system_error_monitor/README.md @@ -108,12 +108,13 @@ endif ### Node Parameters -| Name | Type | Default Value | Explanation | -| ---------------------------- | ------ | ------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `ignore_missing_diagnostics` | bool | `false` | If this parameter is turned off, it will be ignored if required modules have not been received. | -| `add_leaf_diagnostics` | bool | `true` | Required to use children diagnostics. | -| `diag_timeout_sec` | double | `1.0` (sec) | If required diagnostic is not received for a `diag_timeout_sec`, the diagnostic state become STALE state. | -| `data_ready_timeout` | double | `30.0` | If input topics required for system_error_monitor are not available for `data_ready_timeout` seconds, autoware_state will translate to emergency state. | +| Name | Type | Default Value | Explanation | +| ---------------------------- | ------ | ------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `ignore_missing_diagnostics` | bool | `false` | If this parameter is turned off, it will be ignored if required modules have not been received. | +| `add_leaf_diagnostics` | bool | `true` | Required to use children diagnostics. | +| `diag_timeout_sec` | double | `1.0` (sec) | If required diagnostic is not received for a `diag_timeout_sec`, the diagnostic state become STALE state. | +| `data_ready_timeout` | double | `30.0` | If input topics required for system_error_monitor are not available for `data_ready_timeout` seconds, autoware_state will translate to emergency state. | +| `data_heartbeat_timeout` | double | `1.0` | If input topics required for system_error_monitor are not no longer subscribed for `data_heartbeat_timeout` seconds, autoware_state will translate to emergency state. | ### Core Parameters diff --git a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp index 19c94cd21d45c..a170fa8b90742 100644 --- a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp +++ b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp @@ -71,6 +71,7 @@ class AutowareErrorMonitor : public rclcpp::Node bool ignore_missing_diagnostics; bool add_leaf_diagnostics; double data_ready_timeout; + double data_heartbeat_timeout; double diag_timeout_sec; double hazard_recovery_timeout; int emergency_hazard_level; @@ -92,6 +93,7 @@ class AutowareErrorMonitor : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; bool isDataReady(); + bool isDataHeartbeatTimeout(); void onTimer(); // Subscriber @@ -125,6 +127,12 @@ class AutowareErrorMonitor : public rclcpp::Node [[maybe_unused]] std_srvs::srv::Trigger::Request::SharedPtr request, std_srvs::srv::Trigger::Response::SharedPtr response); + // for Heartbeat + rclcpp::Time diag_array_stamp_; + rclcpp::Time autoware_state_stamp_; + rclcpp::Time current_gate_mode_stamp_; + rclcpp::Time control_mode_stamp_; + // Algorithm boost::optional getLatestDiag(const std::string & diag_name) const; uint8_t getHazardLevel(const DiagConfig & required_module, const int diag_level) const; diff --git a/system/system_error_monitor/launch/system_error_monitor.launch.xml b/system/system_error_monitor/launch/system_error_monitor.launch.xml index 76d89c915a8e4..9eb805b0eb2a6 100644 --- a/system/system_error_monitor/launch/system_error_monitor.launch.xml +++ b/system/system_error_monitor/launch/system_error_monitor.launch.xml @@ -3,6 +3,7 @@ + @@ -45,6 +46,7 @@ + diff --git a/system/system_error_monitor/launch/system_error_monitor_node.launch.xml b/system/system_error_monitor/launch/system_error_monitor_node.launch.xml index 161f29f5ee626..6741b4f22ce48 100644 --- a/system/system_error_monitor/launch/system_error_monitor_node.launch.xml +++ b/system/system_error_monitor/launch/system_error_monitor_node.launch.xml @@ -3,6 +3,7 @@ + @@ -23,6 +24,7 @@ + diff --git a/system/system_error_monitor/src/system_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp index 1a0b696bb36f4..9240563741015 100644 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ b/system/system_error_monitor/src/system_error_monitor_core.cpp @@ -234,6 +234,7 @@ AutowareErrorMonitor::AutowareErrorMonitor() get_parameter_or("ignore_missing_diagnostics", params_.ignore_missing_diagnostics, false); get_parameter_or("add_leaf_diagnostics", params_.add_leaf_diagnostics, true); get_parameter_or("data_ready_timeout", params_.data_ready_timeout, 30.0); + get_parameter_or("data_heartbeat_timeout", params_.data_heartbeat_timeout, 1.0); get_parameter_or("diag_timeout_sec", params_.diag_timeout_sec, 1.0); get_parameter_or("hazard_recovery_timeout", params_.hazard_recovery_timeout, 5.0); get_parameter_or( @@ -363,24 +364,36 @@ void AutowareErrorMonitor::onDiagArray( diag_buffer.pop_front(); } } + + // for Heartbeat + diag_array_stamp_ = this->now(); } void AutowareErrorMonitor::onCurrentGateMode( const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg) { current_gate_mode_ = msg; + + // for Heartbeat + current_gate_mode_stamp_ = this->now(); } void AutowareErrorMonitor::onAutowareState( const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr msg) { autoware_state_ = msg; + + // for Heartbeat + autoware_state_stamp_ = this->now(); } void AutowareErrorMonitor::onControlMode( const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) { control_mode_ = msg; + + // for Heartbeat + control_mode_stamp_ = this->now(); } bool AutowareErrorMonitor::isDataReady() @@ -408,6 +421,37 @@ bool AutowareErrorMonitor::isDataReady() return true; } +bool AutowareErrorMonitor::isDataHeartbeatTimeout() +{ + auto isTimeout = [this](const rclcpp::Time & last_stamp, const double threshold) { + const auto time_diff = this->now() - last_stamp; + return time_diff.seconds() > threshold; + }; + + if (isTimeout(diag_array_stamp_, params_.data_heartbeat_timeout)) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "diag_array msg is timeout..."); + return true; + } + + if (isTimeout(current_gate_mode_stamp_, params_.data_heartbeat_timeout)) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "current_gate_mode msg is timeout..."); + return true; + } + + if (isTimeout(autoware_state_stamp_, params_.data_heartbeat_timeout)) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "autoware_state msg is timeout..."); + return true; + } + + if (isTimeout(control_mode_stamp_, params_.data_heartbeat_timeout)) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 5000, "vehicle_state_report msg is timeout..."); + return true; + } + + return false; +} + void AutowareErrorMonitor::onTimer() { if (!isDataReady()) { @@ -420,6 +464,11 @@ void AutowareErrorMonitor::onTimer() return; } + if (isDataHeartbeatTimeout()) { + publishHazardStatus(createTimeoutHazardStatus()); + return; + } + current_mode_ = current_gate_mode_->data == tier4_control_msgs::msg::GateMode::AUTO ? KeyName::autonomous_driving : KeyName::external_control; From de6777ee70fa61f20d60345c3a3344035a71d7c8 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 27 Dec 2022 14:16:05 +0900 Subject: [PATCH 04/92] fix(surround_obstacle_checker): not use boost::assign::list_of (#216) * fix(surround_obstacle_checker): not use boost::assign::list_of * ci(pre-commit): autofix * Update planning/surround_obstacle_checker/src/node.cpp * Update node.cpp * Update node.cpp Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../surround_obstacle_checker/src/node.cpp | 73 ++++++++----------- 1 file changed, 32 insertions(+), 41 deletions(-) diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/surround_obstacle_checker/src/node.cpp index 30cde09775e18..fba6f2d0e99ef 100644 --- a/planning/surround_obstacle_checker/src/node.cpp +++ b/planning/surround_obstacle_checker/src/node.cpp @@ -27,6 +27,8 @@ #include #define EIGEN_MPL2_ONLY +#include "tier4_autoware_utils/geometry/geometry.hpp" + #include #include @@ -388,65 +390,54 @@ Polygon2d SurroundObstacleCheckerNode::createSelfPolygon() const double right = vehicle_info_.min_lateral_offset_m; Polygon2d poly; - boost::geometry::exterior_ring(poly) = boost::assign::list_of(front, left)(front, right)( - rear, right)(rear, left)(front, left); + poly.outer().push_back(Point2d(front, left)); + poly.outer().push_back(Point2d(front, right)); + poly.outer().push_back(Point2d(rear, right)); + poly.outer().push_back(Point2d(rear, left)); + boost::geometry::correct(poly); return poly; } Polygon2d SurroundObstacleCheckerNode::createObjPolygon( const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size) { + auto createPoint32 = [](const double x, const double y, const double z) { + geometry_msgs::msg::Point32 p; + p.x = x; + p.y = y; + p.z = z; + return p; + }; + // rename - const double x = pose.position.x; - const double y = pose.position.y; const double h = size.x; const double w = size.y; - const double yaw = tf2::getYaw(pose.orientation); // create base polygon - Polygon2d obj_poly; - boost::geometry::exterior_ring(obj_poly) = boost::assign::list_of(h / 2.0, w / 2.0)( - -h / 2.0, w / 2.0)(-h / 2.0, -w / 2.0)(h / 2.0, -w / 2.0)(h / 2.0, w / 2.0); - - // rotate polygon(yaw) - boost::geometry::strategy::transform::rotate_transformer - rotate(-yaw); // anti-clockwise -> :clockwise rotation - Polygon2d rotate_obj_poly; - boost::geometry::transform(obj_poly, rotate_obj_poly, rotate); - - // translate polygon(x, y) - boost::geometry::strategy::transform::translate_transformer translate(x, y); - Polygon2d translate_obj_poly; - boost::geometry::transform(rotate_obj_poly, translate_obj_poly, translate); - return translate_obj_poly; + geometry_msgs::msg::Polygon polygon{}; + polygon.points.push_back(createPoint32(h, -w, 0.0)); + polygon.points.push_back(createPoint32(h, w, 0.0)); + polygon.points.push_back(createPoint32(-h, w, 0.0)); + polygon.points.push_back(createPoint32(-h, -w, 0.0)); + + return createObjPolygon(pose, polygon); } Polygon2d SurroundObstacleCheckerNode::createObjPolygon( const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Polygon & footprint) { - // rename - const double x = pose.position.x; - const double y = pose.position.y; - const double yaw = tf2::getYaw(pose.orientation); - - // create base polygon - Polygon2d obj_poly; - for (const auto point : footprint.points) { - const Point2d point2d(point.x, point.y); - obj_poly.outer().push_back(point2d); + geometry_msgs::msg::Polygon transformed_polygon{}; + geometry_msgs::msg::TransformStamped geometry_tf{}; + geometry_tf.transform = tier4_autoware_utils::pose2transform(pose); + tf2::doTransform(footprint, transformed_polygon, geometry_tf); + + Polygon2d object_polygon; + for (const auto & p : transformed_polygon.points) { + object_polygon.outer().push_back(Point2d(p.x, p.y)); } - // rotate polygon(yaw) - boost::geometry::strategy::transform::rotate_transformer - rotate(-yaw); // anti-clockwise -> :clockwise rotation - Polygon2d rotate_obj_poly; - boost::geometry::transform(obj_poly, rotate_obj_poly, rotate); - - // translate polygon(x, y) - boost::geometry::strategy::transform::translate_transformer translate(x, y); - Polygon2d translate_obj_poly; - boost::geometry::transform(rotate_obj_poly, translate_obj_poly, translate); - return translate_obj_poly; + boost::geometry::correct(object_polygon); + return object_polygon; } diagnostic_msgs::msg::DiagnosticStatus SurroundObstacleCheckerNode::makeStopReasonDiag( From a81665246c94f1e52d080fe2e944690190ccd3cf Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 27 Dec 2022 14:16:36 +0900 Subject: [PATCH 05/92] fix(obstacle_stop_planner): add error handling for nullptr of object_ptr (#215) --- planning/obstacle_stop_planner/src/node.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 20a60f9b984ce..05437ebc3b11c 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -575,6 +575,13 @@ void ObstacleStopPlannerNode::pathCallback(const Trajectory::ConstSharedPtr inpu { std::lock_guard lock(mutex_); + if (!object_ptr_) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + "waiting for perception object..."); + return; + } + if (!obstacle_ros_pointcloud_ptr) { RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), From cba06ae9d5fd0aa301e2a31745e1798345bffa0f Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 13 Jan 2023 21:48:58 +0900 Subject: [PATCH 06/92] fix(system_error_monitor): hold emergency when timeout happens (#2634) (#240) * fix(system_error_monitor): hold emergency when timeout * fix: initialize other fault --- .../system_error_monitor_core.hpp | 1 + .../src/system_error_monitor_core.cpp | 49 +++++++++++++------ 2 files changed, 34 insertions(+), 16 deletions(-) diff --git a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp index a170fa8b90742..dc54b25d8e54b 100644 --- a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp +++ b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp @@ -141,6 +141,7 @@ class AutowareErrorMonitor : public rclcpp::Node autoware_auto_system_msgs::msg::HazardStatus * hazard_status) const; autoware_auto_system_msgs::msg::HazardStatus judgeHazardStatus() const; void updateHazardStatus(); + void updateTimeoutHazardStatus(); bool canAutoRecovery() const; bool isEmergencyHoldingRequired() const; }; diff --git a/system/system_error_monitor/src/system_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp index 9240563741015..d6cc771e5dbd2 100644 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ b/system/system_error_monitor/src/system_error_monitor_core.cpp @@ -182,20 +182,6 @@ std::set getErrorModules( return error_modules; } -autoware_auto_system_msgs::msg::HazardStatus createTimeoutHazardStatus() -{ - autoware_auto_system_msgs::msg::HazardStatus hazard_status; - hazard_status.level = autoware_auto_system_msgs::msg::HazardStatus::SINGLE_POINT_FAULT; - hazard_status.emergency = true; - hazard_status.emergency_holding = false; - diagnostic_msgs::msg::DiagnosticStatus diag; - diag.name = "system_error_monitor/input_data_timeout"; - diag.hardware_id = "system_error_monitor"; - diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - hazard_status.diag_single_point_fault.push_back(diag); - return hazard_status; -} - int isInNoFaultCondition( const autoware_auto_system_msgs::msg::AutowareState & autoware_state, const tier4_control_msgs::msg::GateMode & current_gate_mode) @@ -459,13 +445,15 @@ void AutowareErrorMonitor::onTimer() RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "input data is timeout"); - publishHazardStatus(createTimeoutHazardStatus()); + updateTimeoutHazardStatus(); + publishHazardStatus(hazard_status_); } return; } if (isDataHeartbeatTimeout()) { - publishHazardStatus(createTimeoutHazardStatus()); + updateTimeoutHazardStatus(); + publishHazardStatus(hazard_status_); return; } @@ -610,6 +598,35 @@ void AutowareErrorMonitor::updateHazardStatus() } } +void AutowareErrorMonitor::updateTimeoutHazardStatus() +{ + const bool prev_emergency_status = hazard_status_.emergency; + + hazard_status_.level = autoware_auto_system_msgs::msg::HazardStatus::SINGLE_POINT_FAULT; + hazard_status_.emergency = true; + + if (hazard_status_.emergency != prev_emergency_status) { + emergency_state_switch_time_ = this->now(); + } + + hazard_status_.diag_no_fault = std::vector(); + hazard_status_.diag_safe_fault = std::vector(); + hazard_status_.diag_latent_fault = std::vector(); + + diagnostic_msgs::msg::DiagnosticStatus diag; + diag.name = "system_error_monitor/input_data_timeout"; + diag.hardware_id = "system_error_monitor"; + diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + hazard_status_.diag_single_point_fault = + std::vector{diag}; + + // Update emergency_holding condition + if (params_.use_emergency_hold) { + const auto emergency_duration = (this->now() - emergency_state_switch_time_).seconds(); + hazard_status_.emergency_holding = emergency_duration > params_.hazard_recovery_timeout; + } +} + bool AutowareErrorMonitor::canAutoRecovery() const { const auto error_modules = getErrorModules(hazard_status_, params_.emergency_hazard_level); From 6f15b010ef67122c0511061dda3b4a2ed899e5e5 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 27 Feb 2023 18:30:47 +0900 Subject: [PATCH 07/92] fix: pre-commit settings for beta/v0.3.x (#288) fix: pre-commit settings --- .github/workflows/pre-commit.yaml | 16 ++++++++++++++++ .pre-commit-config.yaml | 2 +- 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml index e1b72f706881c..bda722c87ef09 100644 --- a/.github/workflows/pre-commit.yaml +++ b/.github/workflows/pre-commit.yaml @@ -5,12 +5,28 @@ on: jobs: pre-commit: + if: ${{ github.event.repository.private }} # Use pre-commit.ci for public repositories runs-on: ubuntu-latest steps: + - name: Generate token + id: generate-token + uses: tibdex/github-app-token@v1 + with: + app_id: ${{ secrets.APP_ID }} + private_key: ${{ secrets.PRIVATE_KEY }} + - name: Check out repository uses: actions/checkout@v3 + with: + ref: ${{ github.event.pull_request.head.ref }} + + - name: Set git config + uses: autowarefoundation/autoware-github-actions/set-git-config@v1 + with: + token: ${{ steps.generate-token.outputs.token }} - name: Run pre-commit uses: autowarefoundation/autoware-github-actions/pre-commit@v1 with: pre-commit-config: .pre-commit-config.yaml + token: ${{ steps.generate-token.outputs.token }} diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index fa2480a48ee2e..6b74680a99652 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -55,7 +55,7 @@ repos: args: [-w, -s, -i=4] - repo: https://github.com/pycqa/isort - rev: 5.10.1 + rev: 5.12.0 hooks: - id: isort From d64e3c2b67849ecee062d2f387a4590a094c58dd Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 27 Feb 2023 19:17:11 +0900 Subject: [PATCH 08/92] fix: path distance calculator (backport #975, #2892) (#281) * fix: path distance calculator (#975) Signed-off-by: tomoya.kimura * fix(path_distance_calculator): do not calculate distance when too short path is subscribed (#2892) * fix(path_distance_calculator): do not calculate distance when too short path is subscribed Signed-off-by: tomoya.kimura * unify conditions Signed-off-by: tomoya.kimura --------- Signed-off-by: tomoya.kimura --------- Signed-off-by: tomoya.kimura Co-authored-by: Tomoya Kimura --- .../src/path_distance_calculator.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/common/path_distance_calculator/src/path_distance_calculator.cpp b/common/path_distance_calculator/src/path_distance_calculator.cpp index 59b04739c0a63..57acde3f6ae4d 100644 --- a/common/path_distance_calculator/src/path_distance_calculator.cpp +++ b/common/path_distance_calculator/src/path_distance_calculator.cpp @@ -43,8 +43,9 @@ PathDistanceCalculator::PathDistanceCalculator(const rclcpp::NodeOptions & optio RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "no path"); return; } - if (path->points.empty()) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "path empty"); + if (path->points.size() <= 1) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "too short or empty path"); + return; } const double distance = tier4_autoware_utils::calcSignedArcLength( From 632a326635368f35a70c84f18b779ea56262ad1b Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 28 Feb 2023 10:32:08 +0900 Subject: [PATCH 09/92] fix(obstacle_stop_planner): fix for multithread (#231) --- .../include/obstacle_stop_planner/node.hpp | 13 +++-- planning/obstacle_stop_planner/src/node.cpp | 47 +++++++++---------- 2 files changed, 31 insertions(+), 29 deletions(-) diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index e6af015dfd4df..0a4dc99493bc7 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -229,12 +229,14 @@ class ObstacleStopPlannerNode : public rclcpp::Node const TrajectoryPoints & decimate_trajectory, TrajectoryPoints & output, PlannerData & planner_data, const std_msgs::msg::Header & trajectory_header, const VehicleInfo & vehicle_info, const StopParam & stop_param, - const sensor_msgs::msg::PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr); + const sensor_msgs::msg::PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr, + const PredictedObjects::ConstSharedPtr object_ptr, + const nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr); void insertVelocity( TrajectoryPoints & trajectory, PlannerData & planner_data, const std_msgs::msg::Header & trajectory_header, const VehicleInfo & vehicle_info, - const double current_acc, const StopParam & stop_param); + const double current_acc, const double current_vel, const StopParam & stop_param); TrajectoryPoints decimateTrajectory( const TrajectoryPoints & input, const double step_length, std::map & index_map); @@ -284,7 +286,7 @@ class ObstacleStopPlannerNode : public rclcpp::Node SlowDownSection createSlowDownSection( const int idx, const TrajectoryPoints & base_trajectory, const double lateral_deviation, const double dist_remain, const double dist_vehicle_to_obstacle, - const VehicleInfo & vehicle_info, const double current_acc); + const VehicleInfo & vehicle_info, const double current_acc, const double current_vel); SlowDownSection createSlowDownSectionFromMargin( const int idx, const TrajectoryPoints & base_trajectory, const double forward_margin, @@ -299,9 +301,10 @@ class ObstacleStopPlannerNode : public rclcpp::Node void setExternalVelocityLimit(); - void resetExternalVelocityLimit(const double current_acc); + void resetExternalVelocityLimit(const double current_acc, const double current_vel); - void publishDebugData(const PlannerData & planner_data, const double current_acc); + void publishDebugData( + const PlannerData & planner_data, const double current_acc, const double current_vel); }; } // namespace motion_planning diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 05437ebc3b11c..56fc621e0f002 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -570,12 +570,12 @@ void ObstacleStopPlannerNode::pathCallback(const Trajectory::ConstSharedPtr inpu const auto stop_param = stop_param_; const double current_acc = current_acc_; const auto obstacle_ros_pointcloud_ptr = obstacle_ros_pointcloud_ptr_; + const auto object_ptr = object_ptr_; + const auto current_velocity_ptr = current_velocity_ptr_; mutex_.unlock(); { - std::lock_guard lock(mutex_); - - if (!object_ptr_) { + if (!object_ptr) { RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "waiting for perception object..."); @@ -589,7 +589,7 @@ void ObstacleStopPlannerNode::pathCallback(const Trajectory::ConstSharedPtr inpu return; } - if (!current_velocity_ptr_ && node_param_.enable_slow_down) { + if (!current_velocity_ptr && node_param_.enable_slow_down) { RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "waiting for current velocity..."); @@ -601,6 +601,8 @@ void ObstacleStopPlannerNode::pathCallback(const Trajectory::ConstSharedPtr inpu } } + const auto current_vel = current_velocity_ptr->twist.twist.linear.x; + PlannerData planner_data{}; getSelfPose(input_msg->header, tf_buffer_, planner_data.current_pose); @@ -622,30 +624,32 @@ void ObstacleStopPlannerNode::pathCallback(const Trajectory::ConstSharedPtr inpu // search obstacles within slow-down/collision area searchObstacle( decimate_trajectory, output_trajectory_points, planner_data, input_msg->header, vehicle_info, - stop_param, obstacle_ros_pointcloud_ptr); + stop_param, obstacle_ros_pointcloud_ptr, object_ptr, current_velocity_ptr); // insert slow-down-section/stop-point insertVelocity( output_trajectory_points, planner_data, input_msg->header, vehicle_info, current_acc, - stop_param); + current_vel, stop_param); const auto no_slow_down_section = !planner_data.slow_down_require && !latest_slow_down_section_; const auto no_hunting = (rclcpp::Time(input_msg->header.stamp) - last_detection_time_).seconds() > node_param_.hunting_threshold; if (node_param_.enable_slow_down && no_slow_down_section && set_velocity_limit_ && no_hunting) { - resetExternalVelocityLimit(current_acc); + resetExternalVelocityLimit(current_acc, current_vel); } auto trajectory = tier4_autoware_utils::convertToTrajectory(output_trajectory_points); trajectory.header = input_msg->header; path_pub_->publish(trajectory); - publishDebugData(planner_data, current_acc); + publishDebugData(planner_data, current_acc, current_vel); } void ObstacleStopPlannerNode::searchObstacle( const TrajectoryPoints & decimate_trajectory, TrajectoryPoints & output, PlannerData & planner_data, const std_msgs::msg::Header & trajectory_header, const VehicleInfo & vehicle_info, const StopParam & stop_param, - const sensor_msgs::msg::PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr) + const sensor_msgs::msg::PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr, + const PredictedObjects::ConstSharedPtr object_ptr, + const nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr) { // search candidate obstacle pointcloud pcl::PointCloud::Ptr slow_down_pointcloud_ptr(new pcl::PointCloud); @@ -734,7 +738,7 @@ void ObstacleStopPlannerNode::searchObstacle( acc_controller_->insertAdaptiveCruiseVelocity( decimate_trajectory, planner_data.decimate_trajectory_collision_index, planner_data.current_pose, planner_data.nearest_collision_point, - planner_data.nearest_collision_point_time, object_ptr_, current_velocity_ptr_, + planner_data.nearest_collision_point_time, object_ptr, current_velocity_ptr, &planner_data.stop_require, &output, trajectory_header); break; @@ -746,7 +750,7 @@ void ObstacleStopPlannerNode::searchObstacle( void ObstacleStopPlannerNode::insertVelocity( TrajectoryPoints & output, PlannerData & planner_data, const std_msgs::msg::Header & trajectory_header, const VehicleInfo & vehicle_info, - const double current_acc, const StopParam & stop_param) + const double current_acc, const double current_vel, const StopParam & stop_param) { if (planner_data.stop_require) { // insert stop point @@ -787,7 +791,8 @@ void ObstacleStopPlannerNode::insertVelocity( dist_baselink_to_obstacle + index_with_dist_remain.get().second); const auto slow_down_section = createSlowDownSection( index_with_dist_remain.get().first, output, planner_data.lateral_deviation, - index_with_dist_remain.get().second, dist_baselink_to_obstacle, vehicle_info, current_acc); + index_with_dist_remain.get().second, dist_baselink_to_obstacle, vehicle_info, current_acc, + current_vel); if ( !latest_slow_down_section_ && @@ -965,15 +970,9 @@ StopPoint ObstacleStopPlannerNode::createTargetPoint( SlowDownSection ObstacleStopPlannerNode::createSlowDownSection( const int idx, const TrajectoryPoints & base_trajectory, const double lateral_deviation, const double dist_remain, const double dist_baselink_to_obstacle, - const VehicleInfo & vehicle_info, const double current_acc) + const VehicleInfo & vehicle_info, const double current_acc, const double current_vel) { - if (!current_velocity_ptr_) { - // TODO(Satoshi Ota) - return SlowDownSection{}; - } - if (slow_down_param_.consider_constraints) { - const auto & current_vel = current_velocity_ptr_->twist.twist.linear.x; const auto margin_with_vel = calcFeasibleMarginAndVelocity( slow_down_param_, dist_baselink_to_obstacle + dist_remain, current_vel, current_acc); @@ -985,7 +984,7 @@ SlowDownSection ObstacleStopPlannerNode::createSlowDownSection( const auto no_need_velocity_limit = dist_baselink_to_obstacle + dist_remain > slow_down_param_.forward_margin; if (set_velocity_limit_ && no_need_velocity_limit) { - resetExternalVelocityLimit(current_acc); + resetExternalVelocityLimit(current_acc, current_vel); } const auto use_velocity_limit = relax_target_vel || set_velocity_limit_; @@ -1114,6 +1113,7 @@ void ObstacleStopPlannerNode::insertSlowDownSection( void ObstacleStopPlannerNode::dynamicObjectCallback( const PredictedObjects::ConstSharedPtr input_msg) { + std::lock_guard lock(mutex_); object_ptr_ = input_msg; } @@ -1467,9 +1467,9 @@ void ObstacleStopPlannerNode::setExternalVelocityLimit() slow_down_limit_vel->constraints.min_jerk, slow_down_limit_vel->constraints.min_acceleration); } -void ObstacleStopPlannerNode::resetExternalVelocityLimit(const double current_acc) +void ObstacleStopPlannerNode::resetExternalVelocityLimit( + const double current_acc, const double current_vel) { - const auto current_vel = current_velocity_ptr_->twist.twist.linear.x; const auto reach_target_vel = current_vel < slow_down_param_.slow_down_vel + slow_down_param_.vel_threshold_reset_velocity_limit_; @@ -1493,9 +1493,8 @@ void ObstacleStopPlannerNode::resetExternalVelocityLimit(const double current_ac } void ObstacleStopPlannerNode::publishDebugData( - const PlannerData & planner_data, const double current_acc) + const PlannerData & planner_data, const double current_acc, const double current_vel) { - const auto & current_vel = current_velocity_ptr_->twist.twist.linear.x; debug_ptr_->setDebugValues(DebugValues::TYPE::CURRENT_VEL, current_vel); debug_ptr_->setDebugValues(DebugValues::TYPE::CURRENT_ACC, current_acc); debug_ptr_->setDebugValues( From a74f2546ad02e3b206e55c34ebd85e7dbf034812 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 28 Feb 2023 13:15:29 +0900 Subject: [PATCH 10/92] fix(obstacle_avoidance_planner): add guard for too large yaw deviation (backport #2579) (#289) fix(obstacle_avoidance_planner): add guard for too large yaw deviation (#2579) Signed-off-by: Takayuki Murooka Co-authored-by: Takayuki Murooka --- .../obstacle_avoidance_planner/common_structs.hpp | 1 + .../obstacle_avoidance_planner/src/mpt_optimizer.cpp | 10 ++++++++++ 2 files changed, 11 insertions(+) diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp index da7f284085ef6..d293ebc72e2dc 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp @@ -160,6 +160,7 @@ struct DebugData std::vector mpt_ref_poses; std::vector lateral_errors; + std::vector yaw_errors; std::vector eb_traj; std::vector mpt_fixed_traj; diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 818bca69702e2..6c8643ef3785d 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -292,6 +292,15 @@ boost::optional MPTOptimizer::getModelPredictiveTrajecto return boost::none; } } + constexpr double max_yaw_deviation = 50.0 / 180 * 3.14; + for (const double yaw_error : debug_data_ptr->yaw_errors) { + if (max_yaw_deviation < std::abs(yaw_error)) { + RCLCPP_ERROR( + rclcpp::get_logger("mpt_optimizer"), + "return boost::none since yaw deviation is too large."); + return boost::none; + } + } auto full_optimized_ref_points = fixed_ref_points; full_optimized_ref_points.insert( @@ -1175,6 +1184,7 @@ std::vector MPTOptimizer::get ref_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_point.yaw); debug_data_ptr->mpt_ref_poses.push_back(ref_pose); debug_data_ptr->lateral_errors.push_back(lat_error); + debug_data_ptr->yaw_errors.push_back(yaw_error); ref_point.optimized_kinematic_state << lat_error_vec.at(i), yaw_error_vec.at(i); if (i >= fixed_ref_points.size()) { From 2cd904701b669f3eedb87469600f952c1028fa6c Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 28 Feb 2023 14:27:23 +0900 Subject: [PATCH 11/92] ci: fix action for build-test-differential and check-build-depends for beta/v0.3.x (#291) * tmp: change colcon-build action * test commit * fix OS version * Revert "fix OS version" This reverts commit c56a2304a289ee371f193d51045110cb0798dd0f. * change container version * fix tier4_autoware_msgs repos * fix * use commit hash * fix for clang-tidy * Update common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp --- .github/workflows/build-and-test-differential.yaml | 11 +++++++---- .github/workflows/check-build-depends.yaml | 5 +++-- build_depends.repos | 4 ++-- 3 files changed, 12 insertions(+), 8 deletions(-) diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index b77573e0966e0..9939b469151ea 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -6,7 +6,7 @@ on: jobs: build-and-test-differential: runs-on: ubuntu-latest - container: ghcr.io/autowarefoundation/autoware-universe:latest + container: ghcr.io/autowarefoundation/autoware-universe:galactic-latest steps: - name: Cancel previous runs uses: styfle/cancel-workflow-action@0.9.1 @@ -25,20 +25,22 @@ jobs: - name: Build if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} - uses: autowarefoundation/autoware-github-actions/colcon-build@v1 + uses: autowarefoundation/autoware-github-actions/colcon-build@a7cc2c1ce6052f395e5800a0fb6e6221421bf30a with: rosdistro: galactic target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} build-depends-repos: build_depends.repos + include-eol-distros: true - name: Test id: test if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} - uses: autowarefoundation/autoware-github-actions/colcon-test@v1 + uses: autowarefoundation/autoware-github-actions/colcon-test@a7cc2c1ce6052f395e5800a0fb6e6221421bf30a with: rosdistro: galactic target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} build-depends-repos: build_depends.repos + include-eol-distros: true - name: Upload coverage to CodeCov if: ${{ steps.test.outputs.coverage-report-files != '' }} @@ -68,9 +70,10 @@ jobs: - name: Run clang-tidy if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} - uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 + uses: autowarefoundation/autoware-github-actions/clang-tidy@a7cc2c1ce6052f395e5800a0fb6e6221421bf30a with: rosdistro: galactic target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy build-depends-repos: build_depends.repos + include-eol-distros: true diff --git a/.github/workflows/check-build-depends.yaml b/.github/workflows/check-build-depends.yaml index fbea900cde9a7..0894e2aabe52e 100644 --- a/.github/workflows/check-build-depends.yaml +++ b/.github/workflows/check-build-depends.yaml @@ -8,7 +8,7 @@ on: jobs: check-build-depends: runs-on: ubuntu-latest - container: ghcr.io/autowarefoundation/autoware-universe:latest + container: ghcr.io/autowarefoundation/autoware-universe:galactic-latest steps: - name: Cancel previous runs uses: styfle/cancel-workflow-action@0.9.1 @@ -24,8 +24,9 @@ jobs: uses: autowarefoundation/autoware-github-actions/get-self-packages@v1 - name: Build - uses: autowarefoundation/autoware-github-actions/colcon-build@v1 + uses: autowarefoundation/autoware-github-actions/colcon-build@a7cc2c1ce6052f395e5800a0fb6e6221421bf30a with: rosdistro: galactic target-packages: ${{ steps.get-self-packages.outputs.self-packages }} build-depends-repos: build_depends.repos + include-eol-distros: true diff --git a/build_depends.repos b/build_depends.repos index fa60d2107015e..876d011d1b628 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -15,8 +15,8 @@ repositories: # universe universe/tier4_autoware_msgs: type: git - url: https://github.com/tier4/AutowareArchitectureProposal_msgs.git # TODO(Tier IV): Rename to tier4/tier4_autoware_msgs - version: tier4/universe + url: https://github.com/tier4/tier4_autoware_msgs.git + version: a624d255107fa724cf90967c5dcc09463b1d1c73 universe/vendor/grid_map: type: git url: https://github.com/ANYbotics/grid_map.git From 6b2bdd7229956ee384e218b9f838af590bea3683 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 28 Feb 2023 17:04:39 +0900 Subject: [PATCH 12/92] chore: pin commit hash for autoware_common in build_depends.repos (#293) Update build_depends.repos --- build_depends.repos | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/build_depends.repos b/build_depends.repos index 876d011d1b628..ad92967c0e48b 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -3,11 +3,11 @@ repositories: core/autoware_msgs: type: git url: https://github.com/tier4/autoware_auto_msgs.git # TODO(Tier IV): Move to autowarefoundation/autoware_msgs - version: tier4/main + version: c8a5d2001da052137c08cdf9e8d75c1975f99b84 core/common: type: git url: https://github.com/autowarefoundation/autoware_common.git - version: main + version: d95da5d4c3f262259c6e40a0bdc1399c0fdf612f core/autoware: type: git url: https://github.com/autowarefoundation/autoware.core.git From 9e5d81b5f00ec0b8f99ac16e67da533bd7359467 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 28 Feb 2023 17:06:06 +0900 Subject: [PATCH 13/92] fix(obstacle_avoidance_planner): guard trajectory size (#292) Signed-off-by: Takayuki Murooka --- planning/obstacle_avoidance_planner/src/node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index d3f26893b4241..eb75fcbee9006 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -1412,7 +1412,7 @@ ObstacleAvoidancePlanner::alignVelocity( -> std::pair< std::vector, boost::optional> { const auto opt_path_zero_vel_idx = tier4_autoware_utils::searchZeroVelocityIndex(path_points); - if (opt_path_zero_vel_idx) { + if (opt_path_zero_vel_idx && 1 < fine_traj_points.size()) { const auto & zero_vel_path_point = path_points.at(opt_path_zero_vel_idx.get()); const auto opt_traj_seg_idx = tier4_autoware_utils::findNearestSegmentIndex( fine_traj_points, zero_vel_path_point.pose, std::numeric_limits::max(), From ab099546d9ff0f8d03eeea1a9e5dec6d3c52a03b Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 28 Feb 2023 17:07:56 +0900 Subject: [PATCH 14/92] fix(tier4_autoware_utils): exception handling in tier4 autoware utils for beta/v0.3.x (backport #1268) (#282) fix(tier4_autoware_utils): exception handling in tier4 autoware utils for beta/v0.3.x --- .../trajectory/trajectory.hpp | 165 +++++++++++++++--- .../src/longitudinal_controller_utils.cpp | 4 + 2 files changed, 144 insertions(+), 25 deletions(-) diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp index f7b17654f2c86..0b7a6d05d7ec3 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp @@ -23,6 +23,38 @@ #include #include #include +#include + +namespace +{ +template +std::vector removeOverlapPoints(const T & points, const size_t & idx) +{ + std::vector dst; + + for (const auto & pt : points) { + dst.push_back(tier4_autoware_utils::getPoint(pt)); + } + + if (points.empty()) { + return dst; + } + + constexpr double eps = 1.0E-08; + size_t i = idx; + while (i != dst.size() - 1) { + const auto p = tier4_autoware_utils::getPoint(dst.at(i)); + const auto p_next = tier4_autoware_utils::getPoint(dst.at(i + 1)); + const Eigen::Vector3d v{p_next.x - p.x, p_next.y - p.y, 0.0}; + if (v.norm() < eps) { + dst.erase(dst.begin() + i + 1); + } else { + ++i; + } + } + return dst; +} +} // namespace namespace tier4_autoware_utils { @@ -38,7 +70,12 @@ template boost::optional searchZeroVelocityIndex( const T & points_with_twist, const size_t src_idx, const size_t dst_idx) { - validateNonEmpty(points_with_twist); + try { + validateNonEmpty(points_with_twist); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return {}; + } constexpr double epsilon = 1e-3; for (size_t i = src_idx; i < dst_idx; ++i) { @@ -80,7 +117,12 @@ boost::optional findNearestIndex( const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()) { - validateNonEmpty(points); + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return {}; + } const double max_squared_dist = max_dist * max_dist; @@ -120,20 +162,46 @@ boost::optional findNearestIndex( */ template double calcLongitudinalOffsetToSegment( - const T & points, const size_t seg_idx, const geometry_msgs::msg::Point & p_target) + const T & points, const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + const bool throw_exception = false) { - validateNonEmpty(points); + if (seg_idx >= points.size() - 1) { + const std::out_of_range e("Segment index is invalid."); + if (throw_exception) { + throw e; + } + std::cerr << e.what() << std::endl; + return std::nan(""); + } - const auto p_front = getPoint(points.at(seg_idx)); - const auto p_back = getPoint(points.at(seg_idx + 1)); + const auto overlap_removed_points = removeOverlapPoints(points, seg_idx); - const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0}; - const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0}; + if (throw_exception) { + validateNonEmpty(overlap_removed_points); + } else { + try { + validateNonEmpty(overlap_removed_points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return std::nan(""); + } + } - if (segment_vec.norm() == 0.0) { - throw std::runtime_error("Same points are given."); + if (seg_idx >= overlap_removed_points.size() - 1) { + const std::runtime_error e("Same points are given."); + if (throw_exception) { + throw e; + } + std::cerr << e.what() << std::endl; + return std::nan(""); } + const auto p_front = getPoint(overlap_removed_points.at(seg_idx)); + const auto p_back = getPoint(overlap_removed_points.at(seg_idx + 1)); + + const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0}; + const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0}; + return segment_vec.dot(target_vec) / segment_vec.norm(); } @@ -212,22 +280,39 @@ boost::optional findNearestSegmentIndex( * @return length (unsigned) */ template -double calcLateralOffset(const T & points, const geometry_msgs::msg::Point & p_target) +double calcLateralOffset( + const T & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false) { - validateNonEmpty(points); + const auto overlap_removed_points = removeOverlapPoints(points, 0); + + if (throw_exception) { + validateNonEmpty(overlap_removed_points); + } else { + try { + validateNonEmpty(overlap_removed_points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return std::nan(""); + } + } + + if (overlap_removed_points.size() == 1) { + const std::runtime_error e("Same points are given."); + if (throw_exception) { + throw e; + } + std::cerr << e.what() << std::endl; + return std::nan(""); + } - const size_t seg_idx = findNearestSegmentIndex(points, p_target); + const size_t seg_idx = findNearestSegmentIndex(overlap_removed_points, p_target); - const auto p_front = getPoint(points.at(seg_idx)); - const auto p_back = getPoint(points.at(seg_idx + 1)); + const auto p_front = getPoint(overlap_removed_points.at(seg_idx)); + const auto p_back = getPoint(overlap_removed_points.at(seg_idx + 1)); const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0}; const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0}; - if (segment_vec.norm() == 0.0) { - throw std::runtime_error("Same points are given."); - } - const Eigen::Vector3d cross_vec = segment_vec.cross(target_vec); return cross_vec(2) / segment_vec.norm(); } @@ -238,7 +323,12 @@ double calcLateralOffset(const T & points, const geometry_msgs::msg::Point & p_t template double calcSignedArcLength(const T & points, const size_t src_idx, const size_t dst_idx) { - validateNonEmpty(points); + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return 0.0; + } if (src_idx > dst_idx) { return -calcSignedArcLength(points, dst_idx, src_idx); @@ -258,7 +348,12 @@ template double calcSignedArcLength( const T & points, const geometry_msgs::msg::Point & src_point, const size_t & dst_idx) { - validateNonEmpty(points); + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return 0.0; + } const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); @@ -276,7 +371,12 @@ template double calcSignedArcLength( const T & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point) { - validateNonEmpty(points); + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return 0.0; + } return -calcSignedArcLength(points, dst_point, src_idx); } @@ -289,7 +389,12 @@ double calcSignedArcLength( const T & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point) { - validateNonEmpty(points); + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return 0.0; + } const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); const size_t dst_seg_idx = findNearestSegmentIndex(points, dst_point); @@ -313,7 +418,12 @@ boost::optional calcSignedArcLength( const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()) { - validateNonEmpty(points); + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return {}; + } const auto src_seg_idx = findNearestSegmentIndex(points, src_pose, max_dist, max_yaw); if (!src_seg_idx) { @@ -337,7 +447,12 @@ boost::optional calcSignedArcLength( template double calcArcLength(const T & points) { - validateNonEmpty(points); + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return 0.0; + } return calcSignedArcLength(points, 0, points.size() - 1); } diff --git a/control/trajectory_follower/src/longitudinal_controller_utils.cpp b/control/trajectory_follower/src/longitudinal_controller_utils.cpp index 3953cade16a7c..54cb05d3448d1 100644 --- a/control/trajectory_follower/src/longitudinal_controller_utils.cpp +++ b/control/trajectory_follower/src/longitudinal_controller_utils.cpp @@ -73,6 +73,10 @@ float64_t calcStopDistance( const float64_t signed_length_src_offset = tier4_autoware_utils::calcLongitudinalOffsetToSegment( traj.points, *seg_idx, current_pose.position); + if (std::isnan(signed_length_src_offset)) { + return 0.0; + } + // If no zero velocity point, return the length between current_pose to the end of trajectory. if (!stop_idx_opt) { float64_t signed_length_on_traj = From e9ebd0950cc700ec489d9be9b4dd1ef34af4c78a Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 28 Feb 2023 18:34:47 +0900 Subject: [PATCH 15/92] fix(dummy_perception_publisher): independent of pointcloud from detection_successful_rate (backport #527, #926, #1166) (#290) * feat(dummy_perception_publisher): publish realistic dummy pointcloud using raymarchig (#527) * Create pointcloud by raycasting from vehicle Signed-off-by: Hirokazu Ishida * [after-review] Use vector of ObjectInfo Signed-off-by: Hirokazu Ishida * [after-review] Implemented by strategy pattern Signed-off-by: Hirokazu Ishida * [after-review] split files Signed-off-by: Hirokazu Ishida * Use pcl raytracing Tmp Tmp Tmp Signed-off-by: Hirokazu Ishida * Add signed distance function lib Signed-off-by: Hirokazu Ishida * Use sdf library Signed-off-by: Hirokazu Ishida * Remove no longer used functions Signed-off-by: Hirokazu Ishida * Refactor Signed-off-by: Hirokazu Ishida * Simplify getPoint Signed-off-by: Hirokazu Ishida * Raytracing considering inter object relation Signed-off-by: Hirokazu Ishida * Add random noise Signed-off-by: Hirokazu Ishida * Default is object centric Signed-off-by: Hirokazu Ishida * Return if no objects are detected Signed-off-by: Hirokazu Ishida * Change definition of tf_global_to_local (same as other autoware codes) Signed-off-by: Hirokazu Ishida * Remove create method Signed-off-by: Hirokazu Ishida * Reaname: VehicleCentric -> EgoCentric Signed-off-by: Hirokazu Ishida * Refactor a bit Signed-off-by: Hirokazu Ishida * Tune parameter Signed-off-by: Hirokazu Ishida * Fix: Even if selected_idices is zero, pointclouds must be published Signed-off-by: Hirokazu Ishida * Fix launch file Signed-off-by: Hirokazu Ishida * Fix typo Signed-off-by: Hirokazu Ishida * Fix: create merged pointcloud when no idx is selected Signed-off-by: Hirokazu Ishida * Use ray-maching by default Signed-off-by: Hirokazu Ishida * perf(dummy_perception_publisher): tune ego-centric pointcloud generation of dummy perception publisher (#926) * Take advantage of visible range Signed-off-by: Hirokazu Ishida * Tune Signed-off-by: Hirokazu Ishida * Fix: typo Signed-off-by: Hirokazu Ishida * Use hypot * fix(dummy_perception_publisher): independent of pointcloud from detection_successful_rate (#1166) * fix(dummy_perception_publisher): independent of pointcloud from detection_success_rate Signed-off-by: Shumpei Wakabayashi * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --------- Signed-off-by: Hirokazu Ishida Co-authored-by: Hirokazu Ishida <38597814+HiroIshida@users.noreply.github.com> Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../dummy_perception_publisher/CMakeLists.txt | 20 +- .../dummy_perception_publisher/node.hpp | 69 +++- .../signed_distance_function.hpp | 74 ++++ .../dummy_perception_publisher.launch.xml | 1 + .../dummy_perception_publisher/src/node.cpp | 336 ++++++------------ .../src/pointcloud_creator.cpp | 236 ++++++++++++ .../src/signed_distance_function.cpp | 91 +++++ .../src/test_signed_distance_function.cpp | 84 +++++ 8 files changed, 683 insertions(+), 228 deletions(-) create mode 100644 simulator/dummy_perception_publisher/include/dummy_perception_publisher/signed_distance_function.hpp create mode 100644 simulator/dummy_perception_publisher/src/pointcloud_creator.cpp create mode 100644 simulator/dummy_perception_publisher/src/signed_distance_function.cpp create mode 100644 simulator/dummy_perception_publisher/test/src/test_signed_distance_function.cpp diff --git a/simulator/dummy_perception_publisher/CMakeLists.txt b/simulator/dummy_perception_publisher/CMakeLists.txt index 98b288750a059..bd237fecece1d 100644 --- a/simulator/dummy_perception_publisher/CMakeLists.txt +++ b/simulator/dummy_perception_publisher/CMakeLists.txt @@ -12,7 +12,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() # Dependencies for messages -find_package(ament_cmake REQUIRED) +find_package(ament_cmake_auto REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(autoware_auto_perception_msgs REQUIRED) find_package(tier4_perception_msgs REQUIRED) @@ -43,9 +43,18 @@ set(${PROJECT_NAME}_DEPENDENCIES tf2_ros ) +ament_auto_add_library(signed_distance_function SHARED + src/signed_distance_function.cpp +) + ament_auto_add_executable(dummy_perception_publisher_node src/main.cpp src/node.cpp + src/pointcloud_creator.cpp +) + +target_link_libraries(dummy_perception_publisher_node + signed_distance_function ) ament_target_dependencies(dummy_perception_publisher_node ${${PROJECT_NAME}_DEPENDENCIES}) @@ -79,6 +88,15 @@ ament_auto_add_executable(empty_objects_publisher if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(signed_distance_function-test + test/src/test_signed_distance_function.cpp + ) + target_link_libraries(signed_distance_function-test + signed_distance_function + ) endif() ament_auto_package( diff --git a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp index ccbbf2308a310..eef3eb92f7314 100644 --- a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp +++ b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp @@ -33,9 +33,69 @@ #include #include +#include #include #include +struct ObjectInfo +{ + ObjectInfo( + const dummy_perception_publisher::msg::Object & object, const rclcpp::Time & current_time); + double length; + double width; + double height; + double std_dev_x; + double std_dev_y; + double std_dev_z; + double std_dev_yaw; + tf2::Transform tf_map2moved_object; +}; + +class PointCloudCreator +{ +public: + virtual ~PointCloudCreator() {} + + virtual std::vector::Ptr> create_pointclouds( + const std::vector & obj_infos, const tf2::Transform & tf_base_link2map, + std::mt19937 & random_generator, + pcl::PointCloud::Ptr & merged_pointcloud) const = 0; +}; + +class ObjectCentricPointCloudCreator : public PointCloudCreator +{ +public: + explicit ObjectCentricPointCloudCreator(bool enable_ray_tracing) + : enable_ray_tracing_(enable_ray_tracing) + { + } + + std::vector::Ptr> create_pointclouds( + const std::vector & obj_infos, const tf2::Transform & tf_base_link2map, + std::mt19937 & random_generator, + pcl::PointCloud::Ptr & merged_pointcloud) const override; + +private: + void create_object_pointcloud( + const ObjectInfo & obj_info, const tf2::Transform & tf_base_link2map, + std::mt19937 & random_generator, pcl::PointCloud::Ptr pointcloud) const; + + bool enable_ray_tracing_; +}; + +class EgoCentricPointCloudCreator : public PointCloudCreator +{ +public: + explicit EgoCentricPointCloudCreator(double visible_range) : visible_range_(visible_range) {} + std::vector::Ptr> create_pointclouds( + const std::vector & obj_infos, const tf2::Transform & tf_base_link2map, + std::mt19937 & random_generator, + pcl::PointCloud::Ptr & merged_pointcloud) const override; + +private: + double visible_range_; +}; + class DummyPerceptionPublisherNode : public rclcpp::Node { private: @@ -52,13 +112,12 @@ class DummyPerceptionPublisherNode : public rclcpp::Node bool enable_ray_tracing_; bool use_object_recognition_; bool use_real_param_; + std::unique_ptr pointcloud_creator_; + + double angle_increment_; + std::mt19937 random_generator_; void timerCallback(); - void createObjectPointcloud( - const double length, const double width, const double height, const double std_dev_x, - const double std_dev_y, const double std_dev_z, - const tf2::Transform & tf_base_link2moved_object, - pcl::PointCloud::Ptr & pointcloud_ptr); void objectCallback(const dummy_perception_publisher::msg::Object::ConstSharedPtr msg); public: diff --git a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/signed_distance_function.hpp b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/signed_distance_function.hpp new file mode 100644 index 0000000000000..abf24cb8dd4e6 --- /dev/null +++ b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/signed_distance_function.hpp @@ -0,0 +1,74 @@ +// Copyright 2022 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 DUMMY_PERCEPTION_PUBLISHER__SIGNED_DISTANCE_FUNCTION_HPP_ +#define DUMMY_PERCEPTION_PUBLISHER__SIGNED_DISTANCE_FUNCTION_HPP_ + +#include + +#include +#include +#include +#include +#include + +namespace signed_distance_function +{ + +class AbstractSignedDistanceFunction +{ +public: + virtual double operator()(double x, double y) const = 0; + double getSphereTracingDist( + double x_start, double y_start, double angle, + double max_dist = std::numeric_limits::infinity(), double eps = 1e-2) const; + virtual ~AbstractSignedDistanceFunction() {} +}; + +class BoxSDF : public AbstractSignedDistanceFunction +{ +public: + BoxSDF(double length, double width, tf2::Transform tf_global_to_local) + : length_(length), width_(width), tf_local_to_global_(tf_global_to_local.inverse()) + { + } + double operator()(double x, double y) const override; + +private: + double length_; + double width_; + tf2::Transform tf_local_to_global_; +}; + +class CompositeSDF : public AbstractSignedDistanceFunction +{ +public: + explicit CompositeSDF(std::vector> sdf_ptrs) + : sdf_ptrs_(std::move(sdf_ptrs)) + { + if (sdf_ptrs_.empty()) { + throw std::runtime_error("sdf_ptrs must not be empty"); + } + } + double operator()(double x, double y) const override; + + size_t nearest_sdf_index(double x, double y) const; + +private: + std::vector> sdf_ptrs_; +}; + +} // namespace signed_distance_function + +#endif // DUMMY_PERCEPTION_PUBLISHER__SIGNED_DISTANCE_FUNCTION_HPP_ diff --git a/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml b/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml index 3d013a0ab582a..0d17feffed21f 100644 --- a/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml +++ b/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml @@ -18,6 +18,7 @@ + diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index cf4caf61d01e8..87a38ffadb7f6 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/simulator/dummy_perception_publisher/src/node.cpp @@ -16,6 +16,8 @@ #include #include +#include +#include #include #include @@ -25,6 +27,34 @@ #include #include +ObjectInfo::ObjectInfo( + const dummy_perception_publisher::msg::Object & object, const rclcpp::Time & current_time) +: length(object.shape.dimensions.x), + width(object.shape.dimensions.y), + height(object.shape.dimensions.z), + std_dev_x(std::sqrt(object.initial_state.pose_covariance.covariance[0])), + std_dev_y(std::sqrt(object.initial_state.pose_covariance.covariance[7])), + std_dev_z(std::sqrt(object.initial_state.pose_covariance.covariance[14])), + std_dev_yaw(std::sqrt(object.initial_state.pose_covariance.covariance[35])) +{ + const double move_distance = + object.initial_state.twist_covariance.twist.linear.x * + (current_time.seconds() - rclcpp::Time(object.header.stamp).seconds()); + tf2::Transform tf_object_origin2moved_object; + tf2::Transform tf_map2object_origin; + { + geometry_msgs::msg::Transform ros_object_origin2moved_object; + ros_object_origin2moved_object.translation.x = move_distance; + ros_object_origin2moved_object.rotation.x = 0; + ros_object_origin2moved_object.rotation.y = 0; + ros_object_origin2moved_object.rotation.z = 0; + ros_object_origin2moved_object.rotation.w = 1; + tf2::fromMsg(ros_object_origin2moved_object, tf_object_origin2moved_object); + } + tf2::fromMsg(object.initial_state.pose_covariance.pose, tf_map2object_origin); + this->tf_map2moved_object = tf_map2object_origin * tf_object_origin2moved_object; +} + DummyPerceptionPublisherNode::DummyPerceptionPublisherNode() : Node("dummy_perception_publisher"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { @@ -33,6 +63,20 @@ DummyPerceptionPublisherNode::DummyPerceptionPublisherNode() enable_ray_tracing_ = this->declare_parameter("enable_ray_tracing", true); use_object_recognition_ = this->declare_parameter("use_object_recognition", true); + const bool object_centric_pointcloud = + this->declare_parameter("object_centric_pointcloud", false); + + if (object_centric_pointcloud) { + pointcloud_creator_ = + std::unique_ptr(new ObjectCentricPointCloudCreator(enable_ray_tracing_)); + } else { + pointcloud_creator_ = + std::unique_ptr(new EgoCentricPointCloudCreator(visible_range_)); + } + + // parameters for vehicle centric point cloud generation + angle_increment_ = this->declare_parameter("angle_increment", 0.25 * M_PI / 180.0); + std::random_device seed_gen; random_generator_.seed(seed_gen()); @@ -84,128 +128,86 @@ void DummyPerceptionPublisherNode::timerCallback() return; } - std::vector::Ptr> v_pointcloud; - std::vector delete_idxs; + std::vector selected_indices{}; + std::vector obj_infos; static std::uniform_real_distribution<> detection_successful_random(0.0, 1.0); for (size_t i = 0; i < objects_.size(); ++i) { - if (detection_successful_rate_ < detection_successful_random(random_generator_)) { - continue; - } - const double std_dev_x = std::sqrt(objects_.at(i).initial_state.pose_covariance.covariance[0]); - const double std_dev_y = std::sqrt(objects_.at(i).initial_state.pose_covariance.covariance[7]); - const double std_dev_z = std::sqrt(objects_.at(i).initial_state.pose_covariance.covariance[14]); - const double std_dev_yaw = - std::sqrt(objects_.at(i).initial_state.pose_covariance.covariance[35]); - const double move_distance = - objects_.at(i).initial_state.twist_covariance.twist.linear.x * - (current_time.seconds() - rclcpp::Time(objects_.at(i).header.stamp).seconds()); - tf2::Transform tf_object_origin2moved_object; - tf2::Transform tf_map2object_origin; - tf2::Transform tf_map2moved_object; - { - geometry_msgs::msg::Transform ros_object_origin2moved_object; - ros_object_origin2moved_object.translation.x = move_distance; - ros_object_origin2moved_object.rotation.x = 0; - ros_object_origin2moved_object.rotation.y = 0; - ros_object_origin2moved_object.rotation.z = 0; - ros_object_origin2moved_object.rotation.w = 1; - tf2::fromMsg(ros_object_origin2moved_object, tf_object_origin2moved_object); - } - tf2::fromMsg(objects_.at(i).initial_state.pose_covariance.pose, tf_map2object_origin); - tf_map2moved_object = tf_map2object_origin * tf_object_origin2moved_object; - tf2::toMsg(tf_map2moved_object, output_moved_object_pose.pose); - - // pointcloud - pcl::PointCloud::Ptr pointcloud_ptr(new pcl::PointCloud); - createObjectPointcloud( - objects_.at(i).shape.dimensions.x, objects_.at(i).shape.dimensions.y, - objects_.at(i).shape.dimensions.z, std_dev_x, std_dev_y, std_dev_z, - tf_base_link2map * tf_map2moved_object, pointcloud_ptr); - v_pointcloud.push_back(pointcloud_ptr); - - // dynamic object - std::normal_distribution<> x_random(0.0, std_dev_x); - std::normal_distribution<> y_random(0.0, std_dev_y); - std::normal_distribution<> yaw_random(0.0, std_dev_yaw); - tf2::Quaternion noised_quat; - noised_quat.setRPY(0, 0, yaw_random(random_generator_)); - tf2::Transform tf_moved_object2noised_moved_object( - noised_quat, tf2::Vector3(x_random(random_generator_), y_random(random_generator_), 0.0)); - tf2::Transform tf_base_link2noised_moved_object; - tf_base_link2noised_moved_object = - tf_base_link2map * tf_map2moved_object * tf_moved_object2noised_moved_object; - tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object; - feature_object.object.classification.push_back(objects_.at(i).classification); - feature_object.object.kinematics.pose_with_covariance = - objects_.at(i).initial_state.pose_covariance; - feature_object.object.kinematics.twist_with_covariance = - objects_.at(i).initial_state.twist_covariance; - feature_object.object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; - feature_object.object.kinematics.has_twist = false; - tf2::toMsg( - tf_base_link2noised_moved_object, feature_object.object.kinematics.pose_with_covariance.pose); - feature_object.object.shape = objects_.at(i).shape; - pcl::toROSMsg(*pointcloud_ptr, feature_object.feature.cluster); - output_dynamic_object_msg.feature_objects.push_back(feature_object); - - // check delete idx - tf2::Transform tf_base_link2moved_object; - tf_base_link2moved_object = tf_base_link2map * tf_map2moved_object; - double dist = std::sqrt( - tf_base_link2moved_object.getOrigin().x() * tf_base_link2moved_object.getOrigin().x() + - tf_base_link2moved_object.getOrigin().y() * tf_base_link2moved_object.getOrigin().y()); - if (visible_range_ < dist) { - delete_idxs.push_back(i); + if (detection_successful_rate_ >= detection_successful_random(random_generator_)) { + selected_indices.push_back(i); } - } - // delete - for (int delete_idx = delete_idxs.size() - 1; 0 <= delete_idx; --delete_idx) { - objects_.erase(objects_.begin() + delete_idxs.at(delete_idx)); + const auto obj_info = ObjectInfo(objects_.at(i), current_time); + obj_infos.push_back(obj_info); } - // merge all pointcloud pcl::PointCloud::Ptr merged_pointcloud_ptr(new pcl::PointCloud); - for (size_t i = 0; i < v_pointcloud.size(); ++i) { - for (size_t j = 0; j < v_pointcloud.at(i)->size(); ++j) { - merged_pointcloud_ptr->push_back(v_pointcloud.at(i)->at(j)); - } + pcl::PointCloud::Ptr detected_merged_pointcloud_ptr( + new pcl::PointCloud); + + if (objects_.empty()) { + pcl::toROSMsg(*merged_pointcloud_ptr, output_pointcloud_msg); + } else { + pointcloud_creator_->create_pointclouds( + obj_infos, tf_base_link2map, random_generator_, merged_pointcloud_ptr); + pcl::toROSMsg(*merged_pointcloud_ptr, output_pointcloud_msg); } - // no ground - pcl::toROSMsg(*merged_pointcloud_ptr, output_pointcloud_msg); + if (!selected_indices.empty()) { + std::vector detected_obj_infos; + for (const auto selected_idx : selected_indices) { + const auto detected_obj_info = ObjectInfo(objects_.at(selected_idx), current_time); + tf2::toMsg(detected_obj_info.tf_map2moved_object, output_moved_object_pose.pose); + detected_obj_infos.push_back(detected_obj_info); + } - // ray tracing - if (enable_ray_tracing_) { - pcl::PointCloud::Ptr ray_traced_merged_pointcloud_ptr( - new pcl::PointCloud); - pcl::VoxelGridOcclusionEstimation ray_tracing_filter; - ray_tracing_filter.setInputCloud(merged_pointcloud_ptr); - ray_tracing_filter.setLeafSize(0.25, 0.25, 0.25); - ray_tracing_filter.initializeVoxelGrid(); - for (size_t i = 0; i < v_pointcloud.size(); ++i) { - pcl::PointCloud::Ptr ray_traced_pointcloud_ptr( - new pcl::PointCloud); - for (size_t j = 0; j < v_pointcloud.at(i)->size(); ++j) { - Eigen::Vector3i grid_coordinates = ray_tracing_filter.getGridCoordinates( - v_pointcloud.at(i)->at(j).x, v_pointcloud.at(i)->at(j).y, v_pointcloud.at(i)->at(j).z); - int grid_state; - if (ray_tracing_filter.occlusionEstimation(grid_state, grid_coordinates) != 0) { - RCLCPP_ERROR(get_logger(), "ray tracing failed"); - } - if (grid_state == 1) { // occluded - continue; - } else { // not occluded - ray_traced_pointcloud_ptr->push_back(v_pointcloud.at(i)->at(j)); - ray_traced_merged_pointcloud_ptr->push_back(v_pointcloud.at(i)->at(j)); - } + const auto pointclouds = pointcloud_creator_->create_pointclouds( + detected_obj_infos, tf_base_link2map, random_generator_, detected_merged_pointcloud_ptr); + + std::vector delete_idxs; + for (size_t i = 0; i < selected_indices.size(); ++i) { + const auto pointcloud = pointclouds[i]; + const size_t selected_idx = selected_indices[i]; + const auto & object = objects_.at(selected_idx); + const auto object_info = ObjectInfo(object, current_time); + // dynamic object + std::normal_distribution<> x_random(0.0, object_info.std_dev_x); + std::normal_distribution<> y_random(0.0, object_info.std_dev_y); + std::normal_distribution<> yaw_random(0.0, object_info.std_dev_yaw); + tf2::Quaternion noised_quat; + noised_quat.setRPY(0, 0, yaw_random(random_generator_)); + tf2::Transform tf_moved_object2noised_moved_object( + noised_quat, tf2::Vector3(x_random(random_generator_), y_random(random_generator_), 0.0)); + tf2::Transform tf_base_link2noised_moved_object; + tf_base_link2noised_moved_object = + tf_base_link2map * object_info.tf_map2moved_object * tf_moved_object2noised_moved_object; + tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object; + feature_object.object.classification.push_back(object.classification); + feature_object.object.kinematics.pose_with_covariance = object.initial_state.pose_covariance; + feature_object.object.kinematics.twist_with_covariance = + object.initial_state.twist_covariance; + feature_object.object.kinematics.orientation_availability = + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + feature_object.object.kinematics.has_twist = false; + tf2::toMsg( + tf_base_link2noised_moved_object, + feature_object.object.kinematics.pose_with_covariance.pose); + feature_object.object.shape = object.shape; + pcl::toROSMsg(*pointcloud, feature_object.feature.cluster); + output_dynamic_object_msg.feature_objects.push_back(feature_object); + + // check delete idx + tf2::Transform tf_base_link2moved_object; + tf_base_link2moved_object = tf_base_link2map * object_info.tf_map2moved_object; + double dist = std::sqrt( + tf_base_link2moved_object.getOrigin().x() * tf_base_link2moved_object.getOrigin().x() + + tf_base_link2moved_object.getOrigin().y() * tf_base_link2moved_object.getOrigin().y()); + if (visible_range_ < dist) { + delete_idxs.push_back(selected_idx); } - pcl::toROSMsg( - *ray_traced_pointcloud_ptr, - output_dynamic_object_msg.feature_objects.at(i).feature.cluster); - output_dynamic_object_msg.feature_objects.at(i).feature.cluster.header.frame_id = "base_link"; - output_dynamic_object_msg.feature_objects.at(i).feature.cluster.header.stamp = current_time; } - pcl::toROSMsg(*ray_traced_merged_pointcloud_ptr, output_pointcloud_msg); + + // delete + for (int delete_idx = delete_idxs.size() - 1; 0 <= delete_idx; --delete_idx) { + objects_.erase(objects_.begin() + delete_idxs.at(delete_idx)); + } } // create output header @@ -223,116 +225,6 @@ void DummyPerceptionPublisherNode::timerCallback() } } -void DummyPerceptionPublisherNode::createObjectPointcloud( - const double length, const double width, const double height, const double std_dev_x, - const double std_dev_y, const double std_dev_z, const tf2::Transform & tf_base_link2moved_object, - pcl::PointCloud::Ptr & pointcloud_ptr) -{ - std::normal_distribution<> x_random(0.0, std_dev_x); - std::normal_distribution<> y_random(0.0, std_dev_y); - std::normal_distribution<> z_random(0.0, std_dev_z); - auto getBaseLinkTo2DPoint = [tf_base_link2moved_object](double x, double y) -> pcl::PointXYZ { - tf2::Transform tf_moved_object2point; - tf2::Transform tf_base_link2point; - geometry_msgs::msg::Transform ros_moved_object2point; - ros_moved_object2point.translation.x = x; - ros_moved_object2point.translation.y = y; - ros_moved_object2point.translation.z = 0.0; - ros_moved_object2point.rotation.x = 0; - ros_moved_object2point.rotation.y = 0; - ros_moved_object2point.rotation.z = 0; - ros_moved_object2point.rotation.w = 1; - tf2::fromMsg(ros_moved_object2point, tf_moved_object2point); - tf_base_link2point = tf_base_link2moved_object * tf_moved_object2point; - pcl::PointXYZ point; - point.x = tf_base_link2point.getOrigin().x(); - point.y = tf_base_link2point.getOrigin().y(); - point.z = tf_base_link2point.getOrigin().z(); - return point; - }; - const double epsilon = 0.001; - const double step = 0.05; - const double vertical_theta_step = (1.0 / 180.0) * M_PI; - const double vertical_min_theta = (-15.0 / 180.0) * M_PI; - const double vertical_max_theta = (15.0 / 180.0) * M_PI; - const double horizontal_theta_step = (0.1 / 180.0) * M_PI; - const double horizontal_min_theta = (-180.0 / 180.0) * M_PI; - const double horizontal_max_theta = (180.0 / 180.0) * M_PI; - - const double min_z = -1.0 * (height / 2.0) + tf_base_link2moved_object.getOrigin().z(); - const double max_z = 1.0 * (height / 2.0) + tf_base_link2moved_object.getOrigin().z(); - pcl::PointCloud horizontal_candidate_pointcloud; - pcl::PointCloud horizontal_pointcloud; - { - const double y = -1.0 * (width / 2.0); - for (double x = -1.0 * (length / 2.0); x <= ((length / 2.0) + epsilon); x += step) { - horizontal_candidate_pointcloud.push_back(getBaseLinkTo2DPoint(x, y)); - } - } - { - const double y = 1.0 * (width / 2.0); - for (double x = -1.0 * (length / 2.0); x <= ((length / 2.0) + epsilon); x += step) { - horizontal_candidate_pointcloud.push_back(getBaseLinkTo2DPoint(x, y)); - } - } - { - const double x = -1.0 * (length / 2.0); - for (double y = -1.0 * (width / 2.0); y <= ((width / 2.0) + epsilon); y += step) { - horizontal_candidate_pointcloud.push_back(getBaseLinkTo2DPoint(x, y)); - } - } - { - const double x = 1.0 * (length / 2.0); - for (double y = -1.0 * (width / 2.0); y <= ((width / 2.0) + epsilon); y += step) { - horizontal_candidate_pointcloud.push_back(getBaseLinkTo2DPoint(x, y)); - } - } - // 2D ray tracing - size_t ranges_size = - std::ceil((horizontal_max_theta - horizontal_min_theta) / horizontal_theta_step); - std::vector horizontal_ray_traced_2d_pointcloud; - horizontal_ray_traced_2d_pointcloud.assign(ranges_size, std::numeric_limits::infinity()); - const int no_data = -1; - std::vector horizontal_ray_traced_pointcloud_indices; - horizontal_ray_traced_pointcloud_indices.assign(ranges_size, no_data); - for (size_t i = 0; i < horizontal_candidate_pointcloud.points.size(); ++i) { - double angle = - std::atan2(horizontal_candidate_pointcloud.at(i).y, horizontal_candidate_pointcloud.at(i).x); - double range = - std::hypot(horizontal_candidate_pointcloud.at(i).y, horizontal_candidate_pointcloud.at(i).x); - if (angle < horizontal_min_theta || angle > horizontal_max_theta) { - continue; - } - int index = (angle - horizontal_min_theta) / horizontal_theta_step; - if (range < horizontal_ray_traced_2d_pointcloud[index]) { - horizontal_ray_traced_2d_pointcloud[index] = range; - horizontal_ray_traced_pointcloud_indices.at(index) = i; - } - } - for (const auto & pointcloud_index : horizontal_ray_traced_pointcloud_indices) { - if (pointcloud_index != no_data) { - // generate vertical point - horizontal_pointcloud.push_back(horizontal_candidate_pointcloud.at(pointcloud_index)); - const double distance = std::hypot( - horizontal_candidate_pointcloud.at(pointcloud_index).x, - horizontal_candidate_pointcloud.at(pointcloud_index).y); - for (double vertical_theta = vertical_min_theta; - vertical_theta <= vertical_max_theta + epsilon; vertical_theta += vertical_theta_step) { - const double z = distance * std::tan(vertical_theta); - if (min_z <= z && z <= max_z + epsilon) { - pcl::PointXYZ point; - point.x = - horizontal_candidate_pointcloud.at(pointcloud_index).x + x_random(random_generator_); - point.y = - horizontal_candidate_pointcloud.at(pointcloud_index).y + y_random(random_generator_); - point.z = z + z_random(random_generator_); - pointcloud_ptr->push_back(point); - } - } - } - } -} - void DummyPerceptionPublisherNode::objectCallback( const dummy_perception_publisher::msg::Object::ConstSharedPtr msg) { diff --git a/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp b/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp new file mode 100644 index 0000000000000..48943caaeae7d --- /dev/null +++ b/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp @@ -0,0 +1,236 @@ +// Copyright 2020 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. + +#include "dummy_perception_publisher/node.hpp" +#include "dummy_perception_publisher/signed_distance_function.hpp" + +#include + +#include +#include +#include + +#include +#include +#include + +pcl::PointXYZ getPointWrtBaseLink( + const tf2::Transform & tf_base_link2moved_object, double x, double y, double z) +{ + const auto p_wrt_base = tf_base_link2moved_object(tf2::Vector3(x, y, z)); + return pcl::PointXYZ(p_wrt_base.x(), p_wrt_base.y(), p_wrt_base.z()); +}; + +void ObjectCentricPointCloudCreator::create_object_pointcloud( + const ObjectInfo & obj_info, const tf2::Transform & tf_base_link2map, + std::mt19937 & random_generator, pcl::PointCloud::Ptr pointcloud) const +{ + std::normal_distribution<> x_random(0.0, obj_info.std_dev_x); + std::normal_distribution<> y_random(0.0, obj_info.std_dev_y); + std::normal_distribution<> z_random(0.0, obj_info.std_dev_z); + const double epsilon = 0.001; + const double step = 0.05; + const double vertical_theta_step = (1.0 / 180.0) * M_PI; + const double vertical_min_theta = (-15.0 / 180.0) * M_PI; + const double vertical_max_theta = (15.0 / 180.0) * M_PI; + const double horizontal_theta_step = (0.1 / 180.0) * M_PI; + const double horizontal_min_theta = (-180.0 / 180.0) * M_PI; + const double horizontal_max_theta = (180.0 / 180.0) * M_PI; + + const auto tf_base_link2moved_object = tf_base_link2map * obj_info.tf_map2moved_object; + + const double min_z = -1.0 * (obj_info.height / 2.0) + tf_base_link2moved_object.getOrigin().z(); + const double max_z = 1.0 * (obj_info.height / 2.0) + tf_base_link2moved_object.getOrigin().z(); + pcl::PointCloud horizontal_candidate_pointcloud; + pcl::PointCloud horizontal_pointcloud; + { + const double y = -1.0 * (obj_info.width / 2.0); + for (double x = -1.0 * (obj_info.length / 2.0); x <= ((obj_info.length / 2.0) + epsilon); + x += step) { + horizontal_candidate_pointcloud.push_back( + getPointWrtBaseLink(tf_base_link2moved_object, x, y, 0.0)); + } + } + { + const double y = 1.0 * (obj_info.width / 2.0); + for (double x = -1.0 * (obj_info.length / 2.0); x <= ((obj_info.length / 2.0) + epsilon); + x += step) { + horizontal_candidate_pointcloud.push_back( + getPointWrtBaseLink(tf_base_link2moved_object, x, y, 0.0)); + } + } + { + const double x = -1.0 * (obj_info.length / 2.0); + for (double y = -1.0 * (obj_info.width / 2.0); y <= ((obj_info.width / 2.0) + epsilon); + y += step) { + horizontal_candidate_pointcloud.push_back( + getPointWrtBaseLink(tf_base_link2moved_object, x, y, 0.0)); + } + } + { + const double x = 1.0 * (obj_info.length / 2.0); + for (double y = -1.0 * (obj_info.width / 2.0); y <= ((obj_info.width / 2.0) + epsilon); + y += step) { + horizontal_candidate_pointcloud.push_back( + getPointWrtBaseLink(tf_base_link2moved_object, x, y, 0.0)); + } + } + // 2D ray tracing + size_t ranges_size = + std::ceil((horizontal_max_theta - horizontal_min_theta) / horizontal_theta_step); + std::vector horizontal_ray_traced_2d_pointcloud; + horizontal_ray_traced_2d_pointcloud.assign(ranges_size, std::numeric_limits::infinity()); + const int no_data = -1; + std::vector horizontal_ray_traced_pointcloud_indices; + horizontal_ray_traced_pointcloud_indices.assign(ranges_size, no_data); + for (size_t i = 0; i < horizontal_candidate_pointcloud.points.size(); ++i) { + double angle = + std::atan2(horizontal_candidate_pointcloud.at(i).y, horizontal_candidate_pointcloud.at(i).x); + double range = + std::hypot(horizontal_candidate_pointcloud.at(i).y, horizontal_candidate_pointcloud.at(i).x); + if (angle < horizontal_min_theta || angle > horizontal_max_theta) { + continue; + } + int index = (angle - horizontal_min_theta) / horizontal_theta_step; + if (range < horizontal_ray_traced_2d_pointcloud[index]) { + horizontal_ray_traced_2d_pointcloud[index] = range; + horizontal_ray_traced_pointcloud_indices.at(index) = i; + } + } + + for (const auto & pointcloud_index : horizontal_ray_traced_pointcloud_indices) { + if (pointcloud_index != no_data) { + // generate vertical point + horizontal_pointcloud.push_back(horizontal_candidate_pointcloud.at(pointcloud_index)); + const double distance = std::hypot( + horizontal_candidate_pointcloud.at(pointcloud_index).x, + horizontal_candidate_pointcloud.at(pointcloud_index).y); + for (double vertical_theta = vertical_min_theta; + vertical_theta <= vertical_max_theta + epsilon; vertical_theta += vertical_theta_step) { + const double z = distance * std::tan(vertical_theta); + if (min_z <= z && z <= max_z + epsilon) { + pcl::PointXYZ point; + point.x = + horizontal_candidate_pointcloud.at(pointcloud_index).x + x_random(random_generator); + point.y = + horizontal_candidate_pointcloud.at(pointcloud_index).y + y_random(random_generator); + point.z = z + z_random(random_generator); + pointcloud->push_back(point); + } + } + } + } +} + +std::vector::Ptr> ObjectCentricPointCloudCreator::create_pointclouds( + const std::vector & obj_infos, const tf2::Transform & tf_base_link2map, + std::mt19937 & random_generator, pcl::PointCloud::Ptr & merged_pointcloud) const +{ + std::vector::Ptr> pointclouds_tmp; + pcl::PointCloud::Ptr merged_pointcloud_tmp(new pcl::PointCloud); + + for (const auto & obj_info : obj_infos) { + pcl::PointCloud::Ptr pointcloud_shared_ptr(new pcl::PointCloud); + this->create_object_pointcloud( + obj_info, tf_base_link2map, random_generator, pointcloud_shared_ptr); + pointclouds_tmp.push_back(pointcloud_shared_ptr); + } + + for (const auto & cloud : pointclouds_tmp) { + for (const auto & pt : *cloud) { + merged_pointcloud_tmp->push_back(pt); + } + } + + if (!enable_ray_tracing_) { + merged_pointcloud = merged_pointcloud_tmp; + return pointclouds_tmp; + } + + pcl::PointCloud::Ptr ray_traced_merged_pointcloud_ptr( + new pcl::PointCloud); + pcl::VoxelGridOcclusionEstimation ray_tracing_filter; + ray_tracing_filter.setInputCloud(merged_pointcloud_tmp); + ray_tracing_filter.setLeafSize(0.25, 0.25, 0.25); + ray_tracing_filter.initializeVoxelGrid(); + std::vector::Ptr> pointclouds; + for (size_t i = 0; i < pointclouds_tmp.size(); ++i) { + pcl::PointCloud::Ptr ray_traced_pointcloud_ptr( + new pcl::PointCloud); + for (size_t j = 0; j < pointclouds_tmp.at(i)->size(); ++j) { + Eigen::Vector3i grid_coordinates = ray_tracing_filter.getGridCoordinates( + pointclouds_tmp.at(i)->at(j).x, pointclouds_tmp.at(i)->at(j).y, + pointclouds_tmp.at(i)->at(j).z); + int grid_state; + if (ray_tracing_filter.occlusionEstimation(grid_state, grid_coordinates) != 0) { + RCLCPP_ERROR(rclcpp::get_logger("dummy_perception_publisher"), "ray tracing failed"); + } + if (grid_state == 1) { // occluded + continue; + } else { // not occluded + ray_traced_pointcloud_ptr->push_back(pointclouds_tmp.at(i)->at(j)); + ray_traced_merged_pointcloud_ptr->push_back(pointclouds_tmp.at(i)->at(j)); + } + } + pointclouds.push_back(ray_traced_pointcloud_ptr); + } + merged_pointcloud = ray_traced_merged_pointcloud_ptr; + return pointclouds; +} + +std::vector::Ptr> EgoCentricPointCloudCreator::create_pointclouds( + const std::vector & obj_infos, const tf2::Transform & tf_base_link2map, + std::mt19937 & random_generator, pcl::PointCloud::Ptr & merged_pointcloud) const +{ + std::vector> sdf_ptrs; + for (const auto & obj_info : obj_infos) { + const auto sdf_ptr = std::make_shared( + obj_info.length, obj_info.width, tf_base_link2map * obj_info.tf_map2moved_object); + sdf_ptrs.push_back(sdf_ptr); + } + const auto composite_sdf = signed_distance_function::CompositeSDF(sdf_ptrs); + + std::vector::Ptr> pointclouds(obj_infos.size()); + for (size_t i = 0; i < obj_infos.size(); ++i) { + pointclouds.at(i) = (pcl::PointCloud::Ptr(new pcl::PointCloud)); + } + + const double horizontal_theta_step = 0.25 * M_PI / 180.0; + double angle = 0.0; + const auto n_scan = static_cast(std::floor(2 * M_PI / horizontal_theta_step)); + for (size_t i = 0; i < n_scan; ++i) { + angle += horizontal_theta_step; + const auto dist = composite_sdf.getSphereTracingDist(0.0, 0.0, angle, visible_range_); + + if (std::isfinite(dist)) { + const auto x_hit = dist * cos(angle); + const auto y_hit = dist * sin(angle); + const auto idx_hit = composite_sdf.nearest_sdf_index(x_hit, y_hit); + + std::normal_distribution<> x_random(0.0, obj_infos.at(idx_hit).std_dev_x); + std::normal_distribution<> y_random(0.0, obj_infos.at(idx_hit).std_dev_y); + std::normal_distribution<> z_random(0.0, obj_infos.at(idx_hit).std_dev_z); + pointclouds.at(idx_hit)->push_back(pcl::PointXYZ( + x_hit + x_random(random_generator), y_hit + y_random(random_generator), + z_random(random_generator))); + } + } + + for (const auto & cloud : pointclouds) { + for (const auto & pt : *cloud) { + merged_pointcloud->push_back(pt); + } + } + return pointclouds; +} diff --git a/simulator/dummy_perception_publisher/src/signed_distance_function.cpp b/simulator/dummy_perception_publisher/src/signed_distance_function.cpp new file mode 100644 index 0000000000000..c23252e58dc06 --- /dev/null +++ b/simulator/dummy_perception_publisher/src/signed_distance_function.cpp @@ -0,0 +1,91 @@ +// Copyright 2022 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. + +#include "dummy_perception_publisher/signed_distance_function.hpp" + +#include + +#include +#include + +namespace signed_distance_function +{ + +double AbstractSignedDistanceFunction::getSphereTracingDist( + double x_start, double y_start, double angle, double max_dist, double eps) const +{ + // https://computergraphics.stackexchange.com/questions/161/what-is-ray-marching-is-sphere-tracing-the-same-thing/163 + tf2::Vector3 direction(cos(angle), sin(angle), 0.0); + + const size_t max_iter = 40; + + const auto pos_start = tf2::Vector3(x_start, y_start, 0.0); + + auto ray_tip = pos_start; + for (size_t itr = 0; itr < max_iter; ++itr) { + const auto dist = this->operator()(ray_tip.getX(), ray_tip.getY()); + if (dist > max_dist) { + return std::numeric_limits::infinity(); + } + ray_tip = ray_tip + dist * direction; + bool almost_on_surface = std::abs(dist) < eps; + if (almost_on_surface) { + return tf2::tf2Distance(ray_tip, pos_start); + } + } + // ray did not hit the surface. + return std::numeric_limits::infinity(); +} + +double BoxSDF::operator()(double x, double y) const +{ + const auto && vec_global = tf2::Vector3(x, y, 0.0); + const auto vec_local = tf_local_to_global_(vec_global); + + // As for signed distance field for a box, please refere: + // https://www.iquilezles.org/www/articles/distfunctions/distfunctions.htm + const auto sd_val_x = std::abs(vec_local.getX()) - 0.5 * length_; + const auto sd_val_y = std::abs(vec_local.getY()) - 0.5 * width_; + const auto positive_dist_x = std::max(sd_val_x, 0.0); + const auto positive_dist_y = std::max(sd_val_y, 0.0); + + const auto positive_dist = std::hypot(positive_dist_x, positive_dist_y); + if (positive_dist > 0.0) { + return positive_dist; + } + const auto negative_dist = std::min(std::max(sd_val_x, sd_val_y), 0.0); + return negative_dist; +} + +double CompositeSDF::operator()(double x, double y) const +{ + const size_t nearest_idx = nearest_sdf_index(x, y); + return sdf_ptrs_.at(nearest_idx)->operator()(x, y); +} + +size_t CompositeSDF::nearest_sdf_index(double x, double y) const +{ + double min_value = std::numeric_limits::infinity(); + size_t idx_min; + for (size_t i = 0; i < sdf_ptrs_.size(); ++i) { + const auto value = sdf_ptrs_.at(i)->operator()(x, y); + if (value < min_value) { + min_value = value; + idx_min = i; + } + } + return idx_min; +} + +} // namespace signed_distance_function diff --git a/simulator/dummy_perception_publisher/test/src/test_signed_distance_function.cpp b/simulator/dummy_perception_publisher/test/src/test_signed_distance_function.cpp new file mode 100644 index 0000000000000..5b8dcee549ea7 --- /dev/null +++ b/simulator/dummy_perception_publisher/test/src/test_signed_distance_function.cpp @@ -0,0 +1,84 @@ +// Copyright 2022 Tier IV, Inc. All rights reserved. +// +// 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. + +#include "dummy_perception_publisher/signed_distance_function.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +namespace sdf = signed_distance_function; + +TEST(SignedDistanceFunctionTest, BoxSDF) +{ + const double eps = 1e-5; + + { + // test with identity transform + const auto q = tf2::Quaternion(tf2::Vector3(0, 0, 1.0), 0.0); + const auto tf_global2local = tf2::Transform(q); + const auto func = sdf::BoxSDF(1., 2., tf_global2local); + ASSERT_NEAR(func(0.0, 0.0), -0.5, eps); + ASSERT_NEAR(func(0.0, 1.0), 0.0, eps); + ASSERT_NEAR(func(0.0, 1.5), 0.5, eps); + ASSERT_NEAR(func(0.5, 0.0), 0.0, eps); + ASSERT_NEAR(func(1.0, 0.0), 0.5, eps); + ASSERT_NEAR(func(1.0, 2.0), sqrt(0.5 * 0.5 + 1.0 * 1.0), eps); + + ASSERT_NEAR(func.getSphereTracingDist(2.0, 0.0, M_PI * -1.0), 1.5, eps); + ASSERT_NEAR(func.getSphereTracingDist(1.0, 1.5, M_PI * 1.25), sqrt(2.0) * 0.5, eps); + ASSERT_TRUE(std::isinf(func.getSphereTracingDist(2.0, 0.0, 0.0))); + } + + { + // test with rotation (90 deg) and translation + const auto q = tf2::Quaternion(tf2::Vector3(0, 0, 1.0), M_PI * 0.5); + const auto tf_global2local = tf2::Transform(q, tf2::Vector3(1.0, 1.0, 0.0)); + const auto func = sdf::BoxSDF(1., 2., tf_global2local); + ASSERT_NEAR(func(1.0, 1.0), -0.5, eps); + ASSERT_NEAR(func(0.0, 0.0), 0.5, eps); + + // ASSERT_NEAR(func.getSphereTracingDist(0.0, 0.0, M_PI * 0.25, eps), sqrt(2.0) * 0.5, eps); + } +} + +TEST(SignedDistanceFunctionTest, CompositeSDF) +{ + const double eps = 1e-5; + const auto q_identity = tf2::Quaternion(tf2::Vector3(0, 0, 1.0), 0.0); + const auto f1 = + std::make_shared(1., 1., tf2::Transform(q_identity, tf2::Vector3(0, 0, 0))); + const auto f2 = + std::make_shared(1., 1., tf2::Transform(q_identity, tf2::Vector3(0, 2.0, 0))); + const auto func = + sdf::CompositeSDF(std::vector>{f1, f2}); + ASSERT_NEAR(func(0.0, 0.9), 0.4, eps); + ASSERT_NEAR(func(0.0, 1.1), 0.4, eps); + ASSERT_NEAR(func(0.0, 0.1), -0.4, eps); + ASSERT_NEAR(func(0.0, 1.6), -0.1, eps); + + // ASSERT_NEAR(func.getSphereTracingDist(0.0, 1.0, M_PI * 0.5, eps), 0.5, eps); + // ASSERT_NEAR(func.getSphereTracingDist(0.0, 1.0, M_PI * -0.5, eps), 0.5, eps); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 8d49017105db3db91ddae8e82f536ce4cc832fdc Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Thu, 2 Mar 2023 00:20:52 +0900 Subject: [PATCH 16/92] fix(tier4_autoware_utils): fix build error (#296) * fix(tier4_autoware_utils): build_error * fix test code * fix test code * fix test code --- .../trajectory/trajectory.hpp | 2 +- .../test/src/trajectory/test_trajectory.cpp | 36 +++++++++++-------- .../test_longitudinal_controller_utils.cpp | 4 +-- 3 files changed, 24 insertions(+), 18 deletions(-) diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp index 0b7a6d05d7ec3..979d5f0fc3657 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp @@ -47,7 +47,7 @@ std::vector removeOverlapPoints(const T & points, con const auto p_next = tier4_autoware_utils::getPoint(dst.at(i + 1)); const Eigen::Vector3d v{p_next.x - p.x, p_next.y - p.y, 0.0}; if (v.norm() < eps) { - dst.erase(dst.begin() + i + 1); + dst.erase(dst.begin() + static_cast(i) + 1); } else { ++i; } diff --git a/common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp b/common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp index 941f0eba8da97..fb49c7cd33308 100644 --- a/common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp @@ -107,7 +107,7 @@ TEST(trajectory, searchZeroVelocityIndex) using tier4_autoware_utils::searchZeroVelocityIndex; // Empty - EXPECT_THROW(searchZeroVelocityIndex(Trajectory{}.points), std::invalid_argument); + EXPECT_FALSE(searchZeroVelocityIndex(Trajectory{}.points)); // No zero velocity point { @@ -229,8 +229,7 @@ TEST(trajectory, findNearestIndex_Pose_NoThreshold) const auto traj = generateTestTrajectory(10, 1.0); // Empty - EXPECT_THROW( - findNearestIndex(Trajectory{}.points, geometry_msgs::msg::Pose{}, {}), std::invalid_argument); + EXPECT_FALSE(findNearestIndex(Trajectory{}.points, geometry_msgs::msg::Pose{}, {})); // Start point EXPECT_EQ(*findNearestIndex(traj.points, createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)), 0U); @@ -343,26 +342,30 @@ TEST(trajectory, calcLongitudinalOffsetToSegment_StraightTrajectory) using tier4_autoware_utils::calcLongitudinalOffsetToSegment; const auto traj = generateTestTrajectory(10, 1.0); + const bool throw_exception = true; // Empty EXPECT_THROW( - calcLongitudinalOffsetToSegment(Trajectory{}.points, {}, geometry_msgs::msg::Point{}), + calcLongitudinalOffsetToSegment( + Trajectory{}.points, {}, geometry_msgs::msg::Point{}, throw_exception), std::invalid_argument); // Out of range EXPECT_THROW( - calcLongitudinalOffsetToSegment(traj.points, -1, geometry_msgs::msg::Point{}), + calcLongitudinalOffsetToSegment(traj.points, -1, geometry_msgs::msg::Point{}, throw_exception), std::out_of_range); EXPECT_THROW( calcLongitudinalOffsetToSegment( - traj.points, traj.points.size() - 1, geometry_msgs::msg::Point{}), + traj.points, traj.points.size() - 1, geometry_msgs::msg::Point{}, throw_exception), std::out_of_range); // Same close points in trajectory { const auto invalid_traj = generateTestTrajectory(10, 0.0); const auto p = createPoint(3.0, 0.0, 0.0); - EXPECT_THROW(calcLongitudinalOffsetToSegment(invalid_traj.points, 3, p), std::runtime_error); + EXPECT_THROW( + calcLongitudinalOffsetToSegment(invalid_traj.points, 3, p, throw_exception), + std::runtime_error); } // Same point @@ -401,23 +404,26 @@ TEST(trajectory, calcLateralOffset) using tier4_autoware_utils::calcLateralOffset; const auto traj = generateTestTrajectory(10, 1.0); + const bool throw_exception = true; // Empty EXPECT_THROW( - calcLateralOffset(Trajectory{}.points, geometry_msgs::msg::Point{}), std::invalid_argument); + calcLateralOffset(Trajectory{}.points, geometry_msgs::msg::Point{}, throw_exception), + std::invalid_argument); // Trajectory size is 1 { const auto one_point_traj = generateTestTrajectory(1, 1.0); EXPECT_THROW( - calcLateralOffset(one_point_traj.points, geometry_msgs::msg::Point{}), std::out_of_range); + calcLateralOffset(one_point_traj.points, geometry_msgs::msg::Point{}, throw_exception), + std::runtime_error); } // Same close points in trajectory { const auto invalid_traj = generateTestTrajectory(10, 0.0); const auto p = createPoint(3.0, 0.0, 0.0); - EXPECT_THROW(calcLateralOffset(invalid_traj.points, p), std::runtime_error); + EXPECT_THROW(calcLateralOffset(invalid_traj.points, p, throw_exception), std::runtime_error); } // Point on trajectory @@ -452,7 +458,7 @@ TEST(trajectory, calcSignedArcLengthFromIndexToIndex) const auto traj = generateTestTrajectory(10, 1.0); // Empty - EXPECT_THROW(calcSignedArcLength(Trajectory{}.points, {}, {}), std::invalid_argument); + EXPECT_DOUBLE_EQ(calcSignedArcLength(Trajectory{}.points, {}, {}), 0.0); // Out of range EXPECT_THROW(calcSignedArcLength(traj.points, -1, 1), std::out_of_range); @@ -475,7 +481,7 @@ TEST(trajectory, calcSignedArcLengthFromPointToIndex) const auto traj = generateTestTrajectory(10, 1.0); // Empty - EXPECT_THROW(calcSignedArcLength(Trajectory{}.points, {}, {}), std::invalid_argument); + EXPECT_DOUBLE_EQ(calcSignedArcLength(Trajectory{}.points, {}, {}), 0.0); // Same point EXPECT_NEAR(calcSignedArcLength(traj.points, createPoint(3.0, 0.0, 0.0), 3), 0, epsilon); @@ -504,7 +510,7 @@ TEST(trajectory, calcSignedArcLengthFromIndexToPoint) const auto traj = generateTestTrajectory(10, 1.0); // Empty - EXPECT_THROW(calcSignedArcLength(Trajectory{}.points, {}, {}), std::invalid_argument); + EXPECT_DOUBLE_EQ(calcSignedArcLength(Trajectory{}.points, {}, {}), 0.0); // Same point EXPECT_NEAR(calcSignedArcLength(traj.points, 3, createPoint(3.0, 0.0, 0.0)), 0, epsilon); @@ -533,7 +539,7 @@ TEST(trajectory, calcSignedArcLengthFromPointToPoint) const auto traj = generateTestTrajectory(10, 1.0); // Empty - EXPECT_THROW(calcSignedArcLength(Trajectory{}.points, {}, {}), std::invalid_argument); + EXPECT_DOUBLE_EQ(calcSignedArcLength(Trajectory{}.points, {}, {}), 0.0); // Same point { @@ -596,7 +602,7 @@ TEST(trajectory, calcArcLength) const auto traj = generateTestTrajectory(10, 1.0); // Empty - EXPECT_THROW(calcArcLength(Trajectory{}.points), std::invalid_argument); + EXPECT_DOUBLE_EQ(calcArcLength(Trajectory{}.points), 0.0); // Whole Length EXPECT_NEAR(calcArcLength(traj.points), 9.0, epsilon); diff --git a/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp b/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp index cc0c7c1214d3e..4256bf9d0f74e 100644 --- a/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp +++ b/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp @@ -60,8 +60,8 @@ TEST(TestLongitudinalControllerUtils, calcStopDistance) { point.pose.position.y = 0.0; point.longitudinal_velocity_mps = 0.0; traj.points.push_back(point); - EXPECT_THROW( - longitudinal_utils::calcStopDistance(current_pose, traj, max_dist, max_yaw), std::out_of_range); + EXPECT_DOUBLE_EQ( + longitudinal_utils::calcStopDistance(current_pose, traj, max_dist, max_yaw), 0.0); traj.points.clear(); // non stopping trajectory: stop distance = trajectory length point.pose.position.x = 0.0; From da26100d7628e6bfb380f1ef93c19a9c3cb190bf Mon Sep 17 00:00:00 2001 From: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Date: Fri, 17 Mar 2023 16:47:07 +0900 Subject: [PATCH 17/92] feat: add release note workflow v0.3.15 (#327) * ci: dispatch release note repository (#313) Signed-off-by: Shumpei Wakabayashi * ci(dispatch-release-note): use Github Apps instead of GIHUB_TOKEN (#315) * ci(dispatch-release-note): use Github Apps instead of GIHUB_TOKEN Signed-off-by: Shumpei Wakabayashi * fix typo * chore: remove white space Signed-off-by: Shumpei Wakabayashi --------- Signed-off-by: Shumpei Wakabayashi Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> --------- Signed-off-by: Shumpei Wakabayashi Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> --- .github/workflows/dispatch-release-note.yaml | 46 ++++++++++++++++++++ 1 file changed, 46 insertions(+) create mode 100644 .github/workflows/dispatch-release-note.yaml diff --git a/.github/workflows/dispatch-release-note.yaml b/.github/workflows/dispatch-release-note.yaml new file mode 100644 index 0000000000000..0245e2b929cd8 --- /dev/null +++ b/.github/workflows/dispatch-release-note.yaml @@ -0,0 +1,46 @@ +name: dispatch-release-note +on: + push: + branches: + - beta/v* + - tier4/main + tags: + - v* + workflow_dispatch: + inputs: + beta-branch-or-tag-name: + description: The name of the beta branch or tag to write release note + type: string + required: true +jobs: + dispatch-release-note: + runs-on: ubuntu-latest + name: release-repository-dispatch + steps: + - name: Set tag name + id: set-tag-name + run: | + if [ "${{ github.event_name }}" = "workflow_dispatch" ]; then + REF_NAME="${{ github.event.inputs.beta-branch-or-tag-name }}" + else + REF_NAME="${{ github.ref_name }}" + fi + echo ::set-output name=ref-name::"$REF_NAME" + echo ::set-output name=tag-name::"${REF_NAME#beta/}" + + - name: Generate token + id: generate-token + uses: tibdex/github-app-token@v1 + with: + app_id: ${{ secrets.APP_ID }} + private_key: ${{ secrets.PRIVATE_KEY }} + + - name: Repository dispatch for release note + run: | + curl \ + -X POST \ + -H "Accept: application/vnd.github+json" \ + -H "Authorization: token ${{ steps.generate-token.outputs.token }}" \ + -H "X-GitHub-Api-Version: 2022-11-28" \ + "https://api.github.com/repos/tier4/update-release-notes/dispatches" \ + -d '{"event_type":"${{ steps.set-tag-name.outputs.ref-name }}"}' From b20c054b9ff0371f1ac7228806dee21b6fed8a2b Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 31 Mar 2023 16:53:07 +0900 Subject: [PATCH 18/92] feat(elevation_map_loader): large size pointcloud map (backport #2571, #2885, #943) (#337) * feat(elevation_map_loader): reduce memory usage of elevation_map_loader (#2571) * feat: reduce memory usage of elevation_map_loader Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> * chore: remove unnecessary comment Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> * fix: modify variables' name Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> * feat(elevation_map_loader): use polygon iterator to speed up (#2885) * use grid_map::PolygonIterator instead of grid_map::GridMapIterator Signed-off-by: Shunsuke Miura * formatting Signed-off-by: Shunsuke Miura * use use_lane_filter option Signed-off-by: Shunsuke Miura * delete unused use-lane-filter option Signed-off-by: Shunsuke Miura * change use_lane_filter to True, clarify the scope Signed-off-by: Shunsuke Miura * change to use grid_map_utils::PolygonIterator Signed-off-by: Shunsuke Miura * Add lane margin parameter Signed-off-by: Shunsuke Miura * use boost geometry buffer to expand lanes Signed-off-by: Shunsuke Miura * Change use_lane_filter param default to false Signed-off-by: Shunsuke Miura * update README Signed-off-by: Shunsuke Miura --------- Signed-off-by: Shunsuke Miura * perf(behavior_velocity_planner): add faster PolygonIterator (#943) * Add grid_map_utils pkg with faster implementation of PolygonIterator Signed-off-by: Maxime CLEMENT * suppress error --------- Signed-off-by: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> Signed-off-by: Shunsuke Miura Signed-off-by: Maxime CLEMENT Co-authored-by: Shintaro Tomie <58775300+Shin-kyoto@users.noreply.github.com> Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> --- common/grid_map_utils/CMakeLists.txt | 42 +++ common/grid_map_utils/README.md | 52 ++++ .../grid_map_utils/polygon_iterator.hpp | 129 ++++++++ .../media/runtime_comparison.png | Bin 0 -> 18307 bytes common/grid_map_utils/package.xml | 25 ++ .../grid_map_utils/src/polygon_iterator.cpp | 214 +++++++++++++ common/grid_map_utils/test/benchmark.cpp | 182 ++++++++++++ .../test/test_polygon_iterator.cpp | 280 ++++++++++++++++++ .../ground_segmentation.launch.py | 1 + .../elevation_map_loader/CMakeLists.txt | 2 +- perception/elevation_map_loader/README.md | 26 +- .../elevation_map_loader_node.hpp | 23 +- .../launch/elevation_map_loader.launch.xml | 12 - perception/elevation_map_loader/package.xml | 1 + .../src/elevation_map_loader_node.cpp | 223 ++++---------- 15 files changed, 1001 insertions(+), 211 deletions(-) create mode 100644 common/grid_map_utils/CMakeLists.txt create mode 100644 common/grid_map_utils/README.md create mode 100644 common/grid_map_utils/include/grid_map_utils/polygon_iterator.hpp create mode 100644 common/grid_map_utils/media/runtime_comparison.png create mode 100644 common/grid_map_utils/package.xml create mode 100644 common/grid_map_utils/src/polygon_iterator.cpp create mode 100644 common/grid_map_utils/test/benchmark.cpp create mode 100644 common/grid_map_utils/test/test_polygon_iterator.cpp diff --git a/common/grid_map_utils/CMakeLists.txt b/common/grid_map_utils/CMakeLists.txt new file mode 100644 index 0000000000000..0fbb34e94e688 --- /dev/null +++ b/common/grid_map_utils/CMakeLists.txt @@ -0,0 +1,42 @@ +cmake_minimum_required(VERSION 3.5) +project(grid_map_utils) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(autoware_cmake REQUIRED) +autoware_package() +find_package(ament_cmake REQUIRED) + +ament_auto_add_library(${PROJECT_NAME} SHARED + DIRECTORY src +) + +target_link_libraries(${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + ament_add_gtest(test_${PROJECT_NAME} + test/test_polygon_iterator.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) + + find_package(OpenCV REQUIRED) + add_executable(benchmark test/benchmark.cpp) + target_link_libraries(benchmark + ${PROJECT_NAME} + ${OpenCV_LIBS} + ) +endif() + +ament_auto_package() diff --git a/common/grid_map_utils/README.md b/common/grid_map_utils/README.md new file mode 100644 index 0000000000000..ca37cf8919078 --- /dev/null +++ b/common/grid_map_utils/README.md @@ -0,0 +1,52 @@ +# Grid Map Utils + +## Overview + +This packages contains a re-implementation of the `grid_map::PolygonIterator` used to iterate over +all cells of a grid map contained inside some polygon. + +## Algorithm + +This implementation uses the [scan line algorithm](https://en.wikipedia.org/wiki/Scanline_rendering), +a common algorithm used to draw polygons on a rasterized image. +The main idea of the algorithm adapted to a grid map is as follow: + +- calculate intersections between rows of the grid map and the edges of the polygon edges; +- calculate for each row the column between each pair of intersections; +- the resulting `(row, column)` indexes are inside of the polygon. + +More details on the scan line algorithm can be found in the References. + +## API + +The `grid_map_utils::PolygonIterator` follows the same API as the original [`grid_map::PolygonIterator`](https://docs.ros.org/en/kinetic/api/grid_map_core/html/classgrid__map_1_1PolygonIterator.html). + +## Assumptions + +The behavior of the `grid_map_utils::PolygonIterator` is only guaranteed to match the `grid_map::PolygonIterator` if edges of the polygon do not _exactly_ cross any cell center. +In such a case, whether the crossed cell is considered inside or outside of the polygon can vary due to floating precision error. + +## Performances + +Benchmarking code is implemented in `test/benchmarking.cpp` and is also used to validate that the `grid_map_utils::PolygonIterator` behaves exactly like the `grid_map::PolygonIterator`. + +The following figure shows a comparison of the runtime between the implementation of this package (`grid_map_utils`) and the original implementation (`grid_map`). +The time measured includes the construction of the iterator and the iteration over all indexes and is shown using a logarithmic scale. +Results were obtained varying the side size of a square grid map with `100 <= n <= 1000` (size=`n` means a grid of `n x n` cells), +random polygons with a number of vertices `3 <= m <= 100` and with each parameter `(n,m)` repeated 10 times. + +![Runtime comparison](media/runtime_comparison.png) + +## Future improvements + +There exists variations of the scan line algorithm for multiple polygons. +These can be implemented if we want to iterate over the cells contained in at least one of multiple polygons. + +The current implementation imitate the behavior of the original `grid_map::PolygonIterator` where a cell is selected if its center position is inside the polygon. +This behavior could be changed for example to only return all cells overlapped by the polygon. + +## References + +- +- +- diff --git a/common/grid_map_utils/include/grid_map_utils/polygon_iterator.hpp b/common/grid_map_utils/include/grid_map_utils/polygon_iterator.hpp new file mode 100644 index 0000000000000..4f2e149a50f72 --- /dev/null +++ b/common/grid_map_utils/include/grid_map_utils/polygon_iterator.hpp @@ -0,0 +1,129 @@ +// Copyright 2022 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 GRID_MAP_UTILS__POLYGON_ITERATOR_HPP_ +#define GRID_MAP_UTILS__POLYGON_ITERATOR_HPP_ + +#include "grid_map_core/TypeDefs.hpp" + +#include +#include +#include + +#include +#include + +namespace grid_map_utils +{ + +/// @brief Representation of a polygon edge made of 2 vertices +struct Edge +{ + grid_map::Position first; + grid_map::Position second; + + Edge(grid_map::Position f, grid_map::Position s) : first(std::move(f)), second(std::move(s)) {} + + /// @brief Sorting operator resulting in edges sorted from highest to lowest x values + bool operator<(const Edge & e) + { + return first.x() > e.first.x() || (first.x() == e.first.x() && second.x() > e.second.x()); + } +}; + +/** @brief A polygon iterator for grid_map::GridMap based on the scan line algorithm. + @details This iterator allows to iterate over all cells whose center is inside a polygon. \ + This reproduces the behavior of the original grid_map::PolygonIterator which uses\ + a "point in polygon" check for each cell of the gridmap, making it very expensive\ + to run on large maps. In comparison, the scan line algorithm implemented here is \ + much more scalable. +*/ +class PolygonIterator +{ +public: + /// @brief Constructor. + /// @details Calculate the indexes of the gridmap that are inside the polygon using the scan line + /// algorithm. + /// @param grid_map the grid map to iterate on. + /// @param polygon the polygonal area to iterate on. + PolygonIterator(const grid_map::GridMap & grid_map, const grid_map::Polygon & polygon); + /// @brief Compare to another iterator. + /// @param other other iterator. + /// @return whether the current iterator points to a different address than the other one. + bool operator!=(const PolygonIterator & other) const; + /// @brief Dereference the iterator with const. + /// @return the value to which the iterator is pointing. + const grid_map::Index & operator*() const; + /// @brief Increase the iterator to the next element. + /// @return a reference to the updated iterator. + PolygonIterator & operator++(); + /// @brief Indicates if iterator is past end. + /// @return true if iterator is out of scope, false if end has not been reached. + [[nodiscard]] bool isPastEnd() const; + +private: + /** @brief Calculate sorted edges of the given polygon. + @details Vertices in an edge are ordered from higher to lower x. + Edges are sorted in reverse lexicographical order of x. + @param polygon Polygon for which edges are calculated. + @return Sorted edges of the polygon. + */ + static std::vector calculateSortedEdges(const grid_map::Polygon & polygon); + + /// @brief Calculates intersections between lines (i.e., center of rows) and the polygon edges. + /// @param edges Edges of the polygon. + /// @param from_to_row ranges of lines to use for intersection. + /// @param origin Position of the top-left cell in the grid map. + /// @param grid_map grid map. + /// @return for each row the vector of y values with an intersection. + static std::vector> calculateIntersectionsPerLine( + const std::vector & edges, const std::pair from_to_row, + const grid_map::Position & origin, const grid_map::GridMap & grid_map); + + /// @brief Calculates the range of rows covering the given edges. + /// @details The rows are calculated without any shift that might exist in the grid map. + /// @param edges Edges of the polygon. + /// @param origin Position of the top-left cell in the grid map. + /// @param grid_map grid map. + /// @return the range of rows as a pair {first row, last row}. + static std::pair calculateRowRange( + const std::vector & edges, const grid_map::Position & origin, + const grid_map::GridMap & grid_map); + + // Helper functions + /// @brief Increment the current_line_ to the line with intersections + void goToNextLine(); + /// @brief Calculate the initial current_col_ and the current_to_col_ for the current intersection + void calculateColumnIndexes(); + /// @brief Calculate the current_index_ from the current_line_ and current_col_ + void calculateIndex(); + + /// Gridmap info + int row_of_first_line_; + grid_map::Index map_start_idx_; + grid_map::Size map_size_; + double map_resolution_; + double map_origin_y_; + /// Intersections between scan lines and the polygon + std::vector> intersections_per_line_; + std::vector::const_iterator intersection_iter_; + /// current indexes + grid_map::Index current_index_; + size_t current_line_; + int current_col_; + int current_to_col_; +}; +} // namespace grid_map_utils + +#endif // GRID_MAP_UTILS__POLYGON_ITERATOR_HPP_ diff --git a/common/grid_map_utils/media/runtime_comparison.png b/common/grid_map_utils/media/runtime_comparison.png new file mode 100644 index 0000000000000000000000000000000000000000..8c249c083139351c0bf25098994f67ef36d1eaf2 GIT binary patch literal 18307 zcmc({by!r<_b)t@2#6>WiXccSt)zg2g3{eFgft_dbPZqtB1$(>(m8Ywh>C>LF*HgK zL&p$9ynFQf``!0G_x^LA=e^I%f6O^&pS9Osd&Osc_6gHeSD++kAcsI8luC;7+7Jk# zAp}C;cJTuEWp2@{8Uj&JR+4|D>ut6^O_oG6(s2Hg8R=N&yFVk(Y}MRw)R9#GaE9GC zw%}zr`Camh*Zb{hJ~YYCM^F%6prxVLmA`uB5#6{AEzJ{>alI=vSLLxb*wXxifb-pS zB@wCoMwoA=U!&a;&L@5M9yGtG(e^qzWBHRW20{RV$UbGVZ~VkmDy%2OaL^BkqvUpNkmyJ$0WZZlNc+G&iFml>u{RpH7$BOHf~ zPZqLE!Kh5#sTl8=L}U*XfSJjGG1wwy&g%WQ+DP+eHxE)CaL;gufm z2QFWjFbh2O7}`{vDhrf8{QXAIV^w!3pLq|vt?q_>X z$y`HB2FQdWr+rFa^|wSE%=jUCrNerpX97x5mdx3()tFQNrJhuJ8R=^>^bkl&N(phv z+}yKWf}(7u-h!|*um(+7Njg4b>)M_)NwF3+wBkEw^GfS5AEfJ|@~$yaatK26F8gGi zOC)K7&Ci7PNDt{R9=E+LEiL_R__9nadN;lCv>4GMh$}Ng zPJ9@BuxYuO+Pj}S=P`}NM zK!G{_6%lF5`f-)}W$KN~h`7$-N{5!b z-B2vmuo`q zgkG?)Yl>LqnB4d;HOv#J)V zlAoNqMh{l(Bg%-XBE5`Rm!;O*$GYem#1@XVUt-lf!D19zzxol7=Gb&9F@n`aS#Ksf z`f&#QHQXCihP}t1jwects^&T#V5;0xG!;zt%EZ!GRZld%My1euy6{g2ine^PAPd$TNpO#kB1f7Z7^?rJr%^pxUv9tIS^IYS{ zS(HW&MOBT4LM5X~?ej3^*aybBxs2|q$SWrKbn6H9U@4zv2t}8{23xwjyAMU7=hn=-vpGoJNGs=)Ss1!X-FFWAu^;;EmbtRQMLDeSeuI6E9apaLu2R1C=v8Cmu1GV z&4DP4O1rcjYd&XEf^0vgf8ViB8drk(JMrG(G<7s*iuk}@FCL;w=w3Ouum}6k!r>_Y_U(O@g0qs#wg5TVQQEOiu=DG~j{(EmHZ634|@=wu)AHEZGm zgi`(sA@-3T3n!b*4W!ksFgq4D&eBC?jItOoACRYS-XGU9|%>1>k8+eHP<)QfE(|&iL=Dp*njq zAHB%|=DdxH0Pz4lm>d4deNaizd8)oz=B-C7*)`5ihx|0#iDsW&>$cYz?qn&y+9MXvmAJHxYZr;F@i~Q@hvC&1scw&|>2zk*^mZ&<=a})YzAFm*ZqTf?e z^yI;&%OvV2Q%@z|1Yy^n%!x7ty`3b%bP!FJ%{_pSxweDl~Fay zhao)Nq6>QIUKjhEuH=b$nPAeR!g}UV@^9zoWv>&%EZyg5>Q_aE<$k7kKSJJGoErnw^ zALQth{^c~U^cz|Ouqv6ip_iGsTTd1R7DH}2-vg^`SFz8P|A{qQA+pS^chb2@Enqv` zJPEOW;OdVZw;L?d<8ixur^H5Hm^;^Nc8y%ea|jM}@IQp%MnrTWt+YdV*N5N?$_my` zKU}|U0QHEUj!#^p+FW8WHsj1am|h~++kT-pl#dgp39OU-DF?jMs~`C!=E1or`Bqb* zF}sw2(n$t}Qg`2{3U2NLwPoQiH)bXbj{hVT*=QH-j&CNH9`vOcsuEJn#8!4}#LmGa zlm2=v*t;dH7X=Ia|5}yte^ASaU*+S8?w=T$%aBql3Ta_qL~bU2Ri{^JYj4$PI$G|j z)JUbrS4+Y=1D^@Q)+V?rij(#<^Vg2_AA=vpEhk@RB&}E_>r`Hh=W9#tI~Dr- zWXA2EG|@n(duiWMn^<;)tBwu!=l)TVGu#@|$jKZ@E`!I`d{!@&|9Tszy+a7gkHe8##nQ>FzH8f_v5jf;0&#rM1 zoT*LTxjRF@430^NuVCm+q3*+J6g-{XDX=B+IlkuWSB;Yy|4IAUw3O4DBvGK2>SK8D zYwDTmM}!nIG_!|V+S|Y8wT};O{<4q8n39ziU~IIpvQ&ls+YW{457l3*&?{Nv^l^I* z_aAMS-GpvNk8xi(tFrA#g4@rq;D^8TR?J`LTKd^6uAac?m2@bzX@z`IzFJgw zTc7f{oYzR2zUmsc~;Kv!TP0foe7)k zGtEyx1)O=~E(Vt=$?GG`bYpPeIR4FX>XYrZ_6$X9jBT{xu(Sx406#n@XQfj(vGkE_ zQ3z#1rNTzSXBqQ3a?Smk{?)#k(&r*i$8=q zZ@7*19qFB+PlRfI#3wl2MV@==lvkZp;M!%p zY}FklAADSXJjX@|i62Zb4E%Jx@;xf3+&` zNV4I1Lw055x*9ysy{cD-S2h$Ri4>6jo$t?Vr(?^I+>OVs20=TH zZNbcAc0{Il@s{}L;*y~*#raQjNXWPDPgnuZmIetT&C@|y4`1=)@?3l~(mSt&E4%MyE2^9%n8snP5 z8K(?~)55J|k35K7`>uvkS7~7UYn8K=%a0iH8oi=-0wR;B$(Phyij;Bg{L$+sJ@a;J zZXBZd@amt>5x-Geul{+_{e5@yD#Weq4%;_AD?3H;z)gIAxO8ER{_hCD z1&jJzR~~e4R98rJk5f`ebsttz$XGj7g}Ei%s2LpKVQL>7D1rNMGDoTWePUpFz2U1? zQNlB1oHJ@)x3R4I;cxF#iN57i0yPq_JmnhI#IJeo`3NJwX>9HdNuKTX${>mEjSS!J zAI3)&$~>=WT9g|qZThpH=XgFr3Lk!%%oqyxlKmDnutjyB9#!VW57k_}+pmpx|NO?N zZW)-PQ|9XGW!)zRwb3z?2rnm%k?!-O@>4q5DtuJ|9LG=d`$12S;h&zd!R=a>`7(Z^UZSQb!djMB9&W zUY3x)RmLx+0K!ro6;X;5kgXaLT{??;%W+2&x=Q}IvFD*#o-H#%HeR*5vF;2dT6fH`1b7 zPEB-mraBd2CqIyEAxK&UQT{EsjAfzv>(Ac!--A!EmMhF9;ORFRoJ?*ZhV|}3R_CeacUQ|;mZf{KDmE$BGhKWE}6pSle3Bp4Y zp>~nJKU9Hh^%J`$)Fau)qLS`>xbP|DC>ZRHk0lGW7}HmUE&A^Ji|6FJn^y{gID`IP zDA3_C2vQz5KFw;gxdvId1#E&r$r`2ZmY>0I;YCtNqjo^jzXukC#pJ6cS*bCXo}9Td zCR(t|7!9u*w(EF~8bXS>TX|6NwsA9@GxqmRB2?j)&a>mj#`j4qm;eb1irgVoD>KCklHrJ*3{sWw~uZI^p^ z-0PuGXq>^7dtPtkZ#5IO^cE%$JgX364bvC_IrbU=|z&G+oQ@XyJ>pK&2bqj6C0q-nl)^e6AW=^27pcIkT7~y+ca03)0j7BwCk%=5 z`1f<<+i%+at5jty@kQv@9*|C2YT}^gd@9Yd^oOHU9QL5PECrUoHUyE)26oh`QW0cM zxH}tP8M>i-*4*N2JW_yK*CG6Ok-4IPSjL2tR^y;lCtJC=DDl8Yw#ZIB`l}~>{nxnZ z_hrx7D&D)|)*$`%z>Hn_Po^&vG91VFFseP%3NEh_!9UbjEm#_9*4MzeVgfY^UKg3o&`kKza-fWGpOb54O?k@kNf!L?@Xjy-5O>**|T~dNNDegzlHT=U80b2=QWK-Lv=5W1t=lk z2Vx~ZJRqYaEuP*g)De8t${$#nNSw$IU;wZcGuuHX z%%@tNV|Y@PuQc-Rb{^wnknx3)yumsv1*1yiyAw&3wU$Nw@*yT^T)79c{AUe9980_7 zp&&s4&er(7;qXK0r<*e>rpz{ZyNza?ubwcED_oC;O12-2zTn#Mv`Mo9j>Oo1UO}na zA`*yH?G50M;Qo%jv9=<0m`C2>Y^oFXqn`e|z)@Y`43-)nm8h5$`tOsFxtO}7F+rk0 z(o_w3MY7pIq!*Ldcf#X$=)7y^Ke@Q9(jFuY0`c&0H6dgev{$Mj98!%FA`=M z@v$|RDm|QtpyuPR)*yn|2hqhdHhN$@tW4%W<7VkLX zGMI9-Mfe&ZkTDrPZVRTD->*`iq2z3cUIj6Fj#-Z0s+p*P-LXsaNMQ6B)^hC?U+zrc zGg*TMao(4XhoL*d(m*R^V4RbGn!hp%GI5d3BET#O&g?1U^2^ zu8GUbKqtq<2Z@08&(WelAeY$Gi#LCJUemD`Qm31|e*Q`p>cL)9fAEM?BmFsZie7%2 zXUg;0mT7;a2S#7~Xd$*#YRa8Pg@`o@v1j?Sg)Cd`s^%SDT64c(kPrU$C04gMYx2Ol zr9^lJqY%Ya0w901;jn-Lh;Tpe3+Tltf$qY3kI_iB0u>sC$k5f(Q`($C=K39MVEKO_ z0`_@?L<;^m4t##%y4xc**#r@%=i`qlvJ_CU45c`@kZV#F|0Q&{nd0?RR$Hfpz!}~l z)>N@^KL`Y?qfe;cVqEbo_>=csm@8zjWo0G#n zL%LuLYSlvTvqFiH(f!`m-|soFWso4oF2^s&E7_5%nOY3a0Vng!sV{__CWK)ckbN@5 zxp6lRE8KTUxUFbO1712w06F3SEz9$>&2#I)9HqJ4qlK++RAS*X`Krm!9@7Qwt#rmk zXo_d+Nx0^+8A`;EKsq&yC=Dgr=>Q`5*b5o1xyd{1p$2y6t$_j~m0VcSTwBq%VB4Pf zG9)2np|0ctFQv9z$ffAsG|8Vw%NZ`UADLy$)MZZHfjr8a$Hf75pBpIp$@G0u88aLO zUKE6olo0TcokTf6&$Kl)Eo#y84#0|S*U74%1E0P9@y6{ZX_MdI=)F9U2?s8Z_0PnF z6X=1l6%$Yd)!DZ>1FzUjQ`?lOuEt&Zo&SEMHYk9bL9Xh`a&qdWz3pz!Rll(2T|Q`c z!4b|4ksV4L3Cg9N_IR%eQyr(Y4|@zVPy(U#K$^ZB@5ikL$O7xyW@sV(Z*4@-n!Dr$ zw7?B>Au&IYUc_deWx6d7HCdaQGrR#@l2XY~zW1j$K0Y36o7we#ZN?#Q8@Tys!!Isg zDB8>6fg7q^XC^4{pr<2;p3CKUd!s_67%1Ywils<;mF=xh8oUn=U+2$MBI7sb<)|~g zMy1?)R~MuyjbF7(Chm;Hb|?>wByLjb2RaA<$OOgQ4T1DL)+ZX>Ll3F-@c5n5!pi&O zbTt^Oyn1wF^!=Skg+}Q4_@eQJHh^X3;T&S8Pfj*_M5zofK&lyy)Fc#`;nhC65&ABq zDW88Y|2&Y!^<~pS$BoG+IUo8TZKQJ4TW7%~AhHzEBSHJw%%_Uv6cl)Wq~*IjYih}D z1KaoG+Jv_nWq=}e{NMcX`*Cf&O5s^W(lq6Q2-IXzM8v*sN1AFZ9|Eb?1m=TnZSeM{5Sjjdx@QiCAbq?T6Se|r2AG=pEL<1Gq_v%%kgf;Ij^LbFTLhe3%tpF zb+~Kh(n6%6?t6(kPXN{Sh@Gk7E`j2j8?-?|1Jrdc>`uzQ!R~Tj;^)t-Zg>8GbPA7v z+v3h~f#({W>YwbH$rmW0C1zuV@jQwgu;9cDfq`evD-g&qc$;=W>nyLU%TV4&Mz>%0 zmny_NZuyl|J5B0`P~NmXe$}kID??=5r<`%k{_Bv6|Z;H~fiy}Y)IhW62)gGsbUcY6;D^p12Ql}akV z&H&d9j~NNlGc)6-dbkzz>BUzv*UsCaNltbd@LGS+iXTmvohS3ZXFx7#rYq?<(oh4S zqc1a{(R>OX8>z5TxFP2JNj`hvaQ3O#{3j$71o;) zu?T*A`v@T33-5W#%DqS`S(l1q-oHAX%ho@ZUFvk+?07%`DQ^XjfKKK@S<^;L#_5_J z-Zyq{cQ6ao&G;76&*S{oif|VV0}0lkx8o>tltG*D5wK1y`v!TLA#3pZIgPoBO&!>7-^CmY$n=gI zLCz*(iGhL21tKiIL0p~jx-IjQYd2RJ*Q0ziQ6{2ci2-*{Sm@8DsNx@+<%Y+4gQKge z_*6ZalNgdI-s$~CIqo?z1)tpbsaO3Q-9ap=jyw&(-rf?w{V?QVKvH@Z#jeWX(o&}| z37lf4(oYZ^Bs_|esjKtJZj3w5yv`8t3cicbzn>Wi2fVG6>Jm;SN`s+1wJNdYUmyWo z%!yTxk-ID|-}_+xl2MC)15(BG<;x1jz>{sPZ4Mq_vqyui6978lDu8o@6u0}|W~={I z0Dq`n&$ick^eyjVN2@~y;a^RW&4Ey!*m{1txn$XQB&&cRr+_ijfM%$z`2Mg?sgxs= z-&%#>v+oxPM+5F6H$_25X@Xze1oR9hLx~{$IW{7X#xP~B$P1WvY!Oi}9z1WkOibuH zW*KVaH#~v#t2+`hK#em3OouJlsn?a1yI7C%=5uCpu&Suf)ZZVGGSQq%(1mva7FJR} z29&204Q}}*rVTN+HI#i1wQhbkZjg%~5|#3gEHSN^^56woWtjrGL6Ex1+3o?X47F z5{B#Rkh-K=tqTIWS@qv}=xXr&&b~#n`G;X*6m#&Er5LL)g3-l^iex7Ew z)m2uB??2cN6KiQ|veZqvhBt#Qsi#5q%0PDL9tPDnH9pql<$|6;HX&7*h)Y5S+E+o2 zl=MRHXi&%#_=kOR1^F@`j-$@_>08Oa>!|u}eoSZam(DoZ3YDoA6MIj~qu&{JC*6M4 z4b2p6O#d^QSCwNjWC1`{_+|u|flcHTA9c@WmI=hZw?*JxcevXd*PmOC4YuVP8_dgz zWGZsnOuuHYGIz|mu;J@>sOb=S_wKHr;~sHVu!6|%%o}8(b2c}Wog1FwJr)&2mdyvI zrD!a*c$saYv1K9F#dB%)9Aa>6W!vb|Q~Peu@6PW=X9RY;abgv6#92)6)66SZ?OVL` zW6~al_8~ZozTDg+S)O7id+7#bt@!KvB>!V4`5BXIqL%Cl8AR+#YG!nqO9_~f~*qbKNOa~ETYe)TfIZR7SPX%Fs~5p4pAd8pdmK|u_DxeEwNPy3`uvXP48b2t|y!F-FfjD>QP5viW<*zgL??Fbp z&ew5o->K;)B<0d+&<57nH=C29)nRHN#-iTST5Zx`GMh8$0h`jq556)=MMU8BI84Zz z-(BSqtQhZlv7KFh-DON#d`%7Cu9iOGDA@Y~_}(&zTHSb_gigM?wzf7alcZ-H;O-f= z;5H<2SA=gzGfn0cxM3z-F9}cqWFu3_Cs1u{zb&l@4fQdr{vLGgUCYd$%kuN*wC_Sj zh*>FlCVV3fQlr@@?=#p|LyxO*NTKOl1wOMsE~?;IHLvmR+dVUPd*&CQ{zaM>Va;K| z6ma-Q+td{Jjtg&W*)f6V2Z4Y9XjXxh*U>SF_|q3>1-X(R{zW8bh4fkIvcGFr5O;%* zVa*348ftTvI_6=tx2vyjHivps;BzOIMz=VbTki&;xZ(ah8I@%z-w!3Co5inwk=cR5 zhd@c8(pSTA>>#@Di9~z^Z^WrEyMgh@6e6%@Vk#(V6VTn(jRv5HP5ZSe_%w@WC|VUvOS>8Wz#^HK^v`d4irtL2d>xW z7I+HZGl!XR4A~R#bSo;vDu`czh?eF!1$s}a(W@pN3GTM7P>ra!!5c(a!$%bsrbjHV z8JeZg$K9DNgyxiD3f;G>;7rMmRR$yKx#%re^tvO=4`swH@-^Zqe5)Q}e zpy0e7FyY6JcdKLCI&&YHeI{>XGg+QUnEgN@GxDbMLPt%uD#Pzjg%8sBtNKjL zho~vUE*CQ?G2F}cd{s*~Hyq7|3}6!R&0a#T&1;KLh9`%;AHQ1ErPiga_CQTxEah=m z*b)m*R|h|TN=X*_qW?WF7x54digjlQUtGlXapvEJ6E;L}!FtXq`_6%Kw|UG6=gAmu z1m!NF@of|;l7$%vtI=_B37{6vQboNI53dd{9*lfMJ91yge<%Q9t;Q@CyXmerH8o-> z^aO2MS9Wzs@Y3wMrD$PAl>6^6*Gsb4b?%Q{F_bkrW|QxCaYULwFW9HCLWgoN#cPmC zfl}bx-I+70V)p&CAN3}oS?GpPF7n%5S0&aCm-L=_ziympl zbX$I&&Ehsk_kGCt{(4!#)M;au!$WanfFEg!9HZxlMi6EX!{7Xp&zf?o^x8YPs#vwiQ9t~ zBSLA+P%rH502yM*d!_njyhR-);+cFQv(Vm?0Jc6+<2tBxr|WhoBdgt~mm$37EmJe? zTN5q-^X9{rDS(dqL4ws-{mjONC;~A`l|VcHsgm zQ2RZBH*O~OG@bFbiT``p4vhQb!;Qf#xzM*rlMDEj%*qzSc*^%o+<`ur>GxGt@vxd%<74DjH&?VkXz5t z(7t7+*{AB0?`k2y?wYZfq5Q~)XNU9KD85Ia2=x1TUp3Xji^xYIzsEMKj7Ik}MzwFf@DGLluLJ4JPIXkmmzzam5W`K)_Qz3k|kYH(A7m;mlHAjV10pr(>sFHM(~@O5mhNX1#n z&J3}}d0wnYRaaZ5fUcAdIlJix_Yl3psZk__&UyN`oN3*K%9iO^_w`;*f)vypW4^kV zQacZ=?*|L!qtkpLDe1QKQ=Z zfs6>c8BZCT5b}$G2PZLcsX&lQa@4Myz&<4f`epTBj;K*oRFngV#DhpbFF*QkuvYWa zRRdc`&2J;0pM)=f^g;bWrO(#KTTjI=2#Y6DmB_IBh*0uRn-_~dQp;z!BS4XCH|=oa z)pP0P-tZ7H=yinNw~SP#dsKY+x?y1#KX2XEI$A0dA=Qp{knVQP& z6$x&rMvbG2bq0k4;g=R*%oH=K;vp(r9YH5qk*^0r*H-NZGriupo*M>E^tJCgf=E+4 zOAcY6Bq$~cdp@fy9zs_(OfM9vsN^r2HtLvC%gX+TcSg2ov&qv$-WIN9!nRtW6wqL4 zU*q$loaw`-yuLp}QNwnwvLW#6aSg@ab~K4bEs&xacC!SXOoaLrTKSA}2tSyp_4W{r zj}lLqIl7|XBty#}Jts%Dk?VP)Hze*J$Gyd5rlz=pqS%gXbz}+)#|w$_7T*(LLP&I_G2q2 zuf6&QJZBACxfak*zD0(xP|7j97iNT}-D6ECamZ4mG+}5I_IJ2AX?X32aGXe-{&Sc8 zIGog{JOWkvG9p?2tL4U!?)$PR(eX#@dM1Zz-)TBdd=+$;;5S5v+$7ZUhdd;l>^7OH zL{E!8^+m%TLjb79Sp^kV zS%>Bv8#md(T_@i;?iO6XdX1jpQU+C zIQyrG_yW8K+zc!`530BF-4_@=>2S0daR8D?>~RgdZTSHnitOd0jW`hJvx|rMGC=% z2aS9JeyMIa!K!{;0t`UZ)cq_k=M^0(5>_p9^yU?av!!fBYF)%gj0Wtu=|RNyrj8GU5;nW`AtfrOU91V2@bGZOPuloe?Vfsz%}qA#KeMe(hkNk+QGWpm z2G1Ug3{GB?wSBR?_VoX+@2^l z-n~l|v76fQeGCU1>RutTBeDKrS0j@5d4UUJFM82X%`naNl3aRSQ#66`weHJL3n!dh z4i^R9#D*4@35=+>Ba!Uv=FcPEMZQjh^p}A571;YE)0odT)G~gvI?#%^O|eJN<*BKh z??AhU>o{KAr-G@lulPV9d=v!5WbcDt$647EgRl(!(^ZUqyst(=423`lco4yW3+8|A zsmcgOAPiJ>w#fJiuslqQ*9AM7sgIuP$j_Xx+ds{8A826u+?n_% zg+OX98v5kTjR$PV#^=OO1whx|prhi>?5p=)U0_dZ#O<;iOv8a&hMQ&3Z4WPRqG__! z`gXS_15g!VYHyP12uL7b9)0O%lMpfNU*a`LQXY9+tUr-&U$J*ppjZ?oZJW33-CBKf z<+5%X?6H1E7eR6}uXRyDzKIt%Bkhe7vwLXwEa_Q|pWuiK=$*zZyv_(%BiAxhV~!l6 zv8UYy75>iLRHl(^S>I&%+H#NQap(24AAU{&njre`bg!UB-e%<6U9j#gDu2#1(evQU&bH3T<){3rc>fX{sP#MS%U*+h^X zG9Zn8TR&j$;K}TK0JsGL$rUF}X5BR!QuulJniW?2)OuY-qp%dPb1n_lg1(mu~) zW>rO2JLz@YAVL05zjO=fv4sHccBMXcOJ+@OH$-~x!8q%#F5mcZA-g;to$5K|#! z?&-s5@r0IrOsVF@tH=p#<-cI112M1l_dF@B-FG+mmK;b6j#FkxE<$A89)I~NUA%{) zo2mYJf9gSr@v-W5K=X$oPy*BGn-V}$ZW7Wl$baj9uXa@@Lm6Wz{z&QP-4#|3IPu3S z^CH%4bOBCfq7B!rWG*XOiGAt&F7k}06 zsXBu|6mNorfI>kYZ$F{4Se4@ZReot^tfgix{q8#k&~qgL*C)y)x!=rEDCg5-)d|c5 zGd=$_=vzh#8iGv~aND8=w4?4PH?p$VT|!8b6yVPU5*$Nl@pqX-f#^vgxW9 ztR6P^eXu|)f}@_D55bxYiD4=7aK-1L>!3TP#MuUrnk;Z(i0hQ_G+%=({*H%&r6X2%(AD^?;zjc?NKVd#nW@d&LhANAXRhLk@EWU6>eEx%w_0i6v!~Wvex%TkI zy4KUu6I|k*yLj}|{ET&EWMo?!0oz}N@g|?G?XLbLA=Hm3 zHE;dfyhep)u8x+bUb=oy7PO#p?E2CNYhbAAW7j_ht;g};5bU>;j#EJ5=mJM>Zf@>2 z>LXfA`=btj+;mX{2Pm5l=fXLy#s5w+k-z>oA!;)LRfCuHOx*)d5PAM@R?UD&u>+z#MRjF;5Me+xn-2^VHyK74o#k?*LGDD$NC%b|^!?En#F?SVEaR4UORqwE#swn%)ec4_yB{*PQd; z*wqo0viiCAhoXO_qh&%4I^4y1{ro$NENGLbEzNd8c7z}SIIPR}?`1#Z&yNx3F?TZj z2mJ+(XFKxLQo&If6lNq*g-ZG`vw5eR$D`Kgug!h(n(-0z!#;Q8b6fgVD73oq_qCa= z8f_N+{!m?A-Q_@MB(Pk>ee3zEK48F8vo~B>#|4b$CPMzhu>oS|F|$hSZT!dZsbx7he{4Ob^RZ~X^|ng&DPlu zA699yLKH&(TKoGms!ol-W1U?j`_5mD8`eHgHY(0Ohpz-;!Z05H*;ohm0IRHoq4?3m zg*p?!h)_vuff;eAnswM;+gOmVG_TB#ls0~x6?J4hRG+u&e(Z_{-Pt(5O=Ml~KkbS@ z`?hA!PiOzj40~7ND+BBPz5(@lpnwsal>g5q9tn2$4xU8{z zX3Yn0r(3w$^O+B(;;?dRh4OzwiaJeLx6q%$eg6d0ukynFD(r{#IWDKU%*h<*+IE_D z`%cVhOq%C}LnR#Nxm%;&l;*$F1-#08vyP8qGxh-$*Zyj-41Jzm-4)H-KZ-LSHk79weIiIyCqFHPfE6>3>Vt-=yQ=s3~)ip3j(FY}ViHKi8jZIu1%c z-w%{^ci#hNIeruWNo+mIRZSID zQ&v&Q1qYMWHV(Sa4wQVLlP%cA*EmTzb#@$w2_PRe#G9ZrJCUb$bG+8cLpTVygrEh_QTpeFJ~JUpLYcb-0=6lHSo3YjsL##tMuE6LelE?0 zfK8%WOkV`rfn%>@0)VUdf9G=#{|5)L{$JmrSja8Z$?t9>W#%Pid=bRxyOJg7GV{}JNX#_z9%)=<>i|EjVr9;AQa$9cw9sY&dlPj zIrs)@c zWypeNk!+me!3{y{cX0^`Kg=bao423dc<5bD#&PUXw2Q_9`b}!Npu-^hwb5hjIuD>n z3EFhiTnk|Y4XXF5=?{%S-1>0+{#WA4@4Kdfr~BIa`kbJP_4;JpB{2MeV{jKOqQH7} zH{edX_p@e!aS_L$VT{)KVTa5Do-VQ}<2?1i=l3hp5+uRGwS&L`NR*Z5m?GnV(p16& z?YOagHdXbliE4*fjDN&;H*gktx3RvyUOiASOS991@22SUd*+yJd`d764>PWFc6yq% z)R!S3oq;a!@MDoFDZ;QJ*)ng^4*L{5iPq-i_MrlQGBV zLP8_;u8ZB?=5t5x^6RfD`0weYU52h8a1Ye_2v`C+&J9Q*uAOG*IiL1bE+g)_12Sl; zY)-B7^y&o4(&fMx4n8lEf^HOdTS_XFkhE~ZlN?JoaYGqr!ZV2<)ORUwik9>$8i3J_ z-L~i2w^7QicbLS9`i0s5j;x51K||lFcMi&2+vL_Yvs{Ug+=#ME_xVxM)(FxmC{AK+ z-3;OJvLWIDdN{kRqLN5I@|M)E&LQxL6CMx@?V1G!3iHz)>Aqp^8Wepgz!YyZZMGbG zc9ho5a2M;B#(-drKw?vjUQqqlWPTZ0m$k{At6nnb=DCD^wyPq59R6?x4SkJmx$M`3 zf + + + grid_map_utils + 0.0.0 + Utilities for the grid_map library + mclement + Apache License 2.0 + + autoware_cmake + eigen3_cmake_module + + grid_map_core + grid_map_cv + libopencv-dev + tier4_autoware_utils + + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/grid_map_utils/src/polygon_iterator.cpp b/common/grid_map_utils/src/polygon_iterator.cpp new file mode 100644 index 0000000000000..3ab96bd0612da --- /dev/null +++ b/common/grid_map_utils/src/polygon_iterator.cpp @@ -0,0 +1,214 @@ +// Copyright 2022 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. + +#include "grid_map_utils/polygon_iterator.hpp" + +#include "grid_map_core/GridMap.hpp" +#include "grid_map_core/Polygon.hpp" +#include "grid_map_core/TypeDefs.hpp" + +#include +#include +#include + +namespace grid_map_utils +{ + +std::vector PolygonIterator::calculateSortedEdges(const grid_map::Polygon & polygon) +{ + std::vector edges; + edges.reserve(polygon.nVertices()); + const auto & vertices = polygon.getVertices(); + for (auto vertex = vertices.cbegin(); std::next(vertex) != vertices.cend(); ++vertex) { + // order pair by decreasing x and ignore horizontal edges (when x is equal) + if (vertex->x() > std::next(vertex)->x()) + edges.emplace_back(*vertex, *std::next(vertex)); + else if (vertex->x() < std::next(vertex)->x()) + edges.emplace_back(*std::next(vertex), *vertex); + } + std::sort(edges.begin(), edges.end()); + edges.shrink_to_fit(); + return edges; +} + +std::vector> PolygonIterator::calculateIntersectionsPerLine( + const std::vector & edges, const std::pair from_to_row, + const grid_map::Position & origin, const grid_map::GridMap & grid_map) +{ + const auto from_row = from_to_row.first; + const auto to_row = from_to_row.second; + // calculate for each line the y value intersecting with the polygon in decreasing order + std::vector> y_intersections_per_line; + y_intersections_per_line.reserve(to_row - from_row + 1); + for (auto row = from_row; row <= to_row; ++row) { + std::vector y_intersections; + const auto line_x = origin.x() - grid_map.getResolution() * row; + for (const auto & edge : edges) { + // special case when exactly touching a vertex: only count edge for its lowest x + // up-down edge (\/) case: count the vertex twice + // down-down edge case: count the vertex only once + if (edge.second.x() == line_x) { + y_intersections.push_back(edge.second.y()); + } else if (edge.first.x() >= line_x && edge.second.x() < line_x) { + const auto diff = edge.first - edge.second; + const auto y = edge.second.y() + (line_x - edge.second.x()) * diff.y() / diff.x(); + y_intersections.push_back(y); + } else if (edge.first.x() < line_x) { // edge below the line + break; + } + } + std::sort(y_intersections.begin(), y_intersections.end(), std::greater()); + // remove pairs outside of map + auto iter = y_intersections.cbegin(); + while (iter != y_intersections.cend() && std::next(iter) != y_intersections.cend() && + *iter >= origin.y() && *std::next(iter) >= origin.y()) { + iter = y_intersections.erase(iter); + iter = y_intersections.erase(iter); + } + iter = std::lower_bound( + y_intersections.cbegin(), y_intersections.cend(), + origin.y() - (grid_map.getSize()(1) - 1) * grid_map.getResolution(), std::greater()); + while (iter != y_intersections.cend() && std::next(iter) != y_intersections.cend()) { + iter = y_intersections.erase(iter); + iter = y_intersections.erase(iter); + } + y_intersections_per_line.push_back(y_intersections); + } + return y_intersections_per_line; +} + +std::pair PolygonIterator::calculateRowRange( + const std::vector & edges, const grid_map::Position & origin, + const grid_map::GridMap & grid_map) +{ + const auto min_vertex_x = + std::min_element(edges.cbegin(), edges.cend(), [](const Edge & e1, const Edge & e2) { + return e1.second.x() < e2.second.x(); + })->second.x(); + const auto max_vertex_x = edges.front().first.x(); + + const auto dist_min_to_origin = origin.x() - min_vertex_x + grid_map.getResolution(); + const auto dist_max_to_origin = origin.x() - max_vertex_x + grid_map.getResolution(); + const auto min_row = std::clamp( + static_cast(dist_max_to_origin / grid_map.getResolution()), 0, grid_map.getSize()(0) - 1); + const auto max_row = std::clamp( + static_cast(dist_min_to_origin / grid_map.getResolution()), 0, grid_map.getSize()(0) - 1); + + return {min_row, max_row}; +} + +PolygonIterator::PolygonIterator( + const grid_map::GridMap & grid_map, const grid_map::Polygon & polygon) +{ + auto poly = polygon; + if (poly.nVertices() < 3) return; + // repeat the first vertex to get the last edge [last vertex, first vertex] + if (poly.getVertex(0) != poly.getVertex(poly.nVertices() - 1)) poly.addVertex(poly.getVertex(0)); + + map_start_idx_ = grid_map.getStartIndex(); + map_resolution_ = grid_map.getResolution(); + map_size_ = grid_map.getSize(); + const auto origin = [&]() { + grid_map::Position origin; + grid_map.getPosition(map_start_idx_, origin); + return origin; + }(); + map_origin_y_ = origin.y(); + + // We make line scan left -> right / up -> down *in the index frame* (idx[0,0] is pos[up, left]). + // In the position frame, this corresponds to high -> low Y values and high -> low X values. + const std::vector edges = calculateSortedEdges(poly); + if (edges.empty()) return; + const auto from_to_row = calculateRowRange(edges, origin, grid_map); + intersections_per_line_ = calculateIntersectionsPerLine(edges, from_to_row, origin, grid_map); + + row_of_first_line_ = from_to_row.first; + current_col_ = 0; + current_to_col_ = -1; + current_line_ = -1; // goToNextLine() increments the line so assign -1 to start from 0 + + // Initialize iterator to the first (row,column) inside the Polygon + if (!intersections_per_line_.empty()) { + goToNextLine(); + if (!isPastEnd()) { + calculateColumnIndexes(); + calculateIndex(); + } + } +} + +bool PolygonIterator::operator!=(const PolygonIterator & other) const +{ + return current_line_ != other.current_line_ || current_col_ != other.current_col_; +} + +const grid_map::Index & PolygonIterator::operator*() const { return current_index_; } + +void PolygonIterator::goToNextLine() +{ + ++current_line_; + while (current_line_ < intersections_per_line_.size() && + intersections_per_line_[current_line_].size() < 2) + ++current_line_; + if (!isPastEnd()) intersection_iter_ = intersections_per_line_[current_line_].cbegin(); +} + +void PolygonIterator::calculateColumnIndexes() +{ + const auto dist_from_origin = map_origin_y_ - *intersection_iter_ + map_resolution_; + current_col_ = + std::clamp(static_cast(dist_from_origin / map_resolution_), 0, map_size_(1) - 1); + ++intersection_iter_; + const auto dist_to_origin = map_origin_y_ - *intersection_iter_; + current_to_col_ = + std::clamp(static_cast(dist_to_origin / map_resolution_), 0, map_size_(1) - 1); + // Case where intersections do not encompass the center of a cell: iterate again + if (current_to_col_ < current_col_) { + operator++(); + } +} + +void PolygonIterator::calculateIndex() +{ + current_index_(0) = map_start_idx_(0) + row_of_first_line_ + static_cast(current_line_); + grid_map::wrapIndexToRange(current_index_(0), map_size_(0)); + current_index_(1) = map_start_idx_(1) + current_col_; + grid_map::wrapIndexToRange(current_index_(1), map_size_(1)); +} + +PolygonIterator & PolygonIterator::operator++() +{ + ++current_col_; + if (current_col_ > current_to_col_) { + ++intersection_iter_; + if ( + intersection_iter_ == intersections_per_line_[current_line_].cend() || + std::next(intersection_iter_) == intersections_per_line_[current_line_].cend()) { + goToNextLine(); + } + if (!isPastEnd()) { + calculateColumnIndexes(); + } + } + if (!isPastEnd()) { + calculateIndex(); + } + return *this; +} + +[[nodiscard]] bool PolygonIterator::isPastEnd() const +{ + return current_line_ >= intersections_per_line_.size(); +} +} // namespace grid_map_utils diff --git a/common/grid_map_utils/test/benchmark.cpp b/common/grid_map_utils/test/benchmark.cpp new file mode 100644 index 0000000000000..c8e352ce5d185 --- /dev/null +++ b/common/grid_map_utils/test/benchmark.cpp @@ -0,0 +1,182 @@ +// Copyright 2022 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. + +#include "grid_map_core/TypeDefs.hpp" +#include "grid_map_cv/GridMapCvConverter.hpp" +#include "grid_map_cv/GridMapCvProcessing.hpp" +#include "grid_map_utils/polygon_iterator.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +int main(int argc, char * argv[]) +{ + bool visualize = false; + for (int i = 1; i < argc; ++i) { + const auto arg = std::string(argv[i]); + if (arg == "-v" || arg == "--visualize") { + visualize = true; + } + } + std::ofstream result_file; + result_file.open("benchmark_results.csv"); + result_file + << "#Size PolygonVertices PolygonIndexes grid_map_utils_constructor grid_map_utils_iteration " + "grid_map_constructor grid_map_iteration\n"; + tier4_autoware_utils::StopWatch stopwatch; + + constexpr auto nb_iterations = 10; + constexpr auto polygon_side_vertices = + 25; // number of vertex per side of the square base_polygon + const auto grid_map_length = grid_map::Length(10.0, 10.0); + std::random_device r; + std::default_random_engine engine(0); + // TODO(Maxime CLEMENT): moving breaks the polygon visualization + std::uniform_real_distribution move_dist(-2.0, 2.0); + std::uniform_real_distribution poly_x_offset(-2.0, 2.0); + std::uniform_real_distribution poly_y_offset(-2.0, 2.0); + + grid_map::Polygon base_polygon; + const auto top_left = grid_map::Position(-grid_map_length.x() / 2, grid_map_length.y() / 2); + const auto top_right = grid_map::Position(grid_map_length.x() / 2, grid_map_length.y() / 2); + const auto bot_right = grid_map::Position(grid_map_length.x() / 2, -grid_map_length.y() / 2); + const auto bot_left = grid_map::Position(-grid_map_length.x() / 2, -grid_map_length.y() / 2); + const auto top_vector = top_right - top_left; + for (double i = 0; i < polygon_side_vertices; ++i) { + const auto factor = i / polygon_side_vertices; + base_polygon.addVertex(top_left + factor * top_vector); + } + const auto right_vector = bot_right - top_right; + for (double i = 0; i < polygon_side_vertices; ++i) { + const auto factor = i / polygon_side_vertices; + base_polygon.addVertex(top_right + factor * right_vector); + } + const auto bot_vector = bot_left - bot_right; + for (double i = 0; i < polygon_side_vertices; ++i) { + const auto factor = i / polygon_side_vertices; + base_polygon.addVertex(bot_right + factor * bot_vector); + } + const auto left_vector = top_left - bot_left; + for (double i = 0; i < polygon_side_vertices; ++i) { + const auto factor = i / polygon_side_vertices; + base_polygon.addVertex(bot_left + factor * left_vector); + } + + for (auto grid_map_size = 100; grid_map_size <= 1000; grid_map_size += 100) { + std::cout << "Map of size " << grid_map_size << " by " << grid_map_size << std::endl; + const auto resolution = grid_map_length(0) / grid_map_size; + grid_map::GridMap map({"layer"}); + map.setGeometry(grid_map_length, resolution); + for (auto vertices = 3ul; vertices <= base_polygon.nVertices(); ++vertices) { + auto polygon_indexes = 0.0; + std::cout << "\tPolygon with " << vertices << " vertices" << std::endl; + + double grid_map_utils_constructor_duration{}; + double grid_map_constructor_duration{}; + double grid_map_utils_iteration_duration{}; + double grid_map_iteration_duration{}; + + for (auto iteration = 0; iteration < nb_iterations; ++iteration) { + map.setGeometry(grid_map::Length(10.0, 10.0), resolution, grid_map::Position(0.0, 0.0)); + const auto move = grid_map::Position(move_dist(engine), move_dist(engine)); + map.move(move); + + // generate random sub-polygon of base_polygon with some noise + grid_map::Polygon polygon; + std::vector indexes(base_polygon.nVertices()); + for (size_t i = 0; i <= base_polygon.nVertices(); ++i) indexes[i] = i; + std::shuffle(indexes.begin(), indexes.end(), std::default_random_engine(iteration)); + indexes.resize(vertices); + std::sort(indexes.begin(), indexes.end()); + for (const auto idx : indexes) { + const auto offset = grid_map::Position(poly_x_offset(engine), poly_y_offset(engine)); + polygon.addVertex(base_polygon.getVertex(idx) + offset); + } + stopwatch.tic("gmu_ctor"); + grid_map_utils::PolygonIterator grid_map_utils_iterator(map, polygon); + grid_map_utils_constructor_duration += stopwatch.toc("gmu_ctor"); + stopwatch.tic("gm_ctor"); + grid_map::PolygonIterator grid_map_iterator(map, polygon); + grid_map_constructor_duration += stopwatch.toc("gm_ctor"); + bool diff = false; + while (!grid_map_utils_iterator.isPastEnd() && !grid_map_iterator.isPastEnd()) { + stopwatch.tic("gmu_iter"); + const auto gmu_idx = *grid_map_utils_iterator; + ++grid_map_utils_iterator; + grid_map_utils_iteration_duration += stopwatch.toc("gmu_iter"); + stopwatch.tic("gm_iter"); + const auto gm_idx = *grid_map_iterator; + ++grid_map_iterator; + grid_map_iteration_duration += stopwatch.toc("gm_iter"); + ++polygon_indexes; + + if (gmu_idx.x() != gm_idx.x() || gmu_idx.y() != gm_idx.y()) { + diff = true; + } + } + if (grid_map_iterator.isPastEnd() != grid_map_utils_iterator.isPastEnd()) { + diff = true; + } + if (diff || visualize) { + // Prepare images of the cells selected by the two PolygonIterators + auto gridmap = map; + for (grid_map_utils::PolygonIterator iterator(map, polygon); !iterator.isPastEnd(); + ++iterator) + map.at("layer", *iterator) = 100; + for (grid_map::PolygonIterator iterator(gridmap, polygon); !iterator.isPastEnd(); + ++iterator) + gridmap.at("layer", *iterator) = 100; + + cv::Mat img; + cv::Mat custom_img; + cv::Mat gm_img; + cv::Mat diff_img; + + grid_map::GridMapCvConverter::toImage( + map, "layer", CV_8UC1, 0.0, 100, img); + cv::resize(img, custom_img, cv::Size(500, 500), cv::INTER_LINEAR); + grid_map::GridMapCvConverter::toImage( + gridmap, "layer", CV_8UC1, 0.0, 100, img); + cv::resize(img, gm_img, cv::Size(500, 500), cv::INTER_LINEAR); + cv::compare(custom_img, gm_img, diff_img, cv::CMP_EQ); + + cv::imshow("custom", custom_img); + cv::imshow("grid_map", gm_img); + cv::imshow("diff", diff_img); + cv::waitKey(0); + cv::destroyAllWindows(); + } + } + // print results to file + result_file << grid_map_size << " " << vertices << " " << polygon_indexes / nb_iterations + << " " << grid_map_utils_constructor_duration / nb_iterations << " " + << grid_map_utils_iteration_duration / nb_iterations << " " + << grid_map_constructor_duration / nb_iterations << " " + << grid_map_iteration_duration / nb_iterations << "\n"; + } + } + result_file.close(); +} diff --git a/common/grid_map_utils/test/test_polygon_iterator.cpp b/common/grid_map_utils/test/test_polygon_iterator.cpp new file mode 100644 index 0000000000000..71a7e1db8d4f6 --- /dev/null +++ b/common/grid_map_utils/test/test_polygon_iterator.cpp @@ -0,0 +1,280 @@ +// Copyright 2022 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. + +#include "grid_map_utils/polygon_iterator.hpp" + +#include +#include + +// gtest +#include + +// Vector +#include +#include +#include + +using grid_map::GridMap; +using grid_map::Index; +using grid_map::Length; +using grid_map::Polygon; +using grid_map::Position; + +// Copied from grid_map::PolygonIterator +TEST(PolygonIterator, FullCover) +{ + std::vector types; + types.emplace_back("type"); + GridMap map(types); + map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) + + Polygon polygon; + polygon.addVertex(Position(-100.0, 100.0)); + polygon.addVertex(Position(100.0, 100.0)); + polygon.addVertex(Position(100.0, -100.0)); + polygon.addVertex(Position(-100.0, -100.0)); + + grid_map_utils::PolygonIterator iterator(map, polygon); + + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(0, (*iterator)(0)); + EXPECT_EQ(0, (*iterator)(1)); + + ++iterator; + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(0, (*iterator)(0)); + EXPECT_EQ(1, (*iterator)(1)); + + ++iterator; + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(0, (*iterator)(0)); + EXPECT_EQ(2, (*iterator)(1)); + + for (int i = 0; i < 37; ++i) ++iterator; + + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(7, (*iterator)(0)); + EXPECT_EQ(4, (*iterator)(1)); + + ++iterator; + EXPECT_TRUE(iterator.isPastEnd()); +} + +// Copied from grid_map::PolygonIterator +TEST(PolygonIterator, Outside) +{ + GridMap map({"types"}); + map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) + + Polygon polygon; + polygon.addVertex(Position(99.0, 101.0)); + polygon.addVertex(Position(101.0, 101.0)); + polygon.addVertex(Position(101.0, 99.0)); + polygon.addVertex(Position(99.0, 99.0)); + + grid_map_utils::PolygonIterator iterator(map, polygon); + + EXPECT_TRUE(iterator.isPastEnd()); +} + +// Copied from grid_map::PolygonIterator +TEST(PolygonIterator, Square) +{ + GridMap map({"types"}); + map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) + + Polygon polygon; + polygon.addVertex(Position(-1.0, 1.5)); + polygon.addVertex(Position(1.0, 1.5)); + polygon.addVertex(Position(1.0, -1.5)); + polygon.addVertex(Position(-1.0, -1.5)); + + grid_map_utils::PolygonIterator iterator(map, polygon); + + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(3, (*iterator)(0)); + EXPECT_EQ(1, (*iterator)(1)); + + ++iterator; + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(3, (*iterator)(0)); + EXPECT_EQ(2, (*iterator)(1)); + + ++iterator; + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(3, (*iterator)(0)); + EXPECT_EQ(3, (*iterator)(1)); + + ++iterator; + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(4, (*iterator)(0)); + EXPECT_EQ(1, (*iterator)(1)); + + ++iterator; + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(4, (*iterator)(0)); + EXPECT_EQ(2, (*iterator)(1)); + + ++iterator; + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(4, (*iterator)(0)); + EXPECT_EQ(3, (*iterator)(1)); + + ++iterator; + EXPECT_TRUE(iterator.isPastEnd()); +} + +// Copied from grid_map::PolygonIterator +TEST(PolygonIterator, TopLeftTriangle) +{ + GridMap map({"types"}); + map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) + + Polygon polygon; + polygon.addVertex(Position(-40.1, 20.6)); + polygon.addVertex(Position(40.1, 20.4)); + polygon.addVertex(Position(-40.1, -20.6)); + + grid_map_utils::PolygonIterator iterator(map, polygon); + + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(0, (*iterator)(0)); + EXPECT_EQ(0, (*iterator)(1)); + + ++iterator; + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(1, (*iterator)(0)); + EXPECT_EQ(0, (*iterator)(1)); +} + +// Copied from grid_map::PolygonIterator +TEST(PolygonIterator, MoveMap) +{ + GridMap map({"layer"}); + map.setGeometry(Length(8.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) + map.move(Position(2.0, 0.0)); + + Polygon polygon; + polygon.addVertex(Position(6.1, 1.6)); + polygon.addVertex(Position(0.9, 1.6)); + polygon.addVertex(Position(0.9, -1.6)); + polygon.addVertex(Position(6.1, -1.6)); + grid_map_utils::PolygonIterator iterator(map, polygon); + + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(6, (*iterator)(0)); + EXPECT_EQ(1, (*iterator)(1)); + + ++iterator; + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(6, (*iterator)(0)); + EXPECT_EQ(2, (*iterator)(1)); + + for (int i = 0; i < 4; ++i) ++iterator; + + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(7, (*iterator)(0)); + EXPECT_EQ(3, (*iterator)(1)); + + ++iterator; + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(0, (*iterator)(0)); + EXPECT_EQ(1, (*iterator)(1)); + + for (int i = 0; i < 8; ++i) ++iterator; + + EXPECT_FALSE(iterator.isPastEnd()); + EXPECT_EQ(2, (*iterator)(0)); + EXPECT_EQ(3, (*iterator)(1)); + + ++iterator; + EXPECT_TRUE(iterator.isPastEnd()); +} + +// This test shows a difference when an edge passes exactly through the center of a cell +TEST(PolygonIterator, Difference) +{ + GridMap map({"layer"}); + map.setGeometry(Length(5.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) + + // Triangle where the hypothenus is an exact diagonal of the map: difference. + Polygon polygon; + polygon.addVertex(Position(2.5, 2.5)); + polygon.addVertex(Position(-2.5, 2.5)); + polygon.addVertex(Position(-2.5, -2.5)); + grid_map_utils::PolygonIterator iterator(map, polygon); + grid_map::PolygonIterator gm_iterator(map, polygon); + bool diff = false; + while (!iterator.isPastEnd() && !gm_iterator.isPastEnd()) { + if ((*gm_iterator)(0) != (*iterator)(0) || (*gm_iterator)(1) != (*iterator)(1)) diff = true; + ++iterator; + ++gm_iterator; + } + if (iterator.isPastEnd() != gm_iterator.isPastEnd()) { + diff = true; + } + EXPECT_TRUE(diff); + + // Triangle where the hypothenus does not cross any cell center: no difference. + polygon.removeVertices(); + polygon.addVertex(Position(2.5, 2.1)); + polygon.addVertex(Position(-2.5, 2.5)); + polygon.addVertex(Position(-2.5, -2.9)); + iterator = grid_map_utils::PolygonIterator(map, polygon); + gm_iterator = grid_map::PolygonIterator(map, polygon); + diff = false; + while (!iterator.isPastEnd() && !gm_iterator.isPastEnd()) { + if ((*gm_iterator)(0) != (*iterator)(0) || (*gm_iterator)(1) != (*iterator)(1)) diff = true; + ++iterator; + ++gm_iterator; + } + if (iterator.isPastEnd() != gm_iterator.isPastEnd()) { + diff = true; + } + EXPECT_FALSE(diff); +} + +TEST(PolygonIterator, SelfCrossingPolygon) +{ + GridMap map({"layer"}); + map.setGeometry(Length(5.0, 5.0), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) + + // Hour-glass shape + Polygon polygon; + polygon.addVertex(Position(2.5, 2.9)); + polygon.addVertex(Position(2.5, -2.9)); + polygon.addVertex(Position(-2.5, 2.5)); + polygon.addVertex(Position(-2.5, -2.5)); + grid_map_utils::PolygonIterator iterator(map, polygon); + grid_map::PolygonIterator gm_iterator(map, polygon); + + const std::vector expected_indexes = { + Index(0, 0), Index(0, 1), Index(0, 2), Index(0, 3), Index(0, 4), Index(1, 1), Index(1, 2), + Index(1, 3), Index(2, 2), Index(3, 2), Index(4, 1), Index(4, 2), Index(4, 3)}; + bool diff = false; + size_t i = 0; + while (!iterator.isPastEnd() && !gm_iterator.isPastEnd()) { + if ((*gm_iterator)(0) != (*iterator)(0) || (*gm_iterator)(1) != (*iterator)(1)) diff = true; + ASSERT_TRUE(i < expected_indexes.size()); + EXPECT_EQ((*iterator)(0), expected_indexes[i](0)); + EXPECT_EQ((*iterator)(1), expected_indexes[i](1)); + ++i; + ++iterator; + ++gm_iterator; + } + if (iterator.isPastEnd() != gm_iterator.isPastEnd()) { + diff = true; + } + EXPECT_FALSE(diff); +} diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index 654af3d9bfd0b..e9ae7c3c1cd80 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -327,6 +327,7 @@ def create_single_frame_outlier_filter_components(input_topic, output_topic): "use_lane_filter": False, "use_inpaint": True, "inpaint_radius": 1.0, + "lane_margin": 2.0, "param_file_path": PathJoinSubstitution( [ FindPackageShare("tier4_perception_launch"), diff --git a/perception/elevation_map_loader/CMakeLists.txt b/perception/elevation_map_loader/CMakeLists.txt index 8f5faa9a38513..4c221e7af0e80 100644 --- a/perception/elevation_map_loader/CMakeLists.txt +++ b/perception/elevation_map_loader/CMakeLists.txt @@ -8,7 +8,7 @@ if(NOT CMAKE_CXX_STANDARD) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) + add_compile_options(-Wall -Wextra -Wpedantic) endif() find_package(PCL REQUIRED COMPONENTS io) diff --git a/perception/elevation_map_loader/README.md b/perception/elevation_map_loader/README.md index 7bfdf5729127c..d527ebe0754e9 100644 --- a/perception/elevation_map_loader/README.md +++ b/perception/elevation_map_loader/README.md @@ -36,21 +36,17 @@ Cells with No elevation value can be inpainted using the values of neighboring c ### Node parameters -| Name | Type | Description | Default value | -| :-------------------------------- | :---------- | :--------------------------------------------------------------------------------------------------------- | :------------ | -| map_layer_name | std::string | elevation_map layer name | elevation | -| param_file_path | std::string | GridMap parameters config | path_default | -| elevation_map_file_path | std::string | elevation_map file (bag2) | path_default | -| map_frame | std::string | map_frame when loading elevation_map file | map | -| use_inpaint | bool | Whether to inpaint empty cells | true | -| inpaint_radius | float | Radius of a circular neighborhood of each point inpainted that is considered by the algorithm [m] | 0.3 | -| use_elevation_map_cloud_publisher | bool | Whether to publish `output/elevation_map_cloud` | false | -| use_lane_filter | bool | Whether to filter elevation_map with vector_map | false | -| lane_margin | float | Value of how much to expand the range of vector_map [m] | 0.5 | -| lane_height_diff_thresh | float | Only point clouds in the height range of this value from vector_map are used to generate elevation_map [m] | 1.0 | -| lane_filter_voxel_size_x | float | Voxel size x for calculating point clouds in vector_map [m] | 0.04 | -| lane_filter_voxel_size_y | float | Voxel size y for calculating point clouds in vector_map [m] | 0.04 | -| lane_filter_voxel_size_z | float | Voxel size z for calculating point clouds in vector_map [m] | 0.04 | +| Name | Type | Description | Default value | +| :-------------------------------- | :---------- | :-------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| map_layer_name | std::string | elevation_map layer name | elevation | +| param_file_path | std::string | GridMap parameters config | path_default | +| elevation_map_directory | std::string | elevation_map file (bag2) | path_default | +| map_frame | std::string | map_frame when loading elevation_map file | map | +| use_inpaint | bool | Whether to inpaint empty cells | true | +| inpaint_radius | float | Radius of a circular neighborhood of each point inpainted that is considered by the algorithm [m] | 0.3 | +| use_elevation_map_cloud_publisher | bool | Whether to publish `output/elevation_map_cloud` | false | +| use_lane_filter | bool | Whether to filter elevation_map with vector_map | false | +| lane_margin | float | Margin distance from the lane polygon of the area to be included in the inpainting mask [m]. Used only when use_lane_filter=True. | 0.0 | ### GridMap parameters diff --git a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp b/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp index 308e18ddce374..57f7a2429c68c 100644 --- a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp +++ b/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp @@ -73,22 +73,11 @@ class ElevationMapLoaderNode : public rclcpp::Node void publish(); void createElevationMap(); void setVerbosityLevelToDebugIfFlagSet(); - void createElevationMapFromPointcloud(); - tier4_autoware_utils::LinearRing2d getConvexHull( - const pcl::PointCloud::Ptr & input_cloud); - lanelet::ConstLanelets getIntersectedLanelets( - const tier4_autoware_utils::LinearRing2d & convex_hull, - const lanelet::ConstLanelets & road_lanelets_); - pcl::PointCloud::Ptr getLaneFilteredPointCloud( - const lanelet::ConstLanelets & joint_lanelets, - const pcl::PointCloud::Ptr & cloud); - bool checkPointWithinLanelets( - const pcl::PointXYZ & point, const lanelet::ConstLanelets & joint_lanelets); + void createElevationMapFromPointcloud( + const pcl::shared_ptr & grid_map_pcl_loader); void inpaintElevationMap(const float radius); pcl::PointCloud::Ptr createPointcloudFromElevationMap(); void saveElevationMap(); - float calculateDistancePointFromPlane( - const pcl::PointXYZ & point, const lanelet::ConstLanelet & lanelet); grid_map::GridMap elevation_map_; std::string layer_name_; @@ -97,17 +86,13 @@ class ElevationMapLoaderNode : public rclcpp::Node bool use_inpaint_; float inpaint_radius_; bool use_elevation_map_cloud_publisher_; - pcl::shared_ptr grid_map_pcl_loader_; + std::string param_file_path_; DataManager data_manager_; struct LaneFilter { - float voxel_size_x_; - float voxel_size_y_; - float voxel_size_z_; - float lane_margin_; - float lane_height_diff_thresh_; lanelet::ConstLanelets road_lanelets_; + float lane_margin_; bool use_lane_filter_; }; LaneFilter lane_filter_; diff --git a/perception/elevation_map_loader/launch/elevation_map_loader.launch.xml b/perception/elevation_map_loader/launch/elevation_map_loader.launch.xml index ddb99f7ce9e1a..7b415f5b2e770 100644 --- a/perception/elevation_map_loader/launch/elevation_map_loader.launch.xml +++ b/perception/elevation_map_loader/launch/elevation_map_loader.launch.xml @@ -5,13 +5,6 @@ - - - - - - - @@ -20,10 +13,5 @@ - - - - - diff --git a/perception/elevation_map_loader/package.xml b/perception/elevation_map_loader/package.xml index 1a727f591ffcd..9643d9e2b3c92 100644 --- a/perception/elevation_map_loader/package.xml +++ b/perception/elevation_map_loader/package.xml @@ -14,6 +14,7 @@ grid_map_cv grid_map_pcl grid_map_ros + grid_map_utils lanelet2_extension libpcl-all-dev pcl_conversions diff --git a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp index 57f95eb7e1e89..7d0f0d2f7a7d3 100644 --- a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp +++ b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp @@ -19,10 +19,12 @@ #include #include #include +#include #include #include +#include #include #include #include @@ -50,7 +52,7 @@ ElevationMapLoaderNode::ElevationMapLoaderNode(const rclcpp::NodeOptions & optio : Node("elevation_map_loader", options) { layer_name_ = this->declare_parameter("map_layer_name", std::string("elevation")); - std::string param_file_path = this->declare_parameter("param_file_path", "path_default"); + param_file_path_ = this->declare_parameter("param_file_path", "path_default"); map_frame_ = this->declare_parameter("map_frame", "map"); use_inpaint_ = this->declare_parameter("use_inpaint", true); inpaint_radius_ = this->declare_parameter("inpaint_radius", 0.3); @@ -61,16 +63,7 @@ ElevationMapLoaderNode::ElevationMapLoaderNode(const rclcpp::NodeOptions & optio data_manager_.use_lane_filter_ = use_lane_filter; lane_filter_.use_lane_filter_ = use_lane_filter; - lane_filter_.lane_margin_ = this->declare_parameter("lane_margin", 0.5); - lane_filter_.lane_height_diff_thresh_ = this->declare_parameter("lane_height_diff_thresh", 1.0); - lane_filter_.voxel_size_x_ = declare_parameter("lane_filter_voxel_size_x", 0.04); - lane_filter_.voxel_size_y_ = declare_parameter("lane_filter_voxel_size_y", 0.04); - lane_filter_.voxel_size_z_ = declare_parameter("lane_filter_voxel_size_z", 0.04); - - auto grid_map_logger = rclcpp::get_logger("grid_map_logger"); - grid_map_logger.set_level(rclcpp::Logger::Level::Error); - grid_map_pcl_loader_ = pcl::make_shared(grid_map_logger); - grid_map_pcl_loader_->loadParameters(param_file_path); + lane_filter_.lane_margin_ = this->declare_parameter("lane_margin", 0.0); rclcpp::QoS durable_qos{1}; durable_qos.transient_local(); @@ -153,9 +146,11 @@ void ElevationMapLoaderNode::onPointcloudMap( const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_map) { RCLCPP_INFO(this->get_logger(), "subscribe pointcloud_map"); - pcl::PointCloud map_pcl; - pcl::fromROSMsg(*pointcloud_map, map_pcl); - data_manager_.map_pcl_ptr_ = pcl::make_shared>(map_pcl); + { + pcl::PointCloud map_pcl; + pcl::fromROSMsg(*pointcloud_map, map_pcl); + data_manager_.map_pcl_ptr_ = pcl::make_shared>(map_pcl); + } if (data_manager_.isInitialized()) { publish(); } @@ -177,30 +172,29 @@ void ElevationMapLoaderNode::onVectorMap( void ElevationMapLoaderNode::createElevationMap() { - if (lane_filter_.use_lane_filter_) { - const auto convex_hull = getConvexHull(data_manager_.map_pcl_ptr_); - lanelet::ConstLanelets intersected_lanelets = - getIntersectedLanelets(convex_hull, lane_filter_.road_lanelets_); - pcl::PointCloud::Ptr lane_filtered_map_pcl_ptr = - getLaneFilteredPointCloud(intersected_lanelets, data_manager_.map_pcl_ptr_); - grid_map_pcl_loader_->setInputCloud(lane_filtered_map_pcl_ptr); - } else { - grid_map_pcl_loader_->setInputCloud(data_manager_.map_pcl_ptr_); + auto grid_map_logger = rclcpp::get_logger("grid_map_logger"); + grid_map_logger.set_level(rclcpp::Logger::Level::Error); + { + pcl::shared_ptr grid_map_pcl_loader = + pcl::make_shared(grid_map_logger); + grid_map_pcl_loader->loadParameters(param_file_path_); + grid_map_pcl_loader->setInputCloud(data_manager_.map_pcl_ptr_); + createElevationMapFromPointcloud(grid_map_pcl_loader); + elevation_map_ = grid_map_pcl_loader->getGridMap(); } - createElevationMapFromPointcloud(); - elevation_map_ = grid_map_pcl_loader_->getGridMap(); if (use_inpaint_) { inpaintElevationMap(inpaint_radius_); } saveElevationMap(); } -void ElevationMapLoaderNode::createElevationMapFromPointcloud() +void ElevationMapLoaderNode::createElevationMapFromPointcloud( + const pcl::shared_ptr & grid_map_pcl_loader) { const auto start = std::chrono::high_resolution_clock::now(); - grid_map_pcl_loader_->preProcessInputCloud(); - grid_map_pcl_loader_->initializeGridMapGeometryFromInputCloud(); - grid_map_pcl_loader_->addLayerFromInputCloud(layer_name_); + grid_map_pcl_loader->preProcessInputCloud(); + grid_map_pcl_loader->initializeGridMapGeometryFromInputCloud(); + grid_map_pcl_loader->addLayerFromInputCloud(layer_name_); grid_map::grid_map_pcl::printTimeElapsedToRosInfoStream( start, "Finish creating elevation map. Total time: ", this->get_logger()); } @@ -209,12 +203,45 @@ void ElevationMapLoaderNode::inpaintElevationMap(const float radius) { // Convert elevation layer to OpenCV image to fill in holes. // Get the inpaint mask (nonzero pixels indicate where values need to be filled in). + namespace bg = boost::geometry; + using tier4_autoware_utils::Point2d; + elevation_map_.add("inpaint_mask", 0.0); elevation_map_.setBasicLayers(std::vector()); - for (grid_map::GridMapIterator iterator(elevation_map_); !iterator.isPastEnd(); ++iterator) { - if (!elevation_map_.isValid(*iterator, layer_name_)) { - elevation_map_.at("inpaint_mask", *iterator) = 1.0; + if (lane_filter_.use_lane_filter_) { + for (const auto & lanelet : lane_filter_.road_lanelets_) { + auto lane_polygon = lanelet.polygon2d().basicPolygon(); + grid_map::Polygon polygon; + + if (lane_filter_.lane_margin_ > 0) { + lanelet::BasicPolygons2d out; + bg::strategy::buffer::distance_symmetric distance_strategy( + lane_filter_.lane_margin_); + bg::strategy::buffer::join_miter join_strategy; + bg::strategy::buffer::end_flat end_strategy; + bg::strategy::buffer::point_square point_strategy; + bg::strategy::buffer::side_straight side_strategy; + bg::buffer( + lane_polygon, out, distance_strategy, side_strategy, join_strategy, end_strategy, + point_strategy); + lane_polygon = out.front(); + } + for (const auto & p : lane_polygon) { + polygon.addVertex(grid_map::Position(p[0], p[1])); + } + for (grid_map_utils::PolygonIterator iterator(elevation_map_, polygon); !iterator.isPastEnd(); + ++iterator) { + if (!elevation_map_.isValid(*iterator, layer_name_)) { + elevation_map_.at("inpaint_mask", *iterator) = 1.0; + } + } + } + } else { + for (grid_map::GridMapIterator iterator(elevation_map_); !iterator.isPastEnd(); ++iterator) { + if (!elevation_map_.isValid(*iterator, layer_name_)) { + elevation_map_.at("inpaint_mask", *iterator) = 1.0; + } } } cv::Mat original_image; @@ -236,138 +263,6 @@ void ElevationMapLoaderNode::inpaintElevationMap(const float radius) elevation_map_.erase("inpaint_mask"); } -tier4_autoware_utils::LinearRing2d ElevationMapLoaderNode::getConvexHull( - const pcl::PointCloud::Ptr & input_cloud) -{ - // downsample pointcloud to reduce convex hull calculation cost - pcl::PointCloud::Ptr downsampled_cloud(new pcl::PointCloud); - downsampled_cloud->points.reserve(input_cloud->points.size()); - pcl::VoxelGrid filter; - filter.setInputCloud(input_cloud); - filter.setLeafSize(0.5, 0.5, 100.0); - filter.filter(*downsampled_cloud); - - tier4_autoware_utils::MultiPoint2d candidate_points; - for (const auto & p : downsampled_cloud->points) { - candidate_points.emplace_back(p.x, p.y); - } - - tier4_autoware_utils::LinearRing2d convex_hull; - boost::geometry::convex_hull(candidate_points, convex_hull); - - return convex_hull; -} - -lanelet::ConstLanelets ElevationMapLoaderNode::getIntersectedLanelets( - const tier4_autoware_utils::LinearRing2d & convex_hull, - const lanelet::ConstLanelets & road_lanelets) -{ - lanelet::ConstLanelets intersected_lanelets; - for (const auto & road_lanelet : road_lanelets) { - if (boost::geometry::intersects(convex_hull, road_lanelet.polygon2d().basicPolygon())) { - intersected_lanelets.push_back(road_lanelet); - } - } - return intersected_lanelets; -} - -bool ElevationMapLoaderNode::checkPointWithinLanelets( - const pcl::PointXYZ & point, const lanelet::ConstLanelets & intersected_lanelets) -{ - tier4_autoware_utils::Point2d point2d(point.x, point.y); - for (const auto & lanelet : intersected_lanelets) { - if (lane_filter_.lane_margin_ > 0) { - if ( - boost::geometry::distance(point2d, lanelet.polygon2d().basicPolygon()) > - lane_filter_.lane_margin_) { - continue; - } - } else { - if (!boost::geometry::within(point2d, lanelet.polygon2d().basicPolygon())) { - continue; - } - } - - if (lane_filter_.lane_height_diff_thresh_ > 0) { - float distance = calculateDistancePointFromPlane(point, lanelet); - if (distance < lane_filter_.lane_height_diff_thresh_) { - return true; - } - } else { - return true; - } - } - return false; -} - -float ElevationMapLoaderNode::calculateDistancePointFromPlane( - const pcl::PointXYZ & point, const lanelet::ConstLanelet & lanelet) -{ - const Eigen::Vector3d point_3d(point.x, point.y, point.z); - const Eigen::Vector2d point_2d(point.x, point.y); - - const float distance_3d = boost::geometry::distance(point_3d, lanelet.centerline3d()); - const float distance_2d = boost::geometry::distance(point_2d, lanelet.centerline2d()); - const float distance = std::sqrt(distance_3d * distance_3d - distance_2d * distance_2d); - - return distance; -} - -pcl::PointCloud::Ptr ElevationMapLoaderNode::getLaneFilteredPointCloud( - const lanelet::ConstLanelets & intersected_lanelets, - const pcl::PointCloud::Ptr & cloud) -{ - pcl::PointCloud filtered_cloud; - filtered_cloud.header = cloud->header; - - pcl::PointCloud::Ptr centralized_cloud(new pcl::PointCloud); - centralized_cloud->reserve(cloud->size()); - - // The coordinates of the point cloud are too large, resulting in calculation errors, - // so offset them to the center. - // https://github.com/PointCloudLibrary/pcl/issues/4895 - Eigen::Vector4f centroid; - pcl::compute3DCentroid(*cloud, centroid); - for (const auto & p : cloud->points) { - centralized_cloud->points.push_back( - pcl::PointXYZ(p.x - centroid[0], p.y - centroid[1], p.z - centroid[2])); - } - - pcl::PointCloud::Ptr downsampled_cloud(new pcl::PointCloud); - pcl::VoxelGrid voxel_grid; - voxel_grid.setLeafSize(lane_filter_.voxel_size_x_, lane_filter_.voxel_size_y_, 100000.0); - voxel_grid.setInputCloud(centralized_cloud); - voxel_grid.setSaveLeafLayout(true); - voxel_grid.filter(*downsampled_cloud); - - std::unordered_map> downsampled2original_map; - for (const auto & p : centralized_cloud->points) { - if (std::isnan(p.x) || std::isnan(p.y) || std::isnan(p.z)) { - continue; - } - const size_t index = voxel_grid.getCentroidIndex(p); - downsampled2original_map[index].points.push_back(p); - } - - for (auto & point : downsampled_cloud->points) { - if (checkPointWithinLanelets( - pcl::PointXYZ(point.x + centroid[0], point.y + centroid[1], point.z + centroid[2]), - intersected_lanelets)) { - const size_t index = voxel_grid.getCentroidIndex(point); - for (auto & original_point : downsampled2original_map[index].points) { - original_point.x += centroid[0]; - original_point.y += centroid[1]; - original_point.z += centroid[2]; - filtered_cloud.points.push_back(original_point); - } - } - } - - pcl::PointCloud::Ptr filtered_cloud_ptr; - filtered_cloud_ptr = pcl::make_shared>(filtered_cloud); - return filtered_cloud_ptr; -} - pcl::PointCloud::Ptr ElevationMapLoaderNode::createPointcloudFromElevationMap() { pcl::PointCloud output_cloud; From 859af0a6f9cd35cb22b431a1a1c2dc8fa805b6d0 Mon Sep 17 00:00:00 2001 From: Yohei Mishina <66298900+YoheiMishina@users.noreply.github.com> Date: Thu, 6 Apr 2023 13:50:40 +0900 Subject: [PATCH 19/92] fix: segmentation fault from uninitialized pointer (#933)(backport) (#348) fix: segmentation fault from uninitialized pointer (#933) tf2_buffer_ and tf2_listener_ were not initialized, causing segfault when the input cloud frame is not output_frame_ in PointCloudConcatenateDataSynchronizerComponent::transformPointCloud Co-authored-by: Vincent Richard Co-authored-by: Vincent Richard --- .../src/concatenate_data/concatenate_data_nodelet.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp index 8808cf3db0d83..e2d3b44e32299 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp @@ -112,6 +112,12 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro } } + // tf2 listener + { + tf2_buffer_ = std::make_shared(this->get_clock()); + tf2_listener_ = std::make_shared(*tf2_buffer_); + } + // Publishers { pub_output_ = this->create_publisher( From 7123a7fdc06083d6d46b83f24943cea56d5b1a83 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Thu, 13 Apr 2023 10:35:21 +0900 Subject: [PATCH 20/92] fix(trajectory_follower): fix mpc trajectory z pos (backport #2482) (#353) --- control/trajectory_follower/src/mpc_utils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/trajectory_follower/src/mpc_utils.cpp b/control/trajectory_follower/src/mpc_utils.cpp index 3d2cdacd6ddd5..08ec70496755b 100644 --- a/control/trajectory_follower/src/mpc_utils.cpp +++ b/control/trajectory_follower/src/mpc_utils.cpp @@ -248,7 +248,7 @@ bool8_t convertToMPCTrajectory( for (const autoware_auto_planning_msgs::msg::TrajectoryPoint & p : input.points) { const float64_t x = p.pose.position.x; const float64_t y = p.pose.position.y; - const float64_t z = 0.0; + const float64_t z = p.pose.position.z; const float64_t yaw = ::motion::motion_common::to_angle(p.pose.orientation); const float64_t vx = p.longitudinal_velocity_mps; const float64_t k = 0.0; From 2ed77fc9ca3c86a9ff3bbff90463a9e809970dfb Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Thu, 13 Apr 2023 12:58:57 +0900 Subject: [PATCH 21/92] feat(control): add autonomous emergency braking module (backport #2793, #3186, #3248, #3292) (#330) * feat(control): add autonomous emergency braking module (#2793) * fix build error * ci(pre-commit): autofix * fix: fix lookup symbol error * fix(autonomous_emergency_braking): fix typo (#3186) Update node.cpp * fix(autonomous_emergency_braking): publish debug marker on base_link frame (#3248) * feat(autonomous_emergency_braking): keep collision information for user specified seconds (#3292) * feat(autonomous_emergency_braking): keep collision information for user specified seconds * style(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --------- Co-authored-by: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- common/tier4_autoware_utils/CMakeLists.txt | 1 + .../geometry/boost_polygon_utils.hpp | 34 ++ .../tier4_autoware_utils.hpp | 1 + .../src/geometry/boost_polygon_utils.cpp | 44 ++ .../CMakeLists.txt | 26 + .../autonomous_emergency_braking.param.yaml | 17 + .../autonomous_emergency_braking/node.hpp | 190 ++++++ .../autonomous_emergency_braking.launch.xml | 19 + .../autonomous_emergency_braking/package.xml | 41 ++ .../autonomous_emergency_braking/src/node.cpp | 545 ++++++++++++++++++ .../launch/control.launch.py | 35 ++ .../diagnostic_aggregator/control.param.yaml | 14 + .../config/system_error_monitor.param.yaml | 2 + ...ror_monitor.planning_simulation.param.yaml | 2 + 14 files changed, 971 insertions(+) create mode 100644 common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp create mode 100644 common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp create mode 100644 control/autonomous_emergency_braking/CMakeLists.txt create mode 100644 control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml create mode 100644 control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp create mode 100644 control/autonomous_emergency_braking/launch/autonomous_emergency_braking.launch.xml create mode 100644 control/autonomous_emergency_braking/package.xml create mode 100644 control/autonomous_emergency_braking/src/node.cpp diff --git a/common/tier4_autoware_utils/CMakeLists.txt b/common/tier4_autoware_utils/CMakeLists.txt index e8676d9d4a4fb..b81dedd893697 100644 --- a/common/tier4_autoware_utils/CMakeLists.txt +++ b/common/tier4_autoware_utils/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(Boost REQUIRED) ament_auto_add_library(tier4_autoware_utils SHARED src/tier4_autoware_utils.cpp src/planning/planning_marker_helper.cpp + src/geometry/boost_polygon_utils.cpp ) # Test diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp new file mode 100644 index 0000000000000..1904bbe51e30f --- /dev/null +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp @@ -0,0 +1,34 @@ +// Copyright 2022 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 TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_ +#define TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_ + +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" + +#include + +#include +#include + +#include + +namespace tier4_autoware_utils +{ +bool isClockwise(const Polygon2d & polygon); +Polygon2d inverseClockwise(const Polygon2d & polygon); + +} // namespace tier4_autoware_utils + +#endif // TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp index e86af3c17b4e0..24c679424797d 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp @@ -16,6 +16,7 @@ #define TIER4_AUTOWARE_UTILS__TIER4_AUTOWARE_UTILS_HPP_ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/geometry/pose_deviation.hpp" #include "tier4_autoware_utils/math/constants.hpp" diff --git a/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp b/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp new file mode 100644 index 0000000000000..d7bfca9f090ae --- /dev/null +++ b/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp @@ -0,0 +1,44 @@ +// Copyright 2022 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. + +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" + +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include + +namespace tier4_autoware_utils +{ +bool isClockwise(const Polygon2d & polygon) +{ + const int n = polygon.outer().size(); + const double x_offset = polygon.outer().at(0).x(); + const double y_offset = polygon.outer().at(0).y(); + double sum = 0.0; + for (std::size_t i = 0; i < polygon.outer().size(); ++i) { + sum += + (polygon.outer().at(i).x() - x_offset) * (polygon.outer().at((i + 1) % n).y() - y_offset) - + (polygon.outer().at(i).y() - y_offset) * (polygon.outer().at((i + 1) % n).x() - x_offset); + } + + return sum < 0.0; +} + +Polygon2d inverseClockwise(const Polygon2d & polygon) +{ + auto output_polygon = polygon; + boost::geometry::reverse(output_polygon); + return output_polygon; +} +} // namespace tier4_autoware_utils diff --git a/control/autonomous_emergency_braking/CMakeLists.txt b/control/autonomous_emergency_braking/CMakeLists.txt new file mode 100644 index 0000000000000..6027aeac76333 --- /dev/null +++ b/control/autonomous_emergency_braking/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.14) +project(autonomous_emergency_braking) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +set(AEB_NODE ${PROJECT_NAME}_node) +ament_auto_add_library(${AEB_NODE} SHARED + src/node.cpp +) + +rclcpp_components_register_node(${AEB_NODE} + PLUGIN "autoware::motion::control::autonomous_emergency_braking::AEB" + EXECUTABLE ${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml new file mode 100644 index 0000000000000..09db0feb34597 --- /dev/null +++ b/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -0,0 +1,17 @@ +/**: + ros__parameters: + use_predicted_trajectory: true + use_imu_path: true + voxel_grid_x: 0.05 + voxel_grid_y: 0.05 + voxel_grid_z: 100000.0 + min_generated_path_length: 0.5 + expand_width: 0.1 + longitudinal_offset: 2.0 + t_response: 1.0 + a_ego_min: -3.0 + a_obj_min: -1.0 + prediction_time_horizon: 1.5 + prediction_time_interval: 0.1 + collision_keeping_sec: 0.0 + aeb_hz: 10.0 diff --git a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp new file mode 100644 index 0000000000000..81d64e6e354e3 --- /dev/null +++ b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp @@ -0,0 +1,190 @@ +// 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 AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ +#define AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace autoware::motion::control::autonomous_emergency_braking +{ + +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_system_msgs::msg::AutowareState; +using autoware_auto_vehicle_msgs::msg::VelocityReport; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::Imu; +using sensor_msgs::msg::PointCloud2; +using PointCloud = pcl::PointCloud; +using diagnostic_updater::DiagnosticStatusWrapper; +using diagnostic_updater::Updater; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; +using vehicle_info_util::VehicleInfo; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; +using Path = std::vector; + +struct ObjectData +{ + rclcpp::Time stamp; + geometry_msgs::msg::Point position; + double velocity{0.0}; + double rss{0.0}; + double distance_to_object{0.0}; +}; + +class CollisionDataKeeper +{ +public: + explicit CollisionDataKeeper(rclcpp::Clock::SharedPtr clock) { clock_ = clock; } + + void setTimeout(const double timeout_sec) { timeout_sec_ = timeout_sec; } + + bool checkExpired() + { + if (data_ && (clock_->now() - data_->stamp).seconds() > timeout_sec_) { + data_.reset(); + } + return (data_ == nullptr); + } + + void update(const ObjectData & data) { data_.reset(new ObjectData(data)); } + + ObjectData get() + { + if (data_) { + return *data_; + } else { + return ObjectData(); + } + } + +private: + std::unique_ptr data_; + double timeout_sec_{0.0}; + rclcpp::Clock::SharedPtr clock_; +}; + +class AEB : public rclcpp::Node +{ +public: + explicit AEB(const rclcpp::NodeOptions & node_options); + + // subscriber + rclcpp::Subscription::SharedPtr sub_point_cloud_; + rclcpp::Subscription::SharedPtr sub_velocity_; + rclcpp::Subscription::SharedPtr sub_imu_; + rclcpp::Subscription::SharedPtr sub_predicted_traj_; + rclcpp::Subscription::SharedPtr sub_autoware_state_; + + // publisher + rclcpp::Publisher::SharedPtr pub_obstacle_pointcloud_; + rclcpp::Publisher::SharedPtr debug_ego_path_publisher_; // debug + + // timer + rclcpp::TimerBase::SharedPtr timer_; + + // callback + void onPointCloud(const PointCloud2::ConstSharedPtr input_msg); + void onVelocity(const VelocityReport::ConstSharedPtr input_msg); + void onImu(const Imu::ConstSharedPtr input_msg); + void onTimer(); + void onPredictedTrajectory(const Trajectory::ConstSharedPtr input_msg); + void onAutowareState(const AutowareState::ConstSharedPtr input_msg); + + bool isDataReady(); + + // main function + void onCheckCollision(DiagnosticStatusWrapper & stat); + bool checkCollision(MarkerArray & debug_markers); + bool hasCollision( + const double current_v, const Path & ego_path, const std::vector & objects); + + void generateEgoPath( + const double curr_v, const double curr_w, Path & path, std::vector & polygons); + void generateEgoPath( + const Trajectory & predicted_traj, Path & path, std::vector & polygons); + void createObjectData( + const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, + std::vector & objects); + + void addMarker( + const rclcpp::Time & current_time, const Path & path, const std::vector & polygons, + const std::vector & objects, const double color_r, const double color_g, + const double color_b, const double color_a, const std::string & ns, + MarkerArray & debug_markers); + + void addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers); + + PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr}; + VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr}; + Imu::ConstSharedPtr imu_ptr_{nullptr}; + Trajectory::ConstSharedPtr predicted_traj_ptr_{nullptr}; + AutowareState::ConstSharedPtr autoware_state_{nullptr}; + + tf2_ros::Buffer tf_buffer_{get_clock()}; + tf2_ros::TransformListener tf_listener_{tf_buffer_}; + + // vehicle info + VehicleInfo vehicle_info_; + + // diag + Updater updater_{this}; + + // member variables + bool use_predicted_trajectory_; + bool use_imu_path_; + double voxel_grid_x_; + double voxel_grid_y_; + double voxel_grid_z_; + double min_generated_path_length_; + double expand_width_; + double longitudinal_offset_; + double t_response_; + double a_ego_min_; + double a_obj_min_; + double prediction_time_horizon_; + double prediction_time_interval_; + CollisionDataKeeper collision_data_keeper_; +}; +} // namespace autoware::motion::control::autonomous_emergency_braking + +#endif // AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ diff --git a/control/autonomous_emergency_braking/launch/autonomous_emergency_braking.launch.xml b/control/autonomous_emergency_braking/launch/autonomous_emergency_braking.launch.xml new file mode 100644 index 0000000000000..cf6640ec6be52 --- /dev/null +++ b/control/autonomous_emergency_braking/launch/autonomous_emergency_braking.launch.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/control/autonomous_emergency_braking/package.xml b/control/autonomous_emergency_braking/package.xml new file mode 100644 index 0000000000000..f68c16798ca79 --- /dev/null +++ b/control/autonomous_emergency_braking/package.xml @@ -0,0 +1,41 @@ + + + + autonomous_emergency_braking + 0.1.0 + Autonomous Emergency Braking package as a ROS2 node + yutaka + Apache License 2.0 + + ament_cmake + + autoware_cmake + + autoware_auto_control_msgs + autoware_auto_system_msgs + autoware_auto_vehicle_msgs + diagnostic_updater + geometry_msgs + nav_msgs + pcl_conversions + pcl_ros + pointcloud_preprocessor + rclcpp + rclcpp_components + sensor_msgs + std_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + vehicle_info_util + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp new file mode 100644 index 0000000000000..f4d48380abfed --- /dev/null +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -0,0 +1,545 @@ +// Copyright 2022 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. + +#include "autonomous_emergency_braking/node.hpp" + +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include + +#include +#endif + +namespace tf2 +{ +template <> +inline void doTransform( + const geometry_msgs::msg::Pose & t_in, geometry_msgs::msg::Pose & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + KDL::Vector v(t_in.position.x, t_in.position.y, t_in.position.z); + KDL::Rotation r = KDL::Rotation::Quaternion( + t_in.orientation.x, t_in.orientation.y, t_in.orientation.z, t_in.orientation.w); + + KDL::Frame v_out = gmTransformToKDL(transform) * KDL::Frame(r, v); + t_out.position.x = v_out.p[0]; + t_out.position.y = v_out.p[1]; + t_out.position.z = v_out.p[2]; + v_out.M.GetQuaternion( + t_out.orientation.x, t_out.orientation.y, t_out.orientation.z, t_out.orientation.w); +} + +template <> +inline void doTransform( + const geometry_msgs::msg::Point & t_in, geometry_msgs::msg::Point & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + KDL::Vector v_out = gmTransformToKDL(transform) * KDL::Vector(t_in.x, t_in.y, t_in.z); + t_out.x = v_out[0]; + t_out.y = v_out[1]; + t_out.z = v_out[2]; +} +} // namespace tf2 +namespace autoware::motion::control::autonomous_emergency_braking +{ +using diagnostic_msgs::msg::DiagnosticStatus; +namespace bg = boost::geometry; + +void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) +{ + Point2d point; + point.x() = geom_point.x; + point.y() = geom_point.y; + + bg::append(polygon.outer(), point); +} + +Polygon2d createPolygon( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & next_pose, + const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width) +{ + Polygon2d polygon; + + const double longitudinal_offset = vehicle_info.max_longitudinal_offset_m; + const double width = vehicle_info.vehicle_width_m / 2.0 + expand_width; + const double rear_overhang = vehicle_info.rear_overhang_m; + + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(base_pose, longitudinal_offset, width, 0.0).position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(base_pose, longitudinal_offset, -width, 0.0).position); + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(base_pose, -rear_overhang, -width, 0.0).position); + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(base_pose, -rear_overhang, width, 0.0).position); + + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(next_pose, longitudinal_offset, width, 0.0).position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(next_pose, longitudinal_offset, -width, 0.0).position); + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(next_pose, -rear_overhang, -width, 0.0).position); + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(next_pose, -rear_overhang, width, 0.0).position); + + polygon = tier4_autoware_utils::isClockwise(polygon) + ? polygon + : tier4_autoware_utils::inverseClockwise(polygon); + + Polygon2d hull_polygon; + bg::convex_hull(polygon, hull_polygon); + + return hull_polygon; +} + +AEB::AEB(const rclcpp::NodeOptions & node_options) +: Node("AEB", node_options), + vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()), + collision_data_keeper_(this->get_clock()) +{ + // Subscribers + sub_point_cloud_ = this->create_subscription( + "~/input/pointcloud", rclcpp::SensorDataQoS(), + std::bind(&AEB::onPointCloud, this, std::placeholders::_1)); + + sub_velocity_ = this->create_subscription( + "~/input/velocity", rclcpp::QoS{1}, std::bind(&AEB::onVelocity, this, std::placeholders::_1)); + + sub_imu_ = this->create_subscription( + "~/input/imu", rclcpp::QoS{1}, std::bind(&AEB::onImu, this, std::placeholders::_1)); + + sub_predicted_traj_ = this->create_subscription( + "~/input/predicted_trajectory", rclcpp::QoS{1}, + std::bind(&AEB::onPredictedTrajectory, this, std::placeholders::_1)); + + sub_autoware_state_ = this->create_subscription( + "/autoware/state", rclcpp::QoS{1}, + std::bind(&AEB::onAutowareState, this, std::placeholders::_1)); + + // Publisher + pub_obstacle_pointcloud_ = + this->create_publisher("~/debug/obstacle_pointcloud", 1); + debug_ego_path_publisher_ = this->create_publisher("~/debug/markers", 1); + + // Diagnostics + updater_.setHardwareID("autonomous_emergency_braking"); + updater_.add("aeb_emergency_stop", this, &AEB::onCheckCollision); + + // parameter + use_predicted_trajectory_ = declare_parameter("use_predicted_trajectory"); + use_imu_path_ = declare_parameter("use_imu_path"); + voxel_grid_x_ = declare_parameter("voxel_grid_x"); + voxel_grid_y_ = declare_parameter("voxel_grid_y"); + voxel_grid_z_ = declare_parameter("voxel_grid_z"); + min_generated_path_length_ = declare_parameter("min_generated_path_length"); + expand_width_ = declare_parameter("expand_width"); + longitudinal_offset_ = declare_parameter("longitudinal_offset"); + t_response_ = declare_parameter("t_response"); + a_ego_min_ = declare_parameter("a_ego_min"); + a_obj_min_ = declare_parameter("a_obj_min"); + prediction_time_horizon_ = declare_parameter("prediction_time_horizon"); + prediction_time_interval_ = declare_parameter("prediction_time_interval"); + + const auto collision_keeping_sec = declare_parameter("collision_keeping_sec"); + collision_data_keeper_.setTimeout(collision_keeping_sec); + + // start time + const double aeb_hz = declare_parameter("aeb_hz"); + const auto period_ns = rclcpp::Rate(aeb_hz).period(); + timer_ = rclcpp::create_timer(this, get_clock(), period_ns, std::bind(&AEB::onTimer, this)); +} + +void AEB::onTimer() { updater_.force_update(); } + +void AEB::onVelocity(const VelocityReport::ConstSharedPtr input_msg) +{ + current_velocity_ptr_ = input_msg; +} + +void AEB::onImu(const Imu::ConstSharedPtr input_msg) { imu_ptr_ = input_msg; } + +void AEB::onPredictedTrajectory( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg) +{ + predicted_traj_ptr_ = input_msg; +} + +void AEB::onAutowareState(const AutowareState::ConstSharedPtr input_msg) +{ + autoware_state_ = input_msg; +} + +void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) +{ + PointCloud::Ptr pointcloud_ptr(new PointCloud); + pcl::fromROSMsg(*input_msg, *pointcloud_ptr); + + if (input_msg->header.frame_id != "base_link") { + RCLCPP_ERROR_STREAM( + get_logger(), + "[AEB]: Input point cloud frame is not base_link and it is " << input_msg->header.frame_id); + // transform pointcloud + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_.lookupTransform( + "base_link", input_msg->header.frame_id, input_msg->header.stamp, + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + get_logger(), + "[AEB] Failed to look up transform from base_link to" << input_msg->header.frame_id); + return; + } + + // transform by using eigen matrix + PointCloud2 transformed_points{}; + const Eigen::Matrix4f affine_matrix = + tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + pcl_ros::transformPointCloud(affine_matrix, *input_msg, transformed_points); + pcl::fromROSMsg(transformed_points, *pointcloud_ptr); + } + + pcl::VoxelGrid filter; + PointCloud::Ptr no_height_filtered_pointcloud_ptr(new PointCloud); + filter.setInputCloud(pointcloud_ptr); + filter.setLeafSize(voxel_grid_x_, voxel_grid_y_, voxel_grid_z_); + filter.filter(*no_height_filtered_pointcloud_ptr); + + obstacle_ros_pointcloud_ptr_ = std::make_shared(); + pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_); + obstacle_ros_pointcloud_ptr_->header = input_msg->header; + pub_obstacle_pointcloud_->publish(*obstacle_ros_pointcloud_ptr_); +} + +bool AEB::isDataReady() +{ + const auto missing = [this](const auto & name) { + RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "[AEB] waiting for %s", name); + return false; + }; + + if (!current_velocity_ptr_) { + return missing("ego velocity"); + } + + if (!obstacle_ros_pointcloud_ptr_) { + return missing("object pointcloud"); + } + + if (use_imu_path_ && !imu_ptr_) { + return missing("imu"); + } + + if (use_predicted_trajectory_ && !predicted_traj_ptr_) { + return missing("control predicted trajectory"); + } + + if (!autoware_state_) { + return missing("autoware_state"); + } + + return true; +} + +void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) +{ + MarkerArray debug_markers; + checkCollision(debug_markers); + + if (!collision_data_keeper_.checkExpired()) { + const std::string error_msg = "[AEB]: Emergency Brake"; + const auto diag_level = DiagnosticStatus::ERROR; + stat.summary(diag_level, error_msg); + const auto & data = collision_data_keeper_.get(); + stat.addf("RSS", "%.2f", data.rss); + stat.addf("Distance", "%.2f", data.distance_to_object); + addCollisionMarker(data, debug_markers); + } else { + const std::string error_msg = "[AEB]: No Collision"; + const auto diag_level = DiagnosticStatus::OK; + stat.summary(diag_level, error_msg); + } + + // publish debug markers + debug_ego_path_publisher_->publish(debug_markers); +} + +bool AEB::checkCollision(MarkerArray & debug_markers) +{ + // step1. check data + if (!isDataReady()) { + return false; + } + + // if not driving, disable aeb + if (autoware_state_->state != AutowareState::DRIVING) { + return false; + } + + // step2. create velocity data check if the vehicle stops or not + const double current_v = current_velocity_ptr_->longitudinal_velocity; + if (current_v < 0.1) { + return false; + } + + // step3. create ego path based on sensor data + bool has_collision_ego = false; + if (use_imu_path_) { + Path ego_path; + std::vector ego_polys; + const double current_w = imu_ptr_->angular_velocity.z; + constexpr double color_r = 0.0 / 256.0; + constexpr double color_g = 148.0 / 256.0; + constexpr double color_b = 205.0 / 256.0; + constexpr double color_a = 0.999; + const auto current_time = get_clock()->now(); + generateEgoPath(current_v, current_w, ego_path, ego_polys); + + std::vector objects; + createObjectData(ego_path, ego_polys, current_time, objects); + has_collision_ego = hasCollision(current_v, ego_path, objects); + + std::string ns = "ego"; + addMarker( + current_time, ego_path, ego_polys, objects, color_r, color_g, color_b, color_a, ns, + debug_markers); + } + + // step4. transform predicted trajectory from control module + bool has_collision_predicted = false; + if (use_predicted_trajectory_) { + Path predicted_path; + std::vector predicted_polys; + const auto predicted_traj_ptr = predicted_traj_ptr_; + constexpr double color_r = 0.0; + constexpr double color_g = 100.0 / 256.0; + constexpr double color_b = 0.0; + constexpr double color_a = 0.999; + const auto current_time = predicted_traj_ptr->header.stamp; + generateEgoPath(*predicted_traj_ptr, predicted_path, predicted_polys); + std::vector objects; + createObjectData(predicted_path, predicted_polys, current_time, objects); + has_collision_predicted = hasCollision(current_v, predicted_path, objects); + + std::string ns = "predicted"; + addMarker( + current_time, predicted_path, predicted_polys, objects, color_r, color_g, color_b, color_a, + ns, debug_markers); + } + + return has_collision_ego || has_collision_predicted; +} + +bool AEB::hasCollision( + const double current_v, const Path & ego_path, const std::vector & objects) +{ + // calculate RSS + const auto current_p = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); + const double & t = t_response_; + for (const auto & obj : objects) { + const double & obj_v = obj.velocity; + const double rss_dist = current_v * t + (current_v * current_v) / (2 * std::fabs(a_ego_min_)) - + obj_v * obj_v / (2 * std::fabs(a_obj_min_)) + longitudinal_offset_; + const double dist_ego_to_object = + tier4_autoware_utils::calcSignedArcLength(ego_path, current_p, obj.position) - + vehicle_info_.max_longitudinal_offset_m; + if (dist_ego_to_object < rss_dist) { + // collision happens + ObjectData collision_data = obj; + collision_data.rss = rss_dist; + collision_data.distance_to_object = dist_ego_to_object; + collision_data_keeper_.update(collision_data); + return true; + } + } + + return false; +} + +void AEB::generateEgoPath( + const double curr_v, const double curr_w, Path & path, std::vector & polygons) +{ + double curr_x = 0.0; + double curr_y = 0.0; + double curr_yaw = 0.0; + geometry_msgs::msg::Pose ini_pose; + ini_pose.position = tier4_autoware_utils::createPoint(curr_x, curr_y, 0.0); + ini_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(curr_yaw); + path.push_back(ini_pose); + + if (curr_v < 0.1) { + // if current velocity is too small, assume it stops at the same point + return; + } + + constexpr double epsilon = 1e-6; + const double & dt = prediction_time_interval_; + const double & horizon = prediction_time_horizon_; + for (double t = 0.0; t < horizon + epsilon; t += dt) { + curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt; + curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; + curr_yaw = curr_yaw + curr_w * dt; + geometry_msgs::msg::Pose current_pose; + current_pose.position = tier4_autoware_utils::createPoint(curr_x, curr_y, 0.0); + current_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(curr_yaw); + if (tier4_autoware_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { + continue; + } + path.push_back(current_pose); + } + + // If path is shorter than minimum path length + while (tier4_autoware_utils::calcArcLength(path) < min_generated_path_length_) { + curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt; + curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; + curr_yaw = curr_yaw + curr_w * dt; + geometry_msgs::msg::Pose current_pose; + current_pose.position = tier4_autoware_utils::createPoint(curr_x, curr_y, 0.0); + current_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(curr_yaw); + if (tier4_autoware_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { + continue; + } + path.push_back(current_pose); + } + + // generate ego polygons + polygons.resize(path.size() - 1); + for (size_t i = 0; i < path.size() - 1; ++i) { + polygons.at(i) = createPolygon(path.at(i), path.at(i + 1), vehicle_info_, expand_width_); + } +} + +void AEB::generateEgoPath( + const Trajectory & predicted_traj, Path & path, + std::vector & polygons) +{ + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_.lookupTransform( + "base_link", predicted_traj.header.frame_id, predicted_traj.header.stamp, + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM(get_logger(), "[AEB] Failed to look up transform from base_link to map"); + return; + } + + // create path + path.resize(predicted_traj.points.size()); + for (size_t i = 0; i < predicted_traj.points.size(); ++i) { + geometry_msgs::msg::Pose map_pose; + tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped); + path.at(i) = map_pose; + } + + // create polygon + polygons.resize(path.size()); + for (size_t i = 0; i < path.size() - 1; ++i) { + polygons.at(i) = createPolygon(path.at(i), path.at(i + 1), vehicle_info_, expand_width_); + } +} + +void AEB::createObjectData( + const Path & ego_path, const std::vector & ego_polys, + const rclcpp::Time & stamp, std::vector & objects) +{ + // check if the predicted path has valid number of points + if (ego_path.size() < 2 || ego_polys.empty()) { + return; + } + + PointCloud::Ptr obstacle_points_ptr(new PointCloud); + pcl::fromROSMsg(*obstacle_ros_pointcloud_ptr_, *obstacle_points_ptr); + for (const auto & point : obstacle_points_ptr->points) { + ObjectData obj; + obj.stamp = stamp; + obj.position = tier4_autoware_utils::createPoint(point.x, point.y, point.z); + obj.velocity = 0.0; + const Point2d obj_point(point.x, point.y); + const double lat_dist = tier4_autoware_utils::calcLateralOffset(ego_path, obj.position); + if (lat_dist > 5.0) { + continue; + } + for (const auto & ego_poly : ego_polys) { + if (bg::within(obj_point, ego_poly)) { + objects.push_back(obj); + break; + } + } + } +} + +void AEB::addMarker( + const rclcpp::Time & current_time, const Path & path, const std::vector & polygons, + const std::vector & objects, const double color_r, const double color_g, + const double color_b, const double color_a, const std::string & ns, MarkerArray & debug_markers) +{ + auto path_marker = tier4_autoware_utils::createDefaultMarker( + "base_link", current_time, ns + "_path", 0L, Marker::LINE_STRIP, + tier4_autoware_utils::createMarkerScale(0.2, 0.2, 0.2), + tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + path_marker.points.resize(path.size()); + for (size_t i = 0; i < path.size(); ++i) { + path_marker.points.at(i) = path.at(i).position; + } + debug_markers.markers.push_back(path_marker); + + auto polygon_marker = tier4_autoware_utils::createDefaultMarker( + "base_link", current_time, ns + "_polygon", 0, Marker::LINE_LIST, + tier4_autoware_utils::createMarkerScale(0.03, 0.0, 0.0), + tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + for (const auto & poly : polygons) { + for (size_t dp_idx = 0; dp_idx < poly.outer().size(); ++dp_idx) { + const auto & boost_cp = poly.outer().at(dp_idx); + const auto & boost_np = poly.outer().at((dp_idx + 1) % poly.outer().size()); + const auto curr_point = tier4_autoware_utils::createPoint(boost_cp.x(), boost_cp.y(), 0.0); + const auto next_point = tier4_autoware_utils::createPoint(boost_np.x(), boost_np.y(), 0.0); + polygon_marker.points.push_back(curr_point); + polygon_marker.points.push_back(next_point); + } + } + debug_markers.markers.push_back(polygon_marker); + + auto object_data_marker = tier4_autoware_utils::createDefaultMarker( + "base_link", current_time, ns + "_objects", 0, Marker::SPHERE_LIST, + tier4_autoware_utils::createMarkerScale(0.05, 0.05, 0.05), + tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + for (const auto & e : objects) { + object_data_marker.points.push_back(e.position); + } + debug_markers.markers.push_back(object_data_marker); +} + +void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers) +{ + auto point_marker = tier4_autoware_utils::createDefaultMarker( + "base_link", data.stamp, "collision_point", 0, Marker::SPHERE, + tier4_autoware_utils::createMarkerScale(0.3, 0.3, 0.3), + tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.3)); + point_marker.pose.position = data.position; + debug_markers.markers.push_back(point_marker); +} + +} // namespace autoware::motion::control::autonomous_emergency_braking + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::motion::control::autonomous_emergency_braking::AEB) diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index c49c280e2647b..6e3340dcd5703 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -55,6 +55,9 @@ def launch_setup(context, *args, **kwargs): with open(lane_departure_checker_param_path, "r") as f: lane_departure_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("aeb_param_path").perform(context), "r") as f: + aeb_param = yaml.safe_load(f)["/**"]["ros__parameters"] + # lateral controller lat_controller_component = ComposableNode( package="trajectory_follower_nodes", @@ -151,6 +154,33 @@ def launch_setup(context, *args, **kwargs): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) + # autonomous emergency braking + autonomous_emergency_braking = ComposableNode( + package="autonomous_emergency_braking", + plugin="autoware::motion::control::autonomous_emergency_braking::AEB", + name="autonomous_emergency_braking", + remappings=[ + ("~/input/pointcloud", "/perception/obstacle_segmentation/pointcloud"), + ("~/input/velocity", "/vehicle/status/velocity_status"), + ("~/input/imu", "/sensing/imu/imu_data"), + ("~/input/odometry", "/localization/kinematic_state"), + ( + "~/input/predicted_trajectory", + "/control/trajectory_follower/lateral/predicted_trajectory", + ), + ], + parameters=[ + aeb_param, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + autonomous_emergency_braking_loader = LoadComposableNodes( + condition=IfCondition(LaunchConfiguration("enable_autonomous_emergency_braking")), + composable_node_descriptions=[autonomous_emergency_braking], + target_container="/control/control_container", + ) + # vehicle cmd gate vehicle_cmd_gate_component = ComposableNode( package="vehicle_cmd_gate", @@ -251,6 +281,7 @@ def launch_setup(context, *args, **kwargs): external_cmd_selector_loader, external_cmd_converter_loader, lat_controller_loader, + autonomous_emergency_braking_loader, ] ) @@ -333,6 +364,10 @@ def add_launch_arg(name: str, default_value=None, description=None): # external cmd selector add_launch_arg("initial_selector_mode", "remote", "local or remote") + # aeb + add_launch_arg("aeb_param_path") + add_launch_arg("enable_autonomous_emergency_braking") + # component add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") add_launch_arg("use_multithread", "false", "use multithread") diff --git a/system/system_error_monitor/config/diagnostic_aggregator/control.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/control.param.yaml index 0a255b7ef7901..de9de794eecec 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/control.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/control.param.yaml @@ -4,6 +4,20 @@ type: diagnostic_aggregator/AnalyzerGroup path: control analyzers: + autonomous_emergency_braking: + type: diagnostic_aggregator/AnalyzerGroup + path: autonomous_emergency_braking + analyzers: + performance_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: performance_monitoring + analyzers: + emergency_stop: + type: diagnostic_aggregator/GenericAnalyzer + path: emergency_stop + contains: [": aeb_emergency_stop"] + timeout: 1.0 + control_command_gate: type: diagnostic_aggregator/AnalyzerGroup path: control_command_gate diff --git a/system/system_error_monitor/config/system_error_monitor.param.yaml b/system/system_error_monitor/config/system_error_monitor.param.yaml index b9a5a4fa79f52..282c3b919867b 100644 --- a/system/system_error_monitor/config/system_error_monitor.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.param.yaml @@ -20,6 +20,7 @@ /autoware/control/autonomous_driving/node_alive_monitoring: default /autoware/control/autonomous_driving/performance_monitoring/lane_departure: default /autoware/control/control_command_gate/node_alive_monitoring: default + /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} /autoware/localization/node_alive_monitoring: default /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" } @@ -42,6 +43,7 @@ external_control: /autoware/control/control_command_gate/node_alive_monitoring: default + /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} /autoware/control/external_control/external_command_selector/node_alive_monitoring: default /autoware/system/node_alive_monitoring: default diff --git a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml index ce081b75b2836..271555d1d1dc8 100644 --- a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml @@ -20,6 +20,7 @@ /autoware/control/autonomous_driving/node_alive_monitoring: default /autoware/control/autonomous_driving/performance_monitoring/lane_departure: default /autoware/control/control_command_gate/node_alive_monitoring: default + /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} /autoware/localization/node_alive_monitoring: default # /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" } @@ -42,6 +43,7 @@ external_control: /autoware/control/control_command_gate/node_alive_monitoring: default + /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} /autoware/control/external_control/external_command_selector/node_alive_monitoring: default /autoware/system/node_alive_monitoring: default From 2ff3e6a3a59007bacc7717af31153baf2e127a0b Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 9 May 2023 10:41:42 +0900 Subject: [PATCH 22/92] chore(web_controller): add COLCON_IGNORE (#382) * chore(common): add COLCON_IGNORE * Delete COLCON_IGNORE --- common/web_controller/COLCON_IGNORE | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 common/web_controller/COLCON_IGNORE diff --git a/common/web_controller/COLCON_IGNORE b/common/web_controller/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d From 5a3add705eec4f5926366074ba4c136f2f875219 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 9 May 2023 18:52:16 +0900 Subject: [PATCH 23/92] feat(tier4_debug_tools): add COLCON_IGNORE (#415) --- common/tier4_debug_tools/COLCON_IGNORE | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 common/tier4_debug_tools/COLCON_IGNORE diff --git a/common/tier4_debug_tools/COLCON_IGNORE b/common/tier4_debug_tools/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d From 9f3319642b8511f0d155620f4f507c460d0ade8a Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 9 May 2023 18:52:38 +0900 Subject: [PATCH 24/92] feat(tier4_calibration_rviz_plugin): add COLCON_IGNORE (#414) --- common/tier4_calibration_rviz_plugin/COLCON_IGNORE | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 common/tier4_calibration_rviz_plugin/COLCON_IGNORE diff --git a/common/tier4_calibration_rviz_plugin/COLCON_IGNORE b/common/tier4_calibration_rviz_plugin/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d From 7da79c7d4a2f8b7bac24f49aca6fb7072cceba90 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 9 May 2023 18:52:56 +0900 Subject: [PATCH 25/92] feat(tier4_traffic_light_rviz_plugin): add COLCON_IGNORE (#394) * Create COLCON_IGNORE * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- common/tier4_traffic_light_rviz_plugin/COLCON_IGNORE | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 common/tier4_traffic_light_rviz_plugin/COLCON_IGNORE diff --git a/common/tier4_traffic_light_rviz_plugin/COLCON_IGNORE b/common/tier4_traffic_light_rviz_plugin/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d From 79a61568503973fc452c4840364176ca8577f848 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Tue, 9 May 2023 19:05:49 +0900 Subject: [PATCH 26/92] chore(GNSS): add COLCON_IGNORE (#396) chore(GNSS) add COLCON_IGNORE --- sensing/geo_pos_conv/COLCON_IGNORE | 0 sensing/gnss_poser/COLCON_IGNORE | 0 2 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 sensing/geo_pos_conv/COLCON_IGNORE create mode 100644 sensing/gnss_poser/COLCON_IGNORE diff --git a/sensing/geo_pos_conv/COLCON_IGNORE b/sensing/geo_pos_conv/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/sensing/gnss_poser/COLCON_IGNORE b/sensing/gnss_poser/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d From 514381f5c9c2f25d4e08015381a93db9fd454696 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Wed, 10 May 2023 13:49:25 +0900 Subject: [PATCH 27/92] feat(launch): add COLCON_IGNORE (#381) chore(launch): add COLCON_IGNORE --- launch/COLCON_IGNORE | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 launch/COLCON_IGNORE diff --git a/launch/COLCON_IGNORE b/launch/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d From 972c08e5370ea7803564789f6200bade59eac3d8 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Wed, 10 May 2023 15:06:21 +0900 Subject: [PATCH 28/92] chore(planning_evaluator): add COLCON_IGNORE (#410) * Create COLCON_IGNORE * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- planning/planning_evaluator/COLCON_IGNORE | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 planning/planning_evaluator/COLCON_IGNORE diff --git a/planning/planning_evaluator/COLCON_IGNORE b/planning/planning_evaluator/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d From fb0e8478deb4000de49a66d208bd2eace6d33f19 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Thu, 11 May 2023 13:24:28 +0900 Subject: [PATCH 29/92] feat(detection, tracking, prediction): add COLCON_IGNORE (#383) * chore(perception): add COLCON_IGNORE * delete COLCON_IGNORE * Delete COLCON_IGNORE * Delete COLCON_IGNORE * Delete COLCON_IGNORE * Delete COLCON_IGNORE * Delete COLCON_IGNORE --- perception/detected_object_validation/COLCON_IGNORE | 0 perception/detection_by_tracker/COLCON_IGNORE | 0 perception/euclidean_cluster/COLCON_IGNORE | 0 perception/lidar_apollo_instance_segmentation/COLCON_IGNORE | 0 perception/lidar_centerpoint/COLCON_IGNORE | 0 perception/map_based_prediction/COLCON_IGNORE | 0 perception/multi_object_tracker/COLCON_IGNORE | 0 perception/object_merger/COLCON_IGNORE | 0 perception/object_range_splitter/COLCON_IGNORE | 0 perception/roi_cluster_fusion/COLCON_IGNORE | 0 perception/tensorrt_yolo/COLCON_IGNORE | 0 11 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 perception/detected_object_validation/COLCON_IGNORE create mode 100644 perception/detection_by_tracker/COLCON_IGNORE create mode 100644 perception/euclidean_cluster/COLCON_IGNORE create mode 100644 perception/lidar_apollo_instance_segmentation/COLCON_IGNORE create mode 100644 perception/lidar_centerpoint/COLCON_IGNORE create mode 100644 perception/map_based_prediction/COLCON_IGNORE create mode 100644 perception/multi_object_tracker/COLCON_IGNORE create mode 100644 perception/object_merger/COLCON_IGNORE create mode 100644 perception/object_range_splitter/COLCON_IGNORE create mode 100644 perception/roi_cluster_fusion/COLCON_IGNORE create mode 100644 perception/tensorrt_yolo/COLCON_IGNORE diff --git a/perception/detected_object_validation/COLCON_IGNORE b/perception/detected_object_validation/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/perception/detection_by_tracker/COLCON_IGNORE b/perception/detection_by_tracker/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/perception/euclidean_cluster/COLCON_IGNORE b/perception/euclidean_cluster/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/perception/lidar_apollo_instance_segmentation/COLCON_IGNORE b/perception/lidar_apollo_instance_segmentation/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/perception/lidar_centerpoint/COLCON_IGNORE b/perception/lidar_centerpoint/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/perception/map_based_prediction/COLCON_IGNORE b/perception/map_based_prediction/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/perception/multi_object_tracker/COLCON_IGNORE b/perception/multi_object_tracker/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/perception/object_merger/COLCON_IGNORE b/perception/object_merger/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/perception/object_range_splitter/COLCON_IGNORE b/perception/object_range_splitter/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/perception/roi_cluster_fusion/COLCON_IGNORE b/perception/roi_cluster_fusion/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/perception/tensorrt_yolo/COLCON_IGNORE b/perception/tensorrt_yolo/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d From c53ca203f9f97e2f4231ba7761efdbb17786bd32 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Thu, 11 May 2023 13:27:15 +0900 Subject: [PATCH 30/92] feat(parking): add COLCON_IGNORE (#389) chore(parking): add COLCON_IGNORE --- planning/costmap_generator/COLCON_IGNORE | 0 planning/freespace_planner/COLCON_IGNORE | 0 planning/freespace_planning_algorithms/COLCON_IGNORE | 0 3 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 planning/costmap_generator/COLCON_IGNORE create mode 100644 planning/freespace_planner/COLCON_IGNORE create mode 100644 planning/freespace_planning_algorithms/COLCON_IGNORE diff --git a/planning/costmap_generator/COLCON_IGNORE b/planning/costmap_generator/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/planning/freespace_planner/COLCON_IGNORE b/planning/freespace_planner/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/planning/freespace_planning_algorithms/COLCON_IGNORE b/planning/freespace_planning_algorithms/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d From dbd006284fc548ee876457c975dc2e96e2ed2063 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Thu, 11 May 2023 13:36:01 +0900 Subject: [PATCH 31/92] feat(control): add COLCON_IGNORE (#390) * chore(control): add COLCON_IGNORE * Delete COLCON_IGNORE --- control/control_performance_analysis/COLCON_IGNORE | 0 control/joy_controller/COLCON_IGNORE | 0 control/obstacle_collision_checker/COLCON_IGNORE | 0 3 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 control/control_performance_analysis/COLCON_IGNORE create mode 100644 control/joy_controller/COLCON_IGNORE create mode 100644 control/obstacle_collision_checker/COLCON_IGNORE diff --git a/control/control_performance_analysis/COLCON_IGNORE b/control/control_performance_analysis/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/control/joy_controller/COLCON_IGNORE b/control/joy_controller/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/control/obstacle_collision_checker/COLCON_IGNORE b/control/obstacle_collision_checker/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d From 5002c1120b26c51bff0d0bf8a3ec3be6e9388505 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Thu, 11 May 2023 13:36:12 +0900 Subject: [PATCH 32/92] feat(traffic_light): add COLCON_IGNORE (#395) chore(traffic_light): add COLCON_IGNORE --- perception/traffic_light_classifier/COLCON_IGNORE | 0 perception/traffic_light_map_based_detector/COLCON_IGNORE | 0 perception/traffic_light_ssd_fine_detector/COLCON_IGNORE | 0 perception/traffic_light_visualization/COLCON_IGNORE | 0 4 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 perception/traffic_light_classifier/COLCON_IGNORE create mode 100644 perception/traffic_light_map_based_detector/COLCON_IGNORE create mode 100644 perception/traffic_light_ssd_fine_detector/COLCON_IGNORE create mode 100644 perception/traffic_light_visualization/COLCON_IGNORE diff --git a/perception/traffic_light_classifier/COLCON_IGNORE b/perception/traffic_light_classifier/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/perception/traffic_light_map_based_detector/COLCON_IGNORE b/perception/traffic_light_map_based_detector/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/perception/traffic_light_ssd_fine_detector/COLCON_IGNORE b/perception/traffic_light_ssd_fine_detector/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/perception/traffic_light_visualization/COLCON_IGNORE b/perception/traffic_light_visualization/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d From 01ebea8e3b393af9c42fbd1ce2dcfcc141c7f21c Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Thu, 11 May 2023 17:58:57 +0900 Subject: [PATCH 33/92] fix(ci): fix action for Humble (#426) --- .../build-and-test-differential-self-hosted.yaml | 4 ++-- .github/workflows/build-and-test-differential.yaml | 10 +++++----- .github/workflows/build-and-test-self-hosted.yaml | 4 ++-- .github/workflows/build-and-test.yaml | 4 ++-- .github/workflows/check-build-depends.yaml | 4 ++-- 5 files changed, 13 insertions(+), 13 deletions(-) diff --git a/.github/workflows/build-and-test-differential-self-hosted.yaml b/.github/workflows/build-and-test-differential-self-hosted.yaml index a15ced5f3b0c8..81e65e2ddaf38 100644 --- a/.github/workflows/build-and-test-differential-self-hosted.yaml +++ b/.github/workflows/build-and-test-differential-self-hosted.yaml @@ -36,7 +36,7 @@ jobs: if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} uses: autowarefoundation/autoware-github-actions/colcon-build@v1 with: - rosdistro: galactic + rosdistro: humble target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} build-depends-repos: build_depends.repos @@ -44,6 +44,6 @@ jobs: if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} uses: autowarefoundation/autoware-github-actions/colcon-test@v1 with: - rosdistro: galactic + rosdistro: humble target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} build-depends-repos: build_depends.repos diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index 9939b469151ea..f21ce6c8be00c 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -6,7 +6,7 @@ on: jobs: build-and-test-differential: runs-on: ubuntu-latest - container: ghcr.io/autowarefoundation/autoware-universe:galactic-latest + container: ghcr.io/autowarefoundation/autoware-universe:latest steps: - name: Cancel previous runs uses: styfle/cancel-workflow-action@0.9.1 @@ -27,7 +27,7 @@ jobs: if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} uses: autowarefoundation/autoware-github-actions/colcon-build@a7cc2c1ce6052f395e5800a0fb6e6221421bf30a with: - rosdistro: galactic + rosdistro: humble target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} build-depends-repos: build_depends.repos include-eol-distros: true @@ -37,7 +37,7 @@ jobs: if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} uses: autowarefoundation/autoware-github-actions/colcon-test@a7cc2c1ce6052f395e5800a0fb6e6221421bf30a with: - rosdistro: galactic + rosdistro: humble target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} build-depends-repos: build_depends.repos include-eol-distros: true @@ -53,7 +53,7 @@ jobs: clang-tidy-differential: runs-on: ubuntu-latest - container: ros:galactic + container: ros:humble needs: build-and-test-differential steps: - name: Check out repository @@ -72,7 +72,7 @@ jobs: if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} uses: autowarefoundation/autoware-github-actions/clang-tidy@a7cc2c1ce6052f395e5800a0fb6e6221421bf30a with: - rosdistro: galactic + rosdistro: humble target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy build-depends-repos: build_depends.repos diff --git a/.github/workflows/build-and-test-self-hosted.yaml b/.github/workflows/build-and-test-self-hosted.yaml index 9f31a07199783..26416a3207668 100644 --- a/.github/workflows/build-and-test-self-hosted.yaml +++ b/.github/workflows/build-and-test-self-hosted.yaml @@ -24,7 +24,7 @@ jobs: if: ${{ steps.get-self-packages.outputs.self-packages != '' }} uses: autowarefoundation/autoware-github-actions/colcon-build@v1 with: - rosdistro: galactic + rosdistro: humble target-packages: ${{ steps.get-self-packages.outputs.self-packages }} build-depends-repos: build_depends.repos @@ -32,6 +32,6 @@ jobs: if: ${{ steps.get-self-packages.outputs.self-packages != '' }} uses: autowarefoundation/autoware-github-actions/colcon-test@v1 with: - rosdistro: galactic + rosdistro: humble target-packages: ${{ steps.get-self-packages.outputs.self-packages }} build-depends-repos: build_depends.repos diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index f0760d99aebf1..5e68c8ed8c690 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -26,7 +26,7 @@ jobs: if: ${{ steps.get-self-packages.outputs.self-packages != '' }} uses: autowarefoundation/autoware-github-actions/colcon-build@v1 with: - rosdistro: galactic + rosdistro: humble target-packages: ${{ steps.get-self-packages.outputs.self-packages }} build-depends-repos: build_depends.repos @@ -35,7 +35,7 @@ jobs: id: test uses: autowarefoundation/autoware-github-actions/colcon-test@v1 with: - rosdistro: galactic + rosdistro: humble target-packages: ${{ steps.get-self-packages.outputs.self-packages }} build-depends-repos: build_depends.repos diff --git a/.github/workflows/check-build-depends.yaml b/.github/workflows/check-build-depends.yaml index 0894e2aabe52e..7fb0f7f984afb 100644 --- a/.github/workflows/check-build-depends.yaml +++ b/.github/workflows/check-build-depends.yaml @@ -8,7 +8,7 @@ on: jobs: check-build-depends: runs-on: ubuntu-latest - container: ghcr.io/autowarefoundation/autoware-universe:galactic-latest + container: ghcr.io/autowarefoundation/autoware-universe:latest steps: - name: Cancel previous runs uses: styfle/cancel-workflow-action@0.9.1 @@ -26,7 +26,7 @@ jobs: - name: Build uses: autowarefoundation/autoware-github-actions/colcon-build@a7cc2c1ce6052f395e5800a0fb6e6221421bf30a with: - rosdistro: galactic + rosdistro: humble target-packages: ${{ steps.get-self-packages.outputs.self-packages }} build-depends-repos: build_depends.repos include-eol-distros: true From 47089cec3e7c2beab8a0af80db8be86deb67840c Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 12 May 2023 10:09:27 +0900 Subject: [PATCH 34/92] fix: remove unused orocos kdl from dependencies (#848) (#425) * fix: remove unused orocos kdl from dependencies (#848) Signed-off-by: Daisuke Nishimatsu * fix(ci): fix action for Humble --------- Signed-off-by: Daisuke Nishimatsu Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> --- common/autoware_auto_tf2/CMakeLists.txt | 1 - common/autoware_auto_tf2/package.xml | 1 - 2 files changed, 2 deletions(-) diff --git a/common/autoware_auto_tf2/CMakeLists.txt b/common/autoware_auto_tf2/CMakeLists.txt index edeeecd61dbe5..10d836f7c5069 100755 --- a/common/autoware_auto_tf2/CMakeLists.txt +++ b/common/autoware_auto_tf2/CMakeLists.txt @@ -37,7 +37,6 @@ if(BUILD_TESTING) "autoware_auto_system_msgs" "autoware_auto_geometry_msgs" "geometry_msgs" - "orocos_kdl" "tf2" "tf2_ros" ) diff --git a/common/autoware_auto_tf2/package.xml b/common/autoware_auto_tf2/package.xml index 27f07c37912d4..af2c7327bbaaf 100644 --- a/common/autoware_auto_tf2/package.xml +++ b/common/autoware_auto_tf2/package.xml @@ -17,7 +17,6 @@ geometry_msgs tf2 tf2_ros - orocos_kdl ament_cmake_gtest From 0c480dcf84d3864cfb987f66d2760a613271db17 Mon Sep 17 00:00:00 2001 From: v-sugahara7993-esol <109567391+v-sugahara7993-esol@users.noreply.github.com> Date: Fri, 12 May 2023 11:08:00 +0900 Subject: [PATCH 35/92] fix(autoware_auto_common): fix build dependency on geometry_msgs (#428) Signed-off-by: v-sugahara7993-esol --- common/autoware_auto_common/CMakeLists.txt | 6 +++++- common/autoware_auto_common/package.xml | 1 + 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/common/autoware_auto_common/CMakeLists.txt b/common/autoware_auto_common/CMakeLists.txt index 45284843f5644..4c3b140fc440e 100644 --- a/common/autoware_auto_common/CMakeLists.txt +++ b/common/autoware_auto_common/CMakeLists.txt @@ -66,7 +66,11 @@ if(BUILD_TESTING) autoware_set_compile_options(${TEST_COMMON}) target_compile_options(${TEST_COMMON} PRIVATE -Wno-sign-conversion) target_include_directories(${TEST_COMMON} PRIVATE include) - ament_target_dependencies(${TEST_COMMON} builtin_interfaces Eigen3) + ament_target_dependencies(${TEST_COMMON} + builtin_interfaces + Eigen3 + geometry_msgs + ) endif() # Ament Exporting diff --git a/common/autoware_auto_common/package.xml b/common/autoware_auto_common/package.xml index b79ac8cbe2291..c34be9ca28577 100644 --- a/common/autoware_auto_common/package.xml +++ b/common/autoware_auto_common/package.xml @@ -16,6 +16,7 @@ ament_cmake_gtest ament_lint_auto ament_lint_common + geometry_msgs ament_cmake From e5ea2541dcb7cee45b31e3c6d37ebd3238cf36fb Mon Sep 17 00:00:00 2001 From: v-sugahara7993-esol <109567391+v-sugahara7993-esol@users.noreply.github.com> Date: Fri, 12 May 2023 11:38:39 +0900 Subject: [PATCH 36/92] fix(tier4_autoware_utils): change the header extension of the included tf2_geometry_msgs from .h to .hpp (#427) * fix(tier4_autoware_utils): change the header extension of the included tf2_geometry_msgs from .h to .hpp Signed-off-by: v-sugahara7993-esol * ci(pre-commit): autofix --------- Signed-off-by: v-sugahara7993-esol Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Hiroki OTA --- .../include/tier4_autoware_utils/geometry/geometry.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp index 4ec7bbad282d9..84ccf05d5ba4f 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp @@ -31,8 +31,7 @@ #include #include #include - -#include +#include namespace tier4_autoware_utils { From 25f8dfef33abcae1f7b8557e829f0b13a922c539 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 12 May 2023 13:27:09 +0900 Subject: [PATCH 37/92] ci: delete grid_map from build_depends.repos (#431) Update build_depends.repos --- build_depends.repos | 4 ---- 1 file changed, 4 deletions(-) diff --git a/build_depends.repos b/build_depends.repos index ad92967c0e48b..f16cfff7e4770 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -17,10 +17,6 @@ repositories: type: git url: https://github.com/tier4/tier4_autoware_msgs.git version: a624d255107fa724cf90967c5dcc09463b1d1c73 - universe/vendor/grid_map: - type: git - url: https://github.com/ANYbotics/grid_map.git - version: ba2f9cb6e62f7ee9c5bac7401391a211e442e459 universe/vendor/mussp: type: git url: https://github.com/tier4/muSSP.git From b479953e5c40129b2f1a3ca27f0c5e925f3c9954 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 12 May 2023 15:36:45 +0900 Subject: [PATCH 38/92] fix(pure_pursuit): fix build error in Humble (#434) * fix(pure_pursuit): fix build error for Humble * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../pure_pursuit/include/pure_pursuit/util/planning_utils.hpp | 3 ++- control/pure_pursuit/include/pure_pursuit/util/tf_utils.hpp | 3 ++- control/pure_pursuit/package.xml | 1 + 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp b/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp index c065bca252aff..57455bea73d38 100644 --- a/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp +++ b/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp @@ -22,14 +22,15 @@ #include #include #include +#include #include #include #include +#include #include #include -#include #include #include diff --git a/control/pure_pursuit/include/pure_pursuit/util/tf_utils.hpp b/control/pure_pursuit/include/pure_pursuit/util/tf_utils.hpp index b9ec0beaebbd9..773623d2b378d 100644 --- a/control/pure_pursuit/include/pure_pursuit/util/tf_utils.hpp +++ b/control/pure_pursuit/include/pure_pursuit/util/tf_utils.hpp @@ -17,9 +17,10 @@ #include +#include + #include // To be replaced by std::optional in C++17 -#include #include #include diff --git a/control/pure_pursuit/package.xml b/control/pure_pursuit/package.xml index 878314c52a0e3..a406fb2276a49 100644 --- a/control/pure_pursuit/package.xml +++ b/control/pure_pursuit/package.xml @@ -18,6 +18,7 @@ std_msgs tf2 tf2_eigen + tf2_geometry_msgs tf2_ros tier4_autoware_utils vehicle_info_util From 990502c60e3b405efdf2321239998e5ff32ae2a8 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 12 May 2023 16:16:43 +0900 Subject: [PATCH 39/92] feat(accel_brake_map_calibrator): add COLCON_IGNORE (#436) * Create COLCON_IGNORE * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../accel_brake_map_calibrator/COLCON_IGNORE | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/COLCON_IGNORE diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/COLCON_IGNORE b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d From 9f53ba511fb0885ac41766758a82a5a74d06bb17 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 12 May 2023 17:11:16 +0900 Subject: [PATCH 40/92] fix(autoware_auto_tf2): fix build error in Humble (#437) --- common/autoware_auto_tf2/CMakeLists.txt | 1 + .../include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp | 2 +- common/autoware_auto_tf2/package.xml | 1 + common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp | 2 +- 4 files changed, 4 insertions(+), 2 deletions(-) diff --git a/common/autoware_auto_tf2/CMakeLists.txt b/common/autoware_auto_tf2/CMakeLists.txt index 10d836f7c5069..d71073f488ab8 100755 --- a/common/autoware_auto_tf2/CMakeLists.txt +++ b/common/autoware_auto_tf2/CMakeLists.txt @@ -38,6 +38,7 @@ if(BUILD_TESTING) "autoware_auto_geometry_msgs" "geometry_msgs" "tf2" + "tf2_geometry_msgs" "tf2_ros" ) endif() diff --git a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp index c8ce96b1a6eca..7cf7081064c0b 100644 --- a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp +++ b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include #include diff --git a/common/autoware_auto_tf2/package.xml b/common/autoware_auto_tf2/package.xml index af2c7327bbaaf..a8d8710972294 100644 --- a/common/autoware_auto_tf2/package.xml +++ b/common/autoware_auto_tf2/package.xml @@ -16,6 +16,7 @@ autoware_auto_common geometry_msgs tf2 + tf2_geometry_msgs tf2_ros ament_cmake_gtest diff --git a/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp b/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp index bf512b4405d57..1abc773881e77 100755 --- a/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp +++ b/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include #include From 33da0849b898eaa0796ee7af58b29d9fba26fc13 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 12 May 2023 17:14:51 +0900 Subject: [PATCH 41/92] feat(had_map_utils): delete as unused (#438) Delete common/had_map_utils directory --- common/had_map_utils/CMakeLists.txt | 43 -- .../had_map_utils/had_map_computation.hpp | 38 -- .../had_map_utils/had_map_conversion.hpp | 42 -- .../include/had_map_utils/had_map_query.hpp | 61 -- .../include/had_map_utils/had_map_utils.hpp | 45 -- .../had_map_utils/had_map_visualization.hpp | 275 --------- .../had_map_utils/visibility_control.hpp | 38 -- common/had_map_utils/package.xml | 33 -- .../had_map_utils/src/had_map_computation.cpp | 135 ----- .../had_map_utils/src/had_map_conversion.cpp | 73 --- common/had_map_utils/src/had_map_query.cpp | 132 ----- common/had_map_utils/src/had_map_utils.cpp | 175 ------ .../src/had_map_visualization.cpp | 558 ------------------ 13 files changed, 1648 deletions(-) delete mode 100644 common/had_map_utils/CMakeLists.txt delete mode 100644 common/had_map_utils/include/had_map_utils/had_map_computation.hpp delete mode 100644 common/had_map_utils/include/had_map_utils/had_map_conversion.hpp delete mode 100644 common/had_map_utils/include/had_map_utils/had_map_query.hpp delete mode 100644 common/had_map_utils/include/had_map_utils/had_map_utils.hpp delete mode 100644 common/had_map_utils/include/had_map_utils/had_map_visualization.hpp delete mode 100644 common/had_map_utils/include/had_map_utils/visibility_control.hpp delete mode 100644 common/had_map_utils/package.xml delete mode 100644 common/had_map_utils/src/had_map_computation.cpp delete mode 100644 common/had_map_utils/src/had_map_conversion.cpp delete mode 100644 common/had_map_utils/src/had_map_query.cpp delete mode 100644 common/had_map_utils/src/had_map_utils.cpp delete mode 100644 common/had_map_utils/src/had_map_visualization.cpp diff --git a/common/had_map_utils/CMakeLists.txt b/common/had_map_utils/CMakeLists.txt deleted file mode 100644 index 9667a8ceb530b..0000000000000 --- a/common/had_map_utils/CMakeLists.txt +++ /dev/null @@ -1,43 +0,0 @@ -# Copyright 2020 TierIV, Inc. -# All rights reserved. -cmake_minimum_required(VERSION 3.5) - -### Export headers -project(had_map_utils) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) -endif() - -find_package(ament_cmake_auto REQUIRED) -find_package(CGAL REQUIRED COMPONENTS Core) -find_package(Eigen3 REQUIRED) -ament_auto_find_build_dependencies() - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/had_map_utils.cpp - src/had_map_computation.cpp - src/had_map_conversion.cpp - src/had_map_query.cpp - src/had_map_visualization.cpp) - -# Disable warnings due to external dependencies (Eigen) -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC - ${EIGEN3_INCLUDE_DIR} - ${rclcpp_INCLUDE_DIRS} -) - -set(CGAL_DO_NOT_WARN_ABOUT_CMAKE_BUILD_TYPE TRUE) -target_link_libraries(${PROJECT_NAME} CGAL CGAL::CGAL CGAL::CGAL_Core) -autoware_set_compile_options(${PROJECT_NAME}) - -# if(BUILD_TESTING) -# find_package(ament_lint_auto REQUIRED) -# ament_lint_auto_find_test_dependencies() -# endif() - -ament_auto_package() diff --git a/common/had_map_utils/include/had_map_utils/had_map_computation.hpp b/common/had_map_utils/include/had_map_utils/had_map_computation.hpp deleted file mode 100644 index 35f3f3dcf31d2..0000000000000 --- a/common/had_map_utils/include/had_map_utils/had_map_computation.hpp +++ /dev/null @@ -1,38 +0,0 @@ -// Copyright 2021 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 HAD_MAP_UTILS__HAD_MAP_COMPUTATION_HPP_ -#define HAD_MAP_UTILS__HAD_MAP_COMPUTATION_HPP_ - -#include -#include - -#include "visibility_control.hpp" - -namespace autoware -{ -namespace common -{ -namespace had_map_utils -{ - -lanelet::Polygon3d HAD_MAP_UTILS_PUBLIC coalesce_drivable_areas( - const autoware_auto_planning_msgs::msg::HADMapRoute & had_map_route, - const lanelet::LaneletMapPtr & lanelet_map_ptr); - -} // namespace had_map_utils -} // namespace common -} // namespace autoware - -#endif // HAD_MAP_UTILS__HAD_MAP_COMPUTATION_HPP_ diff --git a/common/had_map_utils/include/had_map_utils/had_map_conversion.hpp b/common/had_map_utils/include/had_map_utils/had_map_conversion.hpp deleted file mode 100644 index e0c5ca8c8c9a9..0000000000000 --- a/common/had_map_utils/include/had_map_utils/had_map_conversion.hpp +++ /dev/null @@ -1,42 +0,0 @@ -// Copyright 2020 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 HAD_MAP_UTILS__HAD_MAP_CONVERSION_HPP_ -#define HAD_MAP_UTILS__HAD_MAP_CONVERSION_HPP_ - -#include -#include -#include -#include "had_map_utils/visibility_control.hpp" - -namespace autoware -{ -namespace common -{ -namespace had_map_utils -{ - -void HAD_MAP_UTILS_PUBLIC toBinaryMsg( - const std::shared_ptr & map, - autoware_auto_mapping_msgs::msg::HADMapBin & msg); - -void HAD_MAP_UTILS_PUBLIC fromBinaryMsg( - const autoware_auto_mapping_msgs::msg::HADMapBin & msg, - std::shared_ptr & map); - -} // namespace had_map_utils -} // namespace common -} // namespace autoware - -#endif // HAD_MAP_UTILS__HAD_MAP_CONVERSION_HPP_ diff --git a/common/had_map_utils/include/had_map_utils/had_map_query.hpp b/common/had_map_utils/include/had_map_utils/had_map_query.hpp deleted file mode 100644 index de51b11f5e1d2..0000000000000 --- a/common/had_map_utils/include/had_map_utils/had_map_query.hpp +++ /dev/null @@ -1,61 +0,0 @@ -// Copyright 2020 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 HAD_MAP_UTILS__HAD_MAP_QUERY_HPP_ -#define HAD_MAP_UTILS__HAD_MAP_QUERY_HPP_ - - -#include -#include -#include -#include -#include - -#include -#include -#include "had_map_utils/visibility_control.hpp" - -namespace autoware -{ -namespace common -{ -namespace had_map_utils -{ - -lanelet::Areas HAD_MAP_UTILS_PUBLIC getAreaLayer(const lanelet::LaneletMapPtr ll_map); - -lanelet::Areas HAD_MAP_UTILS_PUBLIC subtypeAreas( - const lanelet::Areas areas, const char subtype[]); - -lanelet::Polygons3d HAD_MAP_UTILS_PUBLIC getPolygonLayer(const lanelet::LaneletMapPtr ll_map); - -lanelet::Polygons3d HAD_MAP_UTILS_PUBLIC subtypePolygons( - const lanelet::Polygons3d polygons, const char subtype[]); - -lanelet::LineStrings3d HAD_MAP_UTILS_PUBLIC getLineStringLayer(const lanelet::LaneletMapPtr ll_map); - -lanelet::LineStrings3d HAD_MAP_UTILS_PUBLIC subtypeLineStrings( - const lanelet::LineStrings3d linestrings, const char subtype[]); - -lanelet::ConstLanelets HAD_MAP_UTILS_PUBLIC getConstLaneletLayer( - const std::shared_ptr & ll_map); - -lanelet::Lanelets HAD_MAP_UTILS_PUBLIC getLaneletLayer( - const std::shared_ptr & ll_map); - -} // namespace had_map_utils -} // namespace common -} // namespace autoware - -#endif // HAD_MAP_UTILS__HAD_MAP_QUERY_HPP_ diff --git a/common/had_map_utils/include/had_map_utils/had_map_utils.hpp b/common/had_map_utils/include/had_map_utils/had_map_utils.hpp deleted file mode 100644 index b5b06016bdcc9..0000000000000 --- a/common/had_map_utils/include/had_map_utils/had_map_utils.hpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2020 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 HAD_MAP_UTILS__HAD_MAP_UTILS_HPP_ -#define HAD_MAP_UTILS__HAD_MAP_UTILS_HPP_ - - -#include -#include -#include -#include - -#include -#include "had_map_utils/visibility_control.hpp" - -namespace autoware -{ -namespace common -{ -namespace had_map_utils -{ - -using autoware::common::types::float64_t; - -void HAD_MAP_UTILS_PUBLIC overwriteLaneletsCenterline( - lanelet::LaneletMapPtr lanelet_map, const autoware::common::types::bool8_t force_overwrite); -lanelet::LineString3d HAD_MAP_UTILS_PUBLIC generateFineCenterline( - const lanelet::ConstLanelet & lanelet_obj, const float64_t resolution); - -} // namespace had_map_utils -} // namespace common -} // namespace autoware - -#endif // HAD_MAP_UTILS__HAD_MAP_UTILS_HPP_ diff --git a/common/had_map_utils/include/had_map_utils/had_map_visualization.hpp b/common/had_map_utils/include/had_map_utils/had_map_visualization.hpp deleted file mode 100644 index 2952724fde4ff..0000000000000 --- a/common/had_map_utils/include/had_map_utils/had_map_visualization.hpp +++ /dev/null @@ -1,275 +0,0 @@ -// Copyright 2020 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 HAD_MAP_UTILS__HAD_MAP_VISUALIZATION_HPP_ -#define HAD_MAP_UTILS__HAD_MAP_VISUALIZATION_HPP_ - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include - -#include "had_map_utils/visibility_control.hpp" - -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; -using autoware::common::types::bool8_t; - -namespace autoware -{ -namespace common -{ -namespace had_map_utils -{ -/** - * \brief Set set rgba information to a Color Object - * \param[out] cl color object to be set - * \param r red value - * \param g green value - * \param b blue value - * \param a alpha value - */ -void HAD_MAP_UTILS_PUBLIC setColor( - std_msgs::msg::ColorRGBA * cl, - const float32_t & r, const float32_t & g, const float32_t & b, const float32_t & a); - -/** - * \brief Set the header information to a marker object - * \param m input marker - * \param id id of the marker - * \param t timestamp of the marker - * \param frame_id frame of the marker - * \param ns namespace of the marker - * \param c color of the marker - * \param action action used to visualize the marker - * \param type type of the marker - * \param scale scale of the marker - * \return visualization_msgs::msg::Marker - */ -void HAD_MAP_UTILS_PUBLIC setMarkerHeader( - visualization_msgs::msg::Marker * m, - const int32_t & id, const rclcpp::Time & t, - const std::string & frame_id, const std::string & ns, - const std_msgs::msg::ColorRGBA & c, const int32_t & action, const int32_t & type, - const float32_t & scale); - -/** - * \brief creates marker with type LINE_STRIP from a lanelet::LineString3d object - * \param t timestamp set to the marker - * \param ls input linestring - * \param frame_id frame id set to the marker - * \param ns namespace set to the marker - * \param c color of the marker - * \param lss linestrip scale (i.e. width) - * \return created visualization_msgs::msg::Marker - */ -visualization_msgs::msg::Marker HAD_MAP_UTILS_PUBLIC lineString2Marker( - const rclcpp::Time & t, - const lanelet::LineString3d & ls, - const std::string & frame_id, const std::string & ns, const std_msgs::msg::ColorRGBA & c, - const float32_t & lss); - -/** - * \brief creates marker with type LINE_STRIP from a lanelet::ConstLineString3d object - * \param t timestamp set to the marker - * \param ls input linestring - * \param frame_id frame id set to the marker - * \param ns namespace set to the marker - * \param c color of the marker - * \param lss linestrip scale (i.e. width) - * \return created visualization_msgs::msg::Marker - */ -visualization_msgs::msg::Marker HAD_MAP_UTILS_PUBLIC lineString2Marker( - const rclcpp::Time & t, - const lanelet::ConstLineString3d & ls, - const std::string & frame_id, const std::string & ns, const std_msgs::msg::ColorRGBA & c, - const float32_t & lss); - -/** - * \brief converts lanelet::LineString into markers with type LINE_STRIP - * \param t time set to returned marker message - * \param ns namespace set to the marker - * \param linestrings input linestring objects - * \param c color of the marker - * \return created visualization_msgs::msg::MarkerArray - */ -visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC -lineStringsAsMarkerArray( - const rclcpp::Time & t, - const std::string & ns, - const lanelet::LineStrings3d & linestrings, - const std_msgs::msg::ColorRGBA & c); - -/** - * \brief converts outer bound of lanelet::Lanelet into markers with type LINE_STRIP - * \param t time set to returned marker message - * \param lanelets input lanelet objects - * \param c color of the marker - * \param viz_centerline option to add centerline to the marker array - * \return created visualization_msgs::msg::MarkerArray - */ -visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC laneletsBoundaryAsMarkerArray( - const rclcpp::Time & t, - const lanelet::ConstLanelets & lanelets, - const std_msgs::msg::ColorRGBA & c, - const bool8_t & viz_centerline); - -/** - * \brief creates marker with type LINE_STRIP from a lanelet::BasicPolygon object - * \param t timestamp set to the marker - * \param line_id id set to the marker - * \param pg input polygon - * \param frame_id frame id set to the marker - * \param ns namespace set to the marker - * \param c color of the marker - * \param lss linestrip scale (i.e. width) - * \return created visualization_msgs::msg::Marker - */ -visualization_msgs::msg::Marker HAD_MAP_UTILS_PUBLIC basicPolygon2Marker( - const rclcpp::Time & t, - const int32_t & line_id, const lanelet::BasicPolygon3d & pg, - const std::string & frame_id, const std::string & ns, - const std_msgs::msg::ColorRGBA & c, const float32_t & lss); - -/** - * \brief converts outer bound of lanelet::Area into markers with type LINE_STRIP - * \param t time set to returned marker message - * \param ns namespace set to the marker - * \param areas input area objects - * \param c color of the marker - * \return created visualization_msgs::msg::MarkerArray - */ -visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC areasBoundaryAsMarkerArray( - const rclcpp::Time & t, - const std::string & ns, const lanelet::Areas & areas, - const std_msgs::msg::ColorRGBA & c); - -/** - * \brief converts outer bound of lanelet::Polygon into markers with type LINE_STRIP - * \param t Time set to returned marker message - * \param ns namespace set to the marker - * \param polygons input polygons - * \param c color of the marker - * \return created visualization_msgs::msg::MarkerArray - */ -visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC polygonsBoundaryAsMarkerArray( - const rclcpp::Time & t, - const std::string & ns, const lanelet::Polygons3d & polygons, - const std_msgs::msg::ColorRGBA & c); - -/** - * \brief creates marker with type LINE_STRIP from a bounding box - * \param t Time set to returned marker message - * \param line_id id set to marker - * \param lower lower bound of the bounding box with length 3(x,y,z) - * \param upper upper bound of the bounding box with length 3(x,y,z) - * \param frame_id frame id set to the marker - * \param ns namespace set to the marker - * \param c color of the marker - * \param lss linestrip scale (i.e. width) - * \return created visualization_msgs::msg::Marker - */ -visualization_msgs::msg::Marker HAD_MAP_UTILS_PUBLIC bbox2Marker( - const rclcpp::Time & t, const int32_t & line_id, - const float64_t lower[], const float64_t upper[], - const std::string & frame_id, const std::string & ns, - const std_msgs::msg::ColorRGBA & c, const float32_t & lss); - -/** - * \brief creates marker array from bounding box - * \param t Time set to returned marker message - * \param ns Namespace set to returned marker message - * \param upper upper bound of the bounding box with length 3(x,y,z) - * \param lower lower bound of the bounding box with length 3(x,y,z) - * \param c Color of the marker array - * \return created visualization_msgs::msg::MarkerArray - */ -visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC boundingBoxAsMarkerArray( - const rclcpp::Time & t, - const std::string & ns, const float64_t upper[], const float64_t lower[], - const std_msgs::msg::ColorRGBA & c); - -/** - * \brief converts area enclosed by lanelet::Lanelet into list of triangles. - * \param ll input lanelet - * \return result of triangulation - */ -std::vector HAD_MAP_UTILS_PUBLIC lanelet2Triangle( - const lanelet::ConstLanelet & ll); - -/** - * \brief converts area enclosed by geometry_msg::msg::Polygon into list of triangles. - * \param polygon input polygon - * \return result of triangulation - */ -std::vector HAD_MAP_UTILS_PUBLIC polygon2Triangle( - const geometry_msgs::msg::Polygon & polygon); - -/** - * \brief converts lanelet::Area into geometry_msgs::msg::Polygon type - * \param area input area - * \return converted geometry_msgs::msg::Polygon - */ -geometry_msgs::msg::Polygon HAD_MAP_UTILS_PUBLIC area2Polygon( - const lanelet::ConstArea & area); - -/** - * \brief converts lanelet::Lanelet into geometry_msgs::msg::Polygon type - * \param ll input lanelet - * \return converted geometry_msgs::msg::Polygon - */ -geometry_msgs::msg::Polygon HAD_MAP_UTILS_PUBLIC lanelet2Polygon( - const lanelet::ConstLanelet & ll); - -/** - * \brief converts bounded area by lanelet::Lanelet into triangle markers - * \param t Time set to returned marker message - * \param ns Namespace set to returned marker message - * \param lanelets input lanelet::Lanelet - * \param c Color of the marker array - * \return Converted triangle markers enclosed by the Lanelet - */ -visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC laneletsAsTriangleMarkerArray( - const rclcpp::Time & t, const std::string & ns, const lanelet::ConstLanelets & lanelets, - const std_msgs::msg::ColorRGBA & c); - -/** - * \brief converts bounded area by lanelet::Area into triangle markers - * \param t Time set to returned marker message - * \param ns Namespace set to returned marker message - * \param areas input lanelet::Area objects - * \param c Color of the marker array - * \return Converted triangle markers enclosed by the area - */ -visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC areasAsTriangleMarkerArray( - const rclcpp::Time & t, const std::string & ns, const lanelet::Areas & areas, - const std_msgs::msg::ColorRGBA & c); - -} // namespace had_map_utils -} // namespace common -} // namespace autoware - -#endif // HAD_MAP_UTILS__HAD_MAP_VISUALIZATION_HPP_ diff --git a/common/had_map_utils/include/had_map_utils/visibility_control.hpp b/common/had_map_utils/include/had_map_utils/visibility_control.hpp deleted file mode 100644 index f53a7dce48093..0000000000000 --- a/common/had_map_utils/include/had_map_utils/visibility_control.hpp +++ /dev/null @@ -1,38 +0,0 @@ -// Copyright 2017-2019 the Autoware Foundation -// -// 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -#ifndef HAD_MAP_UTILS__VISIBILITY_CONTROL_HPP_ -#define HAD_MAP_UTILS__VISIBILITY_CONTROL_HPP_ - -#if defined(_MSC_VER) && defined(_WIN64) - #if defined(HAD_MAP_UTILS_BUILDING_DLL) || defined(HAD_MAP_UTILS_EXPORTS) - #define HAD_MAP_UTILS_PUBLIC __declspec(dllexport) - #define HAD_MAP_UTILS_LOCAL - #else // defined(HAD_MAP_UTILS_BUILDING_DLL) || defined(HAD_MAP_UTILS_EXPORTS) - #define HAD_MAP_UTILS_PUBLIC __declspec(dllimport) - #define HAD_MAP_UTILS_LOCAL - #endif // defined(HAD_MAP_UTILS_BUILDING_DLL) || defined(HAD_MAP_UTILS_EXPORTS) -#elif defined(__GNUC__) && defined(__linux__) - #define HAD_MAP_UTILS_PUBLIC __attribute__((visibility("default"))) - #define HAD_MAP_UTILS_LOCAL __attribute__((visibility("hidden"))) -#elif defined(__GNUC__) && defined(__APPLE__) - #define HAD_MAP_UTILS_PUBLIC __attribute__((visibility("default"))) - #define HAD_MAP_UTILS_LOCAL __attribute__((visibility("hidden"))) -#else // !(defined(__GNUC__) && defined(__APPLE__)) - #error "Unsupported Build Configuration" -#endif // _MSC_VER - -#endif // HAD_MAP_UTILS__VISIBILITY_CONTROL_HPP_ diff --git a/common/had_map_utils/package.xml b/common/had_map_utils/package.xml deleted file mode 100644 index 74d03b4f1e186..0000000000000 --- a/common/had_map_utils/package.xml +++ /dev/null @@ -1,33 +0,0 @@ - - - - had_map_utils - 1.0.0 - Common utility functions and classes for HAD maps - TierIV, Inc. - Apache 2.0 - - ament_cmake_auto - autoware_auto_cmake - rclcpp - sensor_msgs - - cgal - lanelet2_core - lanelet2_io - lanelet2 - - autoware_auto_mapping_msgs - autoware_auto_planning_msgs - autoware_auto_common - visualization_msgs - geometry_msgs - - ament_cmake_gtest - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/common/had_map_utils/src/had_map_computation.cpp b/common/had_map_utils/src/had_map_computation.cpp deleted file mode 100644 index 0a8bd8186d3fd..0000000000000 --- a/common/had_map_utils/src/had_map_computation.cpp +++ /dev/null @@ -1,135 +0,0 @@ -// Copyright 2021 TierIV, 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. - -#include - -#include - -#include - -#include "had_map_utils/had_map_computation.hpp" -#include "had_map_utils/had_map_visualization.hpp" - -namespace autoware -{ -namespace common -{ -namespace had_map_utils -{ - -using Kernel = CGAL::Exact_predicates_exact_constructions_kernel; -using CGAL_Point = Kernel::Point_2; -using CGAL_Polygon = CGAL::Polygon_2; -using CGAL_Polygon_with_holes = CGAL::Polygon_with_holes_2; - -// TODO(s.me) this is getting a bit long, break up -lanelet::Polygon3d coalesce_drivable_areas( - const autoware_auto_planning_msgs::msg::HADMapRoute & had_map_route, - const lanelet::LaneletMapPtr & lanelet_map_ptr) -{ - CGAL_Polygon_with_holes drivable_area; - - for (const auto & map_segment : had_map_route.segments) { - // Attempt to obtain a polygon from the primitive ID - geometry_msgs::msg::Polygon current_area_polygon{}; - const auto & lanelet_layer = lanelet_map_ptr->laneletLayer; - const auto & current_lanelet_candidate = lanelet_layer.find(map_segment.preferred_primitive_id); - if (current_lanelet_candidate != lanelet_layer.end()) { - current_area_polygon = lanelet2Polygon(*current_lanelet_candidate); - } else { - const auto & area_layer = lanelet_map_ptr->areaLayer; - const auto & current_area_candidate = area_layer.find(map_segment.preferred_primitive_id); - if (current_area_candidate != area_layer.end()) { - current_area_polygon = area2Polygon(*current_area_candidate); - } else { - // This might happen if a primitive is on the route, but outside of the bounding box that we - // query the map for. Not sure how to deal with this at this point though. - std::cerr << "Error: primitive ID " << map_segment.preferred_primitive_id << - " not found, skipping" << - std::endl; - continue; - } - } - - if (drivable_area.outer_boundary().size() > 0) { - // Convert current_area_polygon to a CGAL_Polygon and make sure the orientation is correct - CGAL_Polygon to_join{}; - CGAL_Polygon_with_holes temporary_union; - const auto first_point = current_area_polygon.points.begin(); - for (auto area_point_it = - current_area_polygon.points.begin(); - // Stop if we run out of points, or if we encounter the first point again - area_point_it < current_area_polygon.points.end() && - !(first_point != area_point_it && first_point->x == area_point_it->x && - first_point->y == area_point_it->y); - area_point_it++) - { - to_join.push_back(CGAL_Point(area_point_it->x, area_point_it->y)); - } - - if (to_join.is_clockwise_oriented() ) { - to_join.reverse_orientation(); - } - - // Merge this CGAL polygon with the growing drivable_area. We need an intermediate merge - // result because as far as I can tell from the CGAL docs, I can't "join to" a polygon - // in-place with the join() interface. - const auto polygons_overlap = CGAL::join(drivable_area, to_join, temporary_union); - if (!polygons_overlap && !drivable_area.outer_boundary().is_empty()) { - // TODO(s.me) cancel here? Right now we just ignore that polygon, if it doesn't - // overlap with the rest, there is no way to get to it anyway - std::cerr << "Error: polygons in union do not overlap!" << std::endl; - } else { - drivable_area = temporary_union; - } - } else { - // Otherwise, just set the current drivable area equal to the area to add to it, because - // CGAL seems to do "union(empty, non-empty) = empty" for some reason. - const auto first_point = current_area_polygon.points.begin(); - for (auto area_point_it = - current_area_polygon.points.begin(); - area_point_it < current_area_polygon.points.end() && - // Stop if we run out of points, or if we encounter the first point again - !(first_point != area_point_it && first_point->x == area_point_it->x && - first_point->y == area_point_it->y); - area_point_it++) - { - drivable_area.outer_boundary().push_back(CGAL_Point(area_point_it->x, area_point_it->y)); - } - if (drivable_area.outer_boundary().is_clockwise_oriented() ) { - drivable_area.outer_boundary().reverse_orientation(); - } - } - } - - // At this point, all the polygons from the route should be merged into drivable_area, - // and we now need to turn this back into a lanelet polygon. - std::vector lanelet_drivable_area_points{}; - lanelet_drivable_area_points.reserve(drivable_area.outer_boundary().size()); - for (auto p = drivable_area.outer_boundary().vertices_begin(); - p != drivable_area.outer_boundary().vertices_end(); p++) - { - lanelet_drivable_area_points.emplace_back( - lanelet::Point3d( - lanelet::utils::getId(), CGAL::to_double(p->x()), - CGAL::to_double(p->y()), 0.0)); - } - lanelet::Polygon3d lanelet_drivable_area(lanelet::utils::getId(), lanelet_drivable_area_points); - return lanelet_drivable_area; -} - - -} // namespace had_map_utils -} // namespace common -} // namespace autoware diff --git a/common/had_map_utils/src/had_map_conversion.cpp b/common/had_map_utils/src/had_map_conversion.cpp deleted file mode 100644 index c7b2bc711270a..0000000000000 --- a/common/had_map_utils/src/had_map_conversion.cpp +++ /dev/null @@ -1,73 +0,0 @@ -// Copyright 2020 TierIV, 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -//lint -e537 pclint vs cpplint NOLINT - -#include - -#include -#include - -#include -#include -#include -#include -#include - -#include "had_map_utils/had_map_conversion.hpp" - - -namespace autoware -{ -namespace common -{ -namespace had_map_utils -{ - -void toBinaryMsg( - const std::shared_ptr & map, - autoware_auto_mapping_msgs::msg::HADMapBin & msg) -{ - std::stringstream ss; - boost::archive::binary_oarchive oa(ss); - oa << *map; - auto id_counter = lanelet::utils::getId(); - oa << id_counter; - - std::string tmp_str = ss.str(); - msg.data.clear(); - msg.data.resize(tmp_str.size()); - msg.data.assign(tmp_str.begin(), tmp_str.end()); -} - -void fromBinaryMsg( - const autoware_auto_mapping_msgs::msg::HADMapBin & msg, - std::shared_ptr & map) -{ - std::string data_str; - data_str.assign(msg.data.begin(), msg.data.end()); - std::stringstream ss; - ss << data_str; - boost::archive::binary_iarchive oa(ss); - oa >> *map; - lanelet::Id id_counter; - oa >> id_counter; - lanelet::utils::registerId(id_counter); -} - -} // namespace had_map_utils -} // namespace common -} // namespace autoware diff --git a/common/had_map_utils/src/had_map_query.cpp b/common/had_map_utils/src/had_map_query.cpp deleted file mode 100644 index eeca6543b06bc..0000000000000 --- a/common/had_map_utils/src/had_map_query.cpp +++ /dev/null @@ -1,132 +0,0 @@ -// Copyright 2020 TierIV, 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -//lint -e537 pclint vs cpplint NOLINT - -#include -#include -#include -#include -#include - -#include "had_map_utils/had_map_query.hpp" - -namespace autoware -{ -namespace common -{ -namespace had_map_utils -{ - -lanelet::Areas getAreaLayer(const lanelet::LaneletMapPtr ll_map) -{ - lanelet::Areas areas; - for (auto ai = ll_map->areaLayer.begin(); ai != ll_map->areaLayer.end(); ai++) { - areas.push_back(*ai); - } - return areas; -} - -lanelet::Areas subtypeAreas(const lanelet::Areas areas, const char subtype[]) -{ - lanelet::Areas subtype_areas; - for (auto ai = areas.begin(); ai != areas.end(); ai++) { - lanelet::Area a = *ai; - if (a.hasAttribute(lanelet::AttributeName::Subtype)) { - lanelet::Attribute attr = a.attribute(lanelet::AttributeName::Subtype); - if (attr.value() == subtype) { - subtype_areas.push_back(a); - } - } - } - return subtype_areas; -} - -lanelet::Polygons3d getPolygonLayer(const lanelet::LaneletMapPtr ll_map) -{ - lanelet::Polygons3d polygons; - for (auto ai = ll_map->polygonLayer.begin(); ai != ll_map->polygonLayer.end(); ai++) { - polygons.push_back(*ai); - } - return polygons; -} - -lanelet::Polygons3d subtypePolygons(const lanelet::Polygons3d polygons, const char subtype[]) -{ - lanelet::Polygons3d subtype_polygons; - for (auto pi = polygons.begin(); pi != polygons.end(); pi++) { - lanelet::Polygon3d p = *pi; - if (p.hasAttribute(lanelet::AttributeName::Subtype)) { - lanelet::Attribute attr = p.attribute(lanelet::AttributeName::Subtype); - if (attr.value() == subtype) { - subtype_polygons.push_back(p); - } - } - } - return subtype_polygons; -} - - -lanelet::LineStrings3d getLineStringLayer(const lanelet::LaneletMapPtr ll_map) -{ - lanelet::LineStrings3d linestrings; - for (auto lsi = ll_map->lineStringLayer.begin(); - lsi != ll_map->lineStringLayer.end(); lsi++) - { - linestrings.push_back(*lsi); - } - return linestrings; -} - -lanelet::LineStrings3d subtypeLineStrings( - const lanelet::LineStrings3d linestrings, - const char subtype[]) -{ - lanelet::LineStrings3d subtype_linestrings; - for (auto lsi = linestrings.begin(); lsi != linestrings.end(); lsi++) { - lanelet::LineString3d ls = *lsi; - if (ls.hasAttribute(lanelet::AttributeName::Subtype)) { - lanelet::Attribute attr = ls.attribute(lanelet::AttributeName::Subtype); - if (attr.value() == subtype) { - subtype_linestrings.push_back(ls); - } - } - } - return subtype_linestrings; -} - -lanelet::ConstLanelets getConstLaneletLayer(const std::shared_ptr & ll_map) -{ - lanelet::ConstLanelets lanelets; - for (auto li = ll_map->laneletLayer.begin(); li != ll_map->laneletLayer.end(); li++) { - lanelets.push_back(*li); - } - - return lanelets; -} -lanelet::Lanelets getLaneletLayer(const std::shared_ptr & ll_map) -{ - lanelet::Lanelets lanelets; - for (auto li = ll_map->laneletLayer.begin(); li != ll_map->laneletLayer.end(); li++) { - lanelets.push_back(*li); - } - - return lanelets; -} - -} // namespace had_map_utils -} // namespace common -} // namespace autoware diff --git a/common/had_map_utils/src/had_map_utils.cpp b/common/had_map_utils/src/had_map_utils.cpp deleted file mode 100644 index ff56754268864..0000000000000 --- a/common/had_map_utils/src/had_map_utils.cpp +++ /dev/null @@ -1,175 +0,0 @@ -// Copyright 2020 TierIV, 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -//lint -e537 pclint vs cpplint NOLINT - - -#include "had_map_utils/had_map_utils.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include - -using autoware::common::types::float64_t; - -namespace autoware -{ -namespace common -{ -namespace had_map_utils -{ - -std::vector calculateSegmentDistances(const lanelet::ConstLineString3d & line_string) -{ - std::vector segment_distances; - segment_distances.reserve(line_string.size() - 1); - - for (size_t i = 1; i < line_string.size(); ++i) { - const auto distance = lanelet::geometry::distance(line_string[i], line_string[i - 1]); - segment_distances.push_back(distance); - } - - return segment_distances; -} - -std::vector calculateAccumulatedLengths(const lanelet::ConstLineString3d & line_string) -{ - const auto segment_distances = calculateSegmentDistances(line_string); - - std::vector accumulated_lengths{0}; - accumulated_lengths.reserve(segment_distances.size() + 1); - std::partial_sum( - std::begin(segment_distances), std::end(segment_distances), - std::back_inserter(accumulated_lengths)); - - return accumulated_lengths; -} - -std::pair findNearestIndexPair( - const std::vector & accumulated_lengths, const float64_t target_length) -{ - // List size - const auto N = accumulated_lengths.size(); - - // Front - if (target_length < accumulated_lengths.at(1)) { - return std::make_pair(0, 1); - } - - // Back - if (target_length > accumulated_lengths.at(N - 2)) { - return std::make_pair(N - 2, N - 1); - } - - // Middle - for (size_t i = 1; i < N; ++i) { - if ( - accumulated_lengths.at(i - 1) <= target_length && - target_length <= accumulated_lengths.at(i)) - { - return std::make_pair(i - 1, i); - } - } - - // Throw an exception because this never happens - throw std::runtime_error( - "findNearestIndexPair(): No nearest point found."); -} - -std::vector resamplePoints( - const lanelet::ConstLineString3d & line_string, const int32_t num_segments) -{ - // Calculate length - const auto line_length = lanelet::geometry::length(line_string); - - // Calculate accumulated lengths - const auto accumulated_lengths = calculateAccumulatedLengths(line_string); - - // Create each segment - std::vector resampled_points; - for (auto i = 0; i <= num_segments; ++i) { - // Find two nearest points - const float64_t target_length = (static_cast(i) / num_segments) * - static_cast(line_length); - const auto index_pair = findNearestIndexPair(accumulated_lengths, target_length); - - // Apply linear interpolation - const lanelet::BasicPoint3d back_point = line_string[index_pair.first]; - const lanelet::BasicPoint3d front_point = line_string[index_pair.second]; - const auto direction_vector = (front_point - back_point); - - const auto back_length = accumulated_lengths.at(index_pair.first); - const auto front_length = accumulated_lengths.at(index_pair.second); - const auto segment_length = front_length - back_length; - const auto target_point = - back_point + (direction_vector * (target_length - back_length) / segment_length); - - // Add to list - resampled_points.push_back(target_point); - } - - return resampled_points; -} - -lanelet::LineString3d generateFineCenterline( - const lanelet::ConstLanelet & lanelet_obj, const float64_t resolution) -{ - // Get length of longer border - const float64_t left_length = - static_cast(lanelet::geometry::length(lanelet_obj.leftBound())); - const float64_t right_length = - static_cast(lanelet::geometry::length(lanelet_obj.rightBound())); - const float64_t longer_distance = (left_length > right_length) ? left_length : right_length; - const int32_t num_segments = - std::max(static_cast(ceil(longer_distance / resolution)), 1); - - // Resample points - const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments); - const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments); - - // Create centerline - lanelet::LineString3d centerline(lanelet::utils::getId()); - for (size_t i = 0; i < static_cast(num_segments + 1); i++) { - // Add ID for the average point of left and right - const auto center_basic_point = (right_points.at(i) + left_points.at(i)) / 2.0; - const lanelet::Point3d center_point( - lanelet::utils::getId(), center_basic_point.x(), center_basic_point.y(), - center_basic_point.z()); - centerline.push_back(center_point); - } - return centerline; -} - -void overwriteLaneletsCenterline( - lanelet::LaneletMapPtr lanelet_map, - const autoware::common::types::bool8_t force_overwrite) -{ - for (auto & lanelet_obj : lanelet_map->laneletLayer) { - if (force_overwrite || !lanelet_obj.hasCustomCenterline()) { - const auto fine_center_line = generateFineCenterline(lanelet_obj, 2.0); - lanelet_obj.setCenterline(fine_center_line); - } - } -} - -} // namespace had_map_utils -} // namespace common -} // namespace autoware diff --git a/common/had_map_utils/src/had_map_visualization.cpp b/common/had_map_utils/src/had_map_visualization.cpp deleted file mode 100644 index 6133bc1eb22be..0000000000000 --- a/common/had_map_utils/src/had_map_visualization.cpp +++ /dev/null @@ -1,558 +0,0 @@ -// Copyright 2020 TierIV, 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. -// -// Co-developed by Tier IV, Inc. and Apex.AI, Inc. - -//lint -e537 pclint vs cpplint NOLINT - -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "had_map_utils/had_map_visualization.hpp" - -using autoware::common::types::float64_t; - -namespace autoware -{ -namespace common -{ -namespace had_map_utils -{ - -template -bool8_t exists(const std::unordered_set & set, const T & element) -{ - return std::find(set.begin(), set.end(), element) != set.end(); -} - -void setColor( - std_msgs::msg::ColorRGBA * cl, - const float32_t & r, const float32_t & g, const float32_t & b, const float32_t & a) -{ - cl->r = r; - cl->g = g; - cl->b = b; - cl->a = a; -} - -void setMarkerHeader( - visualization_msgs::msg::Marker * m, - const int32_t & id, - const rclcpp::Time & t, - const std::string & frame_id, - const std::string & ns, - const std_msgs::msg::ColorRGBA & c, - const int32_t & action, - const int32_t & type, - const float32_t & scale) -{ - m->header.frame_id = frame_id; - m->header.stamp = t; - m->ns = ns; - m->action = action; - m->type = type; - - m->id = id; - m->pose.position.x = 0.0; - m->pose.position.y = 0.0; - m->pose.position.z = 0.0; - m->pose.orientation.x = 0.0; - m->pose.orientation.y = 0.0; - m->pose.orientation.z = 0.0; - m->pose.orientation.w = 1.0; - m->scale.x = scale; - m->scale.y = scale; - m->scale.z = scale; - m->color = c; -} - -visualization_msgs::msg::Marker lineString2Marker( - const rclcpp::Time & t, - const lanelet::LineString3d & ls, - const std::string & frame_id, - const std::string & ns, const std_msgs::msg::ColorRGBA & c, const float32_t & lss) -{ - visualization_msgs::msg::Marker line_strip; - setMarkerHeader( - &line_strip, static_cast(ls.id()), t, frame_id, ns, c, - visualization_msgs::msg::Marker::ADD, - visualization_msgs::msg::Marker::LINE_STRIP, - lss); - - for (auto i = ls.begin(); i != ls.end(); i++) { - geometry_msgs::msg::Point p; - p.x = (*i).x(); - p.y = (*i).y(); - p.z = (*i).z(); - line_strip.points.push_back(p); - } - return line_strip; -} - -visualization_msgs::msg::Marker lineString2Marker( - const rclcpp::Time & t, - const lanelet::ConstLineString3d & ls, - const std::string & frame_id, const std::string & ns, const std_msgs::msg::ColorRGBA & c, - const float32_t & lss) -{ - visualization_msgs::msg::Marker line_strip; - setMarkerHeader( - &line_strip, static_cast(ls.id()), t, frame_id, ns, c, - visualization_msgs::msg::Marker::ADD, - visualization_msgs::msg::Marker::LINE_STRIP, - lss); - - for (auto i = ls.begin(); i != ls.end(); i++) { - geometry_msgs::msg::Point p; - p.x = (*i).x(); - p.y = (*i).y(); - p.z = (*i).z(); - line_strip.points.push_back(p); - } - return line_strip; -} - -visualization_msgs::msg::MarkerArray lineStringsAsMarkerArray( - const rclcpp::Time & t, - const std::string & ns, - const lanelet::LineStrings3d & linestrings, - const std_msgs::msg::ColorRGBA & c) -{ - float32_t lss = 0.1f; - visualization_msgs::msg::MarkerArray marker_array; - - for (auto lsi = linestrings.begin(); lsi != linestrings.end(); lsi++) { - lanelet::LineString3d ls = *lsi; - visualization_msgs::msg::Marker line_strip = lineString2Marker(t, ls, "map", ns, c, lss); - marker_array.markers.push_back(line_strip); - } - return marker_array; -} - -visualization_msgs::msg::MarkerArray laneletsBoundaryAsMarkerArray( - const rclcpp::Time & t, - const lanelet::ConstLanelets & lanelets, - const std_msgs::msg::ColorRGBA & c, - const bool8_t & viz_centerline) -{ - float32_t lss = 0.1f; - std::unordered_set added; - visualization_msgs::msg::MarkerArray marker_array; - - for (auto li = lanelets.begin(); li != lanelets.end(); li++) { - lanelet::ConstLanelet lll = *li; - lanelet::ConstLineString3d left_ls = lll.leftBound(); - lanelet::ConstLineString3d right_ls = lll.rightBound(); - lanelet::ConstLineString3d center_ls = lll.centerline(); - - visualization_msgs::msg::Marker left_line_strip, right_line_strip, center_line_strip; - if (!exists(added, left_ls.id())) { - left_line_strip = lineString2Marker(t, left_ls, "map", "left_lane_bound", c, lss); - marker_array.markers.push_back(left_line_strip); - added.insert(left_ls.id()); - } - if (!exists(added, right_ls.id())) { - right_line_strip = lineString2Marker( - t, right_ls, "map", "right_lane_bound", c, lss); - marker_array.markers.push_back(right_line_strip); - added.insert(right_ls.id()); - } - if (viz_centerline && !exists(added, center_ls.id())) { - center_line_strip = lineString2Marker( - t, center_ls, "map", "center_lane_line", - c, std::max(lss * 0.1f, 0.01f)); - marker_array.markers.push_back(center_line_strip); - added.insert(center_ls.id()); - } - } - return marker_array; -} - -visualization_msgs::msg::Marker basicPolygon2Marker( - const rclcpp::Time & t, - const int32_t & line_id, - const lanelet::BasicPolygon3d & pg, - const std::string & frame_id, - const std::string & ns, - const std_msgs::msg::ColorRGBA & c, - const float32_t & lss) -{ - visualization_msgs::msg::Marker line_strip; - setMarkerHeader( - &line_strip, line_id, t, frame_id, ns, c, - visualization_msgs::msg::Marker::ADD, - visualization_msgs::msg::Marker::LINE_STRIP, - lss); - - for (auto i = pg.begin(); i != pg.end(); i++) { - geometry_msgs::msg::Point p; - p.x = (*i).x(); - p.y = (*i).y(); - p.z = (*i).z(); - line_strip.points.push_back(p); - } - geometry_msgs::msg::Point pb; - auto i = pg.begin(); - pb.x = (*i).x(); - pb.y = (*i).y(); - pb.z = (*i).z(); - line_strip.points.push_back(pb); - return line_strip; -} - -visualization_msgs::msg::MarkerArray areasBoundaryAsMarkerArray( - const rclcpp::Time & t, - const std::string & ns, - const lanelet::Areas & areas, - const std_msgs::msg::ColorRGBA & c) -{ - int pg_count = 0; - float32_t lss = 0.1f; - visualization_msgs::msg::MarkerArray marker_array; - for (auto area : areas) { - lanelet::CompoundPolygon3d cpg = area.outerBoundPolygon(); - lanelet::BasicPolygon3d bpg = cpg.basicPolygon(); - - visualization_msgs::msg::Marker line_strip = basicPolygon2Marker( - t, pg_count, bpg, "map", ns, c, - lss); - marker_array.markers.push_back(line_strip); - pg_count++; - } - return marker_array; -} - -visualization_msgs::msg::MarkerArray polygonsBoundaryAsMarkerArray( - const rclcpp::Time & t, const std::string & ns, - const lanelet::Polygons3d & polygons, const std_msgs::msg::ColorRGBA & c) -{ - int32_t pg_count = 0; - float32_t lss = 0.1f; - visualization_msgs::msg::MarkerArray marker_array; - for (auto poly : polygons) { - lanelet::BasicPolygon3d bpg = poly.basicPolygon(); - visualization_msgs::msg::Marker line_strip = basicPolygon2Marker( - t, pg_count, bpg, "map", ns, c, - lss); - marker_array.markers.push_back(line_strip); - pg_count++; - } - return marker_array; -} - -visualization_msgs::msg::Marker bbox2Marker( - const rclcpp::Time & t, const int32_t & line_id, const float64_t lower[], const float64_t upper[], - const std::string & frame_id, const std::string & ns, - const std_msgs::msg::ColorRGBA & c, const float32_t & lss) -{ - visualization_msgs::msg::Marker line_strip; - setMarkerHeader( - &line_strip, line_id, t, frame_id, ns, c, - visualization_msgs::msg::Marker::ADD, - visualization_msgs::msg::Marker::LINE_STRIP, - lss); - - geometry_msgs::msg::Point bl, br, tl, tr; - bl.x = lower[0]; bl.y = lower[0]; bl.z = 0.0; - br.x = upper[0]; br.y = lower[0]; br.z = 0.0; - tl.x = lower[0]; tl.y = upper[0]; tl.z = 0.0; - tr.x = upper[0]; tr.y = upper[0]; tr.z = 0.0; - - line_strip.points.push_back(bl); - line_strip.points.push_back(br); - line_strip.points.push_back(tr); - line_strip.points.push_back(tl); - line_strip.points.push_back(bl); - return line_strip; -} - -visualization_msgs::msg::MarkerArray boundingBoxAsMarkerArray( - const rclcpp::Time & t, - const std::string & ns, const float64_t upper[], const float64_t lower[], - const std_msgs::msg::ColorRGBA & c) -{ - float32_t lss = 0.2f; - visualization_msgs::msg::MarkerArray marker_array; - visualization_msgs::msg::Marker line_strip = bbox2Marker(t, 0, upper, lower, "map", ns, c, lss); - marker_array.markers.push_back(line_strip); - return marker_array; -} - -geometry_msgs::msg::Point toGeomMsgPt(const geometry_msgs::msg::Point32 & src) -{ - geometry_msgs::msg::Point dst; - dst.x = static_cast(src.x); - dst.y = static_cast(src.y); - dst.z = static_cast(src.z); - return dst; -} - -geometry_msgs::msg::Point32 toGeomMsgPt32(const lanelet::BasicPoint3d & src) -{ - geometry_msgs::msg::Point32 dst; - dst.x = static_cast(src.x()); - dst.y = static_cast(src.y()); - dst.z = static_cast(src.z()); - return dst; -} -void adjacentPoints( - const size_t i, const size_t N, - const geometry_msgs::msg::Polygon poly, - geometry_msgs::msg::Point32 * p0, - geometry_msgs::msg::Point32 * p1, - geometry_msgs::msg::Point32 * p2) -{ - *p1 = poly.points[i]; - if (i == 0) { - *p0 = poly.points[N - 1]; - } else { - *p0 = poly.points[i - 1]; - } - if (i < N - 1) { - *p2 = poly.points[i + 1]; - } else { - *p2 = poly.points[0]; - } -} - -std::vector lanelet2Triangle( - const lanelet::ConstLanelet & ll) -{ - geometry_msgs::msg::Polygon ls_poly = lanelet2Polygon(ll); - return polygon2Triangle(ls_poly); -} - -std::vector area2Triangle( - const lanelet::Area & area) -{ - geometry_msgs::msg::Polygon ls_poly = area2Polygon(area); - return polygon2Triangle(ls_poly); -} - -// Is angle AOB less than 180? -// https://qiita.com/fujii-kotaro/items/a411f2a45627ed2f156e -bool8_t isAcuteAngle( - const geometry_msgs::msg::Point32 & vertex_a, - const geometry_msgs::msg::Point32 & vertex_o, - const geometry_msgs::msg::Point32 & vertex_b) -{ - return (vertex_a.x - vertex_o.x) * (vertex_b.y - vertex_o.y) - - (vertex_a.y - vertex_o.y) * (vertex_b.x - vertex_o.x) >= 0; -} - -// https://qiita.com/fujii-kotaro/items/a411f2a45627ed2f156e -bool8_t isWithinTriangle( - const geometry_msgs::msg::Point32 & vertex_a, - const geometry_msgs::msg::Point32 & vertex_b, - const geometry_msgs::msg::Point32 & vertex_c, - const geometry_msgs::msg::Point32 & pt) -{ - float64_t c1 = (vertex_b.x - vertex_a.x) * (pt.y - vertex_b.y) - - (vertex_b.y - vertex_a.y) * (pt.x - vertex_b.x); - float64_t c2 = (vertex_c.x - vertex_b.x) * (pt.y - vertex_c.y) - - (vertex_c.y - vertex_b.y) * (pt.x - vertex_c.x); - float64_t c3 = (vertex_a.x - vertex_c.x) * (pt.y - vertex_a.y) - - (vertex_a.y - vertex_c.y) * (pt.x - vertex_a.x); - - return (c1 > 0.0 && c2 > 0.0 && c3 > 0.0) || (c1 < 0.0 && c2 < 0.0 && c3 < 0.0); -} - -std::vector polygon2Triangle( - const geometry_msgs::msg::Polygon & polygon) -{ - std::vector triangles; - geometry_msgs::msg::Polygon poly = polygon; - size_t num_vertices = poly.points.size(); - - std::vector is_acute_angle; - is_acute_angle.assign(num_vertices, false); - for (size_t i = 0; i < num_vertices; i++) { - geometry_msgs::msg::Point32 p0, p1, p2; - - adjacentPoints(i, num_vertices, poly, &p0, &p1, &p2); - is_acute_angle.at(i) = isAcuteAngle(p0, p1, p2); - } - while (num_vertices >= 3) { - size_t clipped_vertex = 0; - - for (size_t i = 0; i < num_vertices; i++) { - bool8_t theta = is_acute_angle.at(i); - if (theta == true) { - geometry_msgs::msg::Point32 p0, p1, p2; - adjacentPoints(i, num_vertices, poly, &p0, &p1, &p2); - - size_t j_begin = (i + 2) % num_vertices; - size_t j_end = (i - 1 + num_vertices) % num_vertices; - bool8_t is_ear = true; - for (size_t j = j_begin; j != j_end; j = (j + 1) % num_vertices) { - if (isWithinTriangle(p0, p1, p2, poly.points.at(j))) { - is_ear = false; - break; - } - } - - if (is_ear) { - clipped_vertex = i; - break; - } - } - } - geometry_msgs::msg::Point32 p0, p1, p2; - adjacentPoints(clipped_vertex, num_vertices, poly, &p0, &p1, &p2); - geometry_msgs::msg::Polygon triangle; - triangle.points.push_back(p0); - triangle.points.push_back(p1); - triangle.points.push_back(p2); - triangles.push_back(triangle); - auto it = poly.points.begin(); - std::advance(it, clipped_vertex); - poly.points.erase(it); - - auto it_angle = is_acute_angle.begin(); - std::advance(it_angle, clipped_vertex); - is_acute_angle.erase(it_angle); - - num_vertices = poly.points.size(); - if (clipped_vertex == num_vertices) { - clipped_vertex = 0; - } - adjacentPoints(clipped_vertex, num_vertices, poly, &p0, &p1, &p2); - is_acute_angle.at(clipped_vertex) = isAcuteAngle(p0, p1, p2); - - size_t i_prev = (clipped_vertex == 0) ? num_vertices - 1 : clipped_vertex - 1; - adjacentPoints(i_prev, num_vertices, poly, &p0, &p1, &p2); - is_acute_angle.at(i_prev) = isAcuteAngle(p0, p1, p2); - } - return triangles; -} - - -geometry_msgs::msg::Polygon area2Polygon( - const lanelet::ConstArea & area) -{ - geometry_msgs::msg::Polygon polygon; - polygon.points.clear(); - polygon.points.reserve(area.outerBoundPolygon().size()); - - std::transform( - area.outerBoundPolygon().begin(), - area.outerBoundPolygon().end(), - std::back_inserter(polygon.points), - [](lanelet::ConstPoint3d pt) {return toGeomMsgPt32(pt.basicPoint());}); - return polygon; -} - -geometry_msgs::msg::Polygon lanelet2Polygon( - const lanelet::ConstLanelet & ll) -{ - geometry_msgs::msg::Polygon polygon; - - const lanelet::CompoundPolygon3d & ll_poly = ll.polygon3d(); - polygon.points.clear(); - polygon.points.reserve(ll_poly.size()); - - std::transform( - ll_poly.begin(), - ll_poly.end(), - std::back_inserter(polygon.points), - [](lanelet::ConstPoint3d pt) {return toGeomMsgPt32(pt.basicPoint());}); - return polygon; -} - -visualization_msgs::msg::MarkerArray laneletsAsTriangleMarkerArray( - const rclcpp::Time & t, - const std::string & ns, - const lanelet::ConstLanelets & lanelets, - const std_msgs::msg::ColorRGBA & c) -{ - visualization_msgs::msg::MarkerArray marker_array; - visualization_msgs::msg::Marker marker; - - if (lanelets.empty()) { - return marker_array; - } - - setMarkerHeader( - &marker, 0, t, "map", ns, c, - visualization_msgs::msg::Marker::ADD, - visualization_msgs::msg::Marker::TRIANGLE_LIST, - 1.0); - - for (auto ll : lanelets) { - std::vector triangles = lanelet2Triangle(ll); - - for (auto tri : triangles) { - geometry_msgs::msg::Point tri0[3]; - - for (size_t i = 0; i < 3; i++) { - tri0[i] = toGeomMsgPt(tri.points[i]); - - marker.points.push_back(tri0[i]); - marker.colors.push_back(c); - } - } - } - if (!marker.points.empty()) { - marker_array.markers.push_back(marker); - } - - return marker_array; -} - -visualization_msgs::msg::MarkerArray areasAsTriangleMarkerArray( - const rclcpp::Time & t, const std::string & ns, const lanelet::Areas & areas, - const std_msgs::msg::ColorRGBA & c) -{ - visualization_msgs::msg::MarkerArray marker_array; - visualization_msgs::msg::Marker marker; - - if (areas.empty()) { - return marker_array; - } - - setMarkerHeader( - &marker, 0, t, "map", ns, c, - visualization_msgs::msg::Marker::ADD, - visualization_msgs::msg::Marker::TRIANGLE_LIST, - 1.0); - - for (auto area : areas) { - std::vector triangles = area2Triangle(area); - for (auto tri : triangles) { - for (size_t i = 0; i < 3; i++) { - marker.points.push_back(toGeomMsgPt(tri.points[i])); - marker.colors.push_back(c); - } - } - } - - if (!marker.points.empty()) { - marker_array.markers.push_back(marker); - } - return marker_array; -} - -} // namespace had_map_utils -} // namespace common -} // namespace autoware From 5573cc905f7bf3b1581e7ad5910dbd08b98823af Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 12 May 2023 17:20:29 +0900 Subject: [PATCH 42/92] fix(osqp_interface): fix build error in Humble (#435) --- common/osqp_interface/CMakeLists.txt | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/common/osqp_interface/CMakeLists.txt b/common/osqp_interface/CMakeLists.txt index fa33d4b7aacd4..c06aa6509e4d5 100644 --- a/common/osqp_interface/CMakeLists.txt +++ b/common/osqp_interface/CMakeLists.txt @@ -19,14 +19,17 @@ project(osqp_interface) # require that dependencies from package.xml be available find_package(ament_cmake_auto REQUIRED) find_package(Eigen3 REQUIRED) -find_package(osqp REQUIRED) -get_target_property(OSQP_INCLUDE_DIR osqp::osqp INTERFACE_INCLUDE_DIRECTORIES) ament_auto_find_build_dependencies(REQUIRED ${${PROJECT_NAME}_BUILD_DEPENDS} ${${PROJECT_NAME}_BUILDTOOL_DEPENDS} ) +# after find_package(osqp_vendor) in ament_auto_find_build_dependencies +find_package(osqp REQUIRED) +get_target_property(OSQP_INCLUDE_SUB_DIR osqp::osqp INTERFACE_INCLUDE_DIRECTORIES) +get_filename_component(OSQP_INCLUDE_DIR ${OSQP_INCLUDE_SUB_DIR} PATH) + set(OSQP_INTERFACE_LIB_SRC src/csc_matrix_conv.cpp src/osqp_interface.cpp @@ -46,7 +49,10 @@ ament_auto_add_library(${PROJECT_NAME} SHARED autoware_set_compile_options(${PROJECT_NAME}) target_compile_options(${PROJECT_NAME} PRIVATE -Wno-error=old-style-cast -Wno-error=useless-cast) -target_include_directories(osqp_interface PUBLIC "${OSQP_INCLUDE_DIR}") +target_include_directories(osqp_interface + SYSTEM PUBLIC + "${OSQP_INCLUDE_DIR}" +) ament_target_dependencies(osqp_interface Eigen3 osqp_vendor From 25964211ef08947023a16cea569d860b1f2be967 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 12 May 2023 17:25:37 +0900 Subject: [PATCH 43/92] fix(motion_common, motion_testing): fix buid error in Humble (#440) --- common/motion_common/include/motion_common/motion_common.hpp | 2 +- common/motion_common/src/motion_common/motion_common.cpp | 2 +- common/motion_testing/src/motion_testing/motion_testing.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/common/motion_common/include/motion_common/motion_common.hpp b/common/motion_common/include/motion_common/motion_common.hpp index 6856636439240..7fd9acc8429a2 100644 --- a/common/motion_common/include/motion_common/motion_common.hpp +++ b/common/motion_common/include/motion_common/motion_common.hpp @@ -24,7 +24,7 @@ #include #include -#include +#include #include #include diff --git a/common/motion_common/src/motion_common/motion_common.cpp b/common/motion_common/src/motion_common/motion_common.cpp index d89ba33d9021e..0fcf4e2e6b1cf 100644 --- a/common/motion_common/src/motion_common/motion_common.cpp +++ b/common/motion_common/src/motion_common/motion_common.cpp @@ -20,7 +20,7 @@ #include "helper_functions/angle_utils.hpp" #include "tf2/utils.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace motion { diff --git a/common/motion_testing/src/motion_testing/motion_testing.cpp b/common/motion_testing/src/motion_testing/motion_testing.cpp index e3f6704c2284c..18f9efc94e730 100644 --- a/common/motion_testing/src/motion_testing/motion_testing.cpp +++ b/common/motion_testing/src/motion_testing/motion_testing.cpp @@ -16,7 +16,7 @@ #include #include #include -#include +#include #include #include From b870dd125e78682fb50f1c369a01d8080f55d7c0 Mon Sep 17 00:00:00 2001 From: v-nojiri7841-esol <109567032+v-nojiri7841-esol@users.noreply.github.com> Date: Fri, 12 May 2023 17:31:07 +0900 Subject: [PATCH 44/92] fix(tier4_pcl_extensions): modify build error in humble (#439) Signed-off-by: v-nojiri7841-esol --- .../tier4_pcl_extensions/voxel_grid_nearest_centroid.hpp | 9 +++++++-- .../voxel_grid_nearest_centroid_impl.hpp | 4 ++++ 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid.hpp b/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid.hpp index d3ae763ffe4bf..670e58fb936cc 100644 --- a/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid.hpp +++ b/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid.hpp @@ -51,7 +51,12 @@ #ifndef TIER4_PCL_EXTENSIONS__VOXEL_GRID_NEAREST_CENTROID_HPP_ #define TIER4_PCL_EXTENSIONS__VOXEL_GRID_NEAREST_CENTROID_HPP_ +#include + +#if PCL_VERSION < PCL_VERSION_CALC(1, 12, 0) #include +#endif + #include #include #include @@ -97,8 +102,8 @@ class VoxelGridNearestCentroid : public VoxelGrid typedef typename PointCloud::ConstPtr PointCloudConstPtr; public: - typedef boost::shared_ptr> Ptr; - typedef boost::shared_ptr> ConstPtr; + typedef pcl::shared_ptr> Ptr; + typedef pcl::shared_ptr> ConstPtr; /** \brief Simple structure to hold a centroid, covariance and the number of points in a leaf. * Inverse covariance, eigen vectors and eigen values are precomputed. */ diff --git a/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid_impl.hpp b/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid_impl.hpp index 84ebacf4e57ed..5a8a8c3619cdd 100644 --- a/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid_impl.hpp +++ b/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid_impl.hpp @@ -57,7 +57,11 @@ #include #include +#include + +#if PCL_VERSION < PCL_VERSION_CALC(1, 12, 0) #include +#endif #include #include From 2502fef91bfab35c0c00f84b5f34fb95697e84b3 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 12 May 2023 18:10:46 +0900 Subject: [PATCH 45/92] fix(pointcloud_preprocessor): fix build error in Humble (#441) * fix(pointcloud_preprocessor): fix build error in Humble * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../distortion_corrector/distortion_corrector.hpp | 2 +- .../passthrough_filter/passthrough_uint16.hpp | 4 ++-- .../vector_map_filter/lanelet2_map_filter_nodelet.hpp | 2 +- .../radius_search_2d_outlier_filter_nodelet.cpp | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index d121fdbf269f0..4aa3fb26868f1 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp @@ -20,10 +20,10 @@ #include #include #include +#include #include #include -#include #include #include diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp index 266aa13b06bbb..5336efb7d81e9 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/passthrough_filter/passthrough_uint16.hpp @@ -100,8 +100,8 @@ class PassThroughUInt16 : public FilterIndices typedef typename pcl::traits::fieldList::type FieldList; public: - typedef boost::shared_ptr> Ptr; - typedef boost::shared_ptr> ConstPtr; + typedef pcl::shared_ptr> Ptr; + typedef pcl::shared_ptr> ConstPtr; /** \brief Constructor. * \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp index 59ce02d8439a0..a31d8e63e2246 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -25,7 +26,6 @@ #include #include -#include #include #include diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp index 3e31e16309b7c..3a512eb8f83ad 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp @@ -32,7 +32,7 @@ RadiusSearch2DOutlierFilterComponent::RadiusSearch2DOutlierFilterComponent( search_radius_ = static_cast(declare_parameter("search_radius", 0.2)); } - kd_tree_ = boost::make_shared>(false); + kd_tree_ = pcl::make_shared>(false); using std::placeholders::_1; set_param_res_ = this->add_on_set_parameters_callback( From 7561b9ac6101f737bc8d25daae1ea990f20d49bc Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 11:35:34 +0900 Subject: [PATCH 46/92] fix(ndt_pcl_modified): fix buid error in Humble (#445) --- localization/ndt_pcl_modified/CMakeLists.txt | 5 +++++ .../include/ndt_pcl_modified/impl/ndt.hpp | 10 ++++++++++ .../ndt_pcl_modified/include/ndt_pcl_modified/ndt.hpp | 4 ++++ 3 files changed, 19 insertions(+) diff --git a/localization/ndt_pcl_modified/CMakeLists.txt b/localization/ndt_pcl_modified/CMakeLists.txt index b7faa1eda40c0..47605e0cace66 100644 --- a/localization/ndt_pcl_modified/CMakeLists.txt +++ b/localization/ndt_pcl_modified/CMakeLists.txt @@ -41,6 +41,11 @@ target_include_directories(ndt_pcl_modified $ ) +target_include_directories(ndt_pcl_modified + SYSTEM PUBLIC + ${PCL_INCLUDE_DIRS} +) + ament_target_dependencies(ndt_pcl_modified PCL) ament_export_targets(export_ndt_pcl_modified HAS_LIBRARY_TARGET) ament_export_dependencies(PCL) diff --git a/localization/ndt_pcl_modified/include/ndt_pcl_modified/impl/ndt.hpp b/localization/ndt_pcl_modified/include/ndt_pcl_modified/impl/ndt.hpp index 0feb97a19d839..dc4da99e6222b 100644 --- a/localization/ndt_pcl_modified/include/ndt_pcl_modified/impl/ndt.hpp +++ b/localization/ndt_pcl_modified/include/ndt_pcl_modified/impl/ndt.hpp @@ -81,8 +81,13 @@ void pcl::NormalDistributionsTransformModified::comput } // Initialize Point Gradient and Hessian +#if PCL_VERSION >= PCL_VERSION_CALC(1, 12, 1) + point_jacobian_.setZero(); + point_jacobian_.block(0, 0, 3, 3).setIdentity(); +#else point_gradient_.setZero(); point_gradient_.block(0, 0, 3, 3).setIdentity(); +#endif point_hessian_.setZero(); Eigen::Transform eig_transformation; @@ -366,8 +371,13 @@ double pcl::NormalDistributionsTransformModified::comp // Hessian is unnecessary for step length determination but gradients are required // so derivative and transform data is stored for the next iteration. if (step_iterations) { +#if PCL_VERSION >= PCL_VERSION_CALC(1, 12, 1) + NormalDistributionsTransformModified::computeHessian( + hessian, trans_cloud); +#else NormalDistributionsTransformModified::computeHessian( hessian, trans_cloud, x_t); +#endif } return a_t; diff --git a/localization/ndt_pcl_modified/include/ndt_pcl_modified/ndt.hpp b/localization/ndt_pcl_modified/include/ndt_pcl_modified/ndt.hpp index 81a2e219f2b81..7a9958bfdd441 100644 --- a/localization/ndt_pcl_modified/include/ndt_pcl_modified/ndt.hpp +++ b/localization/ndt_pcl_modified/include/ndt_pcl_modified/ndt.hpp @@ -102,7 +102,11 @@ class NormalDistributionsTransformModified using NormalDistributionsTransform::step_size_; using NormalDistributionsTransform::gauss_d1_; using NormalDistributionsTransform::gauss_d2_; +#if PCL_VERSION >= PCL_VERSION_CALC(1, 12, 1) + using NormalDistributionsTransform::point_jacobian_; +#else using NormalDistributionsTransform::point_gradient_; +#endif using NormalDistributionsTransform::point_hessian_; using NormalDistributionsTransform::trans_probability_; From 772b1f392c904b0053c7aeb70cb7f8b529c3c304 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 12:01:10 +0900 Subject: [PATCH 47/92] fix(ndt): fix build error in Humble (#446) --- localization/ndt/CMakeLists.txt | 5 +++++ localization/ndt/include/ndt/base.hpp | 12 ++++++------ localization/ndt/include/ndt/impl/omp.hpp | 10 +++++----- localization/ndt/include/ndt/impl/pcl_generic.hpp | 10 +++++----- localization/ndt/include/ndt/impl/pcl_modified.hpp | 10 +++++----- localization/ndt/include/ndt/omp.hpp | 14 +++++++------- localization/ndt/include/ndt/pcl_generic.hpp | 14 +++++++------- localization/ndt/include/ndt/pcl_modified.hpp | 14 +++++++------- 8 files changed, 47 insertions(+), 42 deletions(-) diff --git a/localization/ndt/CMakeLists.txt b/localization/ndt/CMakeLists.txt index ceaa9f6e9c0c1..e7542aee07cfb 100644 --- a/localization/ndt/CMakeLists.txt +++ b/localization/ndt/CMakeLists.txt @@ -46,6 +46,11 @@ target_include_directories(ndt $ ) +target_include_directories(ndt + SYSTEM PUBLIC + ${PCL_INCLUDE_DIRS} +) + ament_target_dependencies(ndt PUBLIC ndt_omp ndt_pcl_modified) # Can't use ament_target_dependencies() here because it doesn't link PCL # properly, see ndt_omp for more information. diff --git a/localization/ndt/include/ndt/base.hpp b/localization/ndt/include/ndt/base.hpp index f06e5d057f273..b81e7f1c57f6f 100644 --- a/localization/ndt/include/ndt/base.hpp +++ b/localization/ndt/include/ndt/base.hpp @@ -15,7 +15,7 @@ #ifndef NDT__BASE_HPP_ #define NDT__BASE_HPP_ -#include +#include #include #include #include @@ -30,8 +30,8 @@ class NormalDistributionsTransformBase virtual ~NormalDistributionsTransformBase() = default; virtual void align(pcl::PointCloud & output, const Eigen::Matrix4f & guess) = 0; - virtual void setInputTarget(const boost::shared_ptr> & map_ptr) = 0; - virtual void setInputSource(const boost::shared_ptr> & scan_ptr) = 0; + virtual void setInputTarget(const pcl::shared_ptr> & map_ptr) = 0; + virtual void setInputSource(const pcl::shared_ptr> & scan_ptr) = 0; virtual void setMaximumIterations(int max_iter) = 0; virtual void setResolution(float res) = 0; @@ -46,15 +46,15 @@ class NormalDistributionsTransformBase virtual double getTransformationProbability() const = 0; virtual double getNearestVoxelTransformationLikelihood() const = 0; virtual double getFitnessScore() = 0; - virtual boost::shared_ptr> getInputTarget() const = 0; - virtual boost::shared_ptr> getInputSource() const = 0; + virtual pcl::shared_ptr> getInputTarget() const = 0; + virtual pcl::shared_ptr> getInputSource() const = 0; virtual Eigen::Matrix4f getFinalTransformation() const = 0; virtual std::vector> getFinalTransformationArray() const = 0; virtual Eigen::Matrix getHessian() const = 0; - virtual boost::shared_ptr> getSearchMethodTarget() const = 0; + virtual pcl::shared_ptr> getSearchMethodTarget() const = 0; virtual double calculateTransformationProbability( const pcl::PointCloud & trans_cloud) const = 0; diff --git a/localization/ndt/include/ndt/impl/omp.hpp b/localization/ndt/include/ndt/impl/omp.hpp index 1308ec97d479f..353439332c76f 100644 --- a/localization/ndt/include/ndt/impl/omp.hpp +++ b/localization/ndt/include/ndt/impl/omp.hpp @@ -34,14 +34,14 @@ void NormalDistributionsTransformOMP::align( template void NormalDistributionsTransformOMP::setInputTarget( - const boost::shared_ptr> & map_ptr) + const pcl::shared_ptr> & map_ptr) { ndt_ptr_->setInputTarget(map_ptr); } template void NormalDistributionsTransformOMP::setInputSource( - const boost::shared_ptr> & scan_ptr) + const pcl::shared_ptr> & scan_ptr) { ndt_ptr_->setInputSource(scan_ptr); } @@ -122,14 +122,14 @@ double NormalDistributionsTransformOMP::getFitnessScor } template -boost::shared_ptr> +pcl::shared_ptr> NormalDistributionsTransformOMP::getInputTarget() const { return ndt_ptr_->getInputTarget(); } template -boost::shared_ptr> +pcl::shared_ptr> NormalDistributionsTransformOMP::getInputSource() const { return ndt_ptr_->getInputSource(); @@ -158,7 +158,7 @@ Eigen::Matrix NormalDistributionsTransformOMP -boost::shared_ptr> +pcl::shared_ptr> NormalDistributionsTransformOMP::getSearchMethodTarget() const { return ndt_ptr_->getSearchMethodTarget(); diff --git a/localization/ndt/include/ndt/impl/pcl_generic.hpp b/localization/ndt/include/ndt/impl/pcl_generic.hpp index d05c946a459a4..329093cd28589 100644 --- a/localization/ndt/include/ndt/impl/pcl_generic.hpp +++ b/localization/ndt/include/ndt/impl/pcl_generic.hpp @@ -35,14 +35,14 @@ void NormalDistributionsTransformPCLGeneric::align( template void NormalDistributionsTransformPCLGeneric::setInputTarget( - const boost::shared_ptr> & map_ptr) + const pcl::shared_ptr> & map_ptr) { ndt_ptr_->setInputTarget(map_ptr); } template void NormalDistributionsTransformPCLGeneric::setInputSource( - const boost::shared_ptr> & scan_ptr) + const pcl::shared_ptr> & scan_ptr) { ndt_ptr_->setInputSource(scan_ptr); } @@ -125,14 +125,14 @@ double NormalDistributionsTransformPCLGeneric::getFitn } template -boost::shared_ptr> +pcl::shared_ptr> NormalDistributionsTransformPCLGeneric::getInputTarget() const { return ndt_ptr_->getInputTarget(); } template -boost::shared_ptr> +pcl::shared_ptr> NormalDistributionsTransformPCLGeneric::getInputSource() const { return ndt_ptr_->getInputSource(); @@ -162,7 +162,7 @@ NormalDistributionsTransformPCLGeneric::getHessian() c } template -boost::shared_ptr> +pcl::shared_ptr> NormalDistributionsTransformPCLGeneric::getSearchMethodTarget() const { return ndt_ptr_->getSearchMethodTarget(); diff --git a/localization/ndt/include/ndt/impl/pcl_modified.hpp b/localization/ndt/include/ndt/impl/pcl_modified.hpp index 4715b85ffb4d4..f6c359fcb7158 100644 --- a/localization/ndt/include/ndt/impl/pcl_modified.hpp +++ b/localization/ndt/include/ndt/impl/pcl_modified.hpp @@ -35,14 +35,14 @@ void NormalDistributionsTransformPCLModified::align( template void NormalDistributionsTransformPCLModified::setInputTarget( - const boost::shared_ptr> & map_ptr) + const pcl::shared_ptr> & map_ptr) { ndt_ptr_->setInputTarget(map_ptr); } template void NormalDistributionsTransformPCLModified::setInputSource( - const boost::shared_ptr> & scan_ptr) + const pcl::shared_ptr> & scan_ptr) { ndt_ptr_->setInputSource(scan_ptr); } @@ -126,14 +126,14 @@ double NormalDistributionsTransformPCLModified::getFit } template -boost::shared_ptr> +pcl::shared_ptr> NormalDistributionsTransformPCLModified::getInputTarget() const { return ndt_ptr_->getInputTarget(); } template -boost::shared_ptr> +pcl::shared_ptr> NormalDistributionsTransformPCLModified::getInputSource() const { return ndt_ptr_->getInputSource(); @@ -162,7 +162,7 @@ NormalDistributionsTransformPCLModified::getHessian() } template -boost::shared_ptr> +pcl::shared_ptr> NormalDistributionsTransformPCLModified::getSearchMethodTarget() const { return ndt_ptr_->getSearchMethodTarget(); diff --git a/localization/ndt/include/ndt/omp.hpp b/localization/ndt/include/ndt/omp.hpp index 95c5f1bd4b2dc..9c323097cc8b6 100644 --- a/localization/ndt/include/ndt/omp.hpp +++ b/localization/ndt/include/ndt/omp.hpp @@ -17,7 +17,7 @@ #include "ndt/base.hpp" -#include +#include #include #include #include @@ -33,8 +33,8 @@ class NormalDistributionsTransformOMP ~NormalDistributionsTransformOMP() = default; void align(pcl::PointCloud & output, const Eigen::Matrix4f & guess) override; - void setInputTarget(const boost::shared_ptr> & map_ptr) override; - void setInputSource(const boost::shared_ptr> & scan_ptr) override; + void setInputTarget(const pcl::shared_ptr> & map_ptr) override; + void setInputSource(const pcl::shared_ptr> & scan_ptr) override; void setMaximumIterations(int max_iter) override; void setResolution(float res) override; @@ -49,15 +49,15 @@ class NormalDistributionsTransformOMP double getTransformationProbability() const override; double getNearestVoxelTransformationLikelihood() const override; double getFitnessScore() override; - boost::shared_ptr> getInputTarget() const override; - boost::shared_ptr> getInputSource() const override; + pcl::shared_ptr> getInputTarget() const override; + pcl::shared_ptr> getInputSource() const override; Eigen::Matrix4f getFinalTransformation() const override; std::vector> getFinalTransformationArray() const override; Eigen::Matrix getHessian() const override; - boost::shared_ptr> getSearchMethodTarget() const override; + pcl::shared_ptr> getSearchMethodTarget() const override; double calculateTransformationProbability( const pcl::PointCloud & trans_cloud) const override; @@ -72,7 +72,7 @@ class NormalDistributionsTransformOMP pclomp::NeighborSearchMethod getNeighborhoodSearchMethod() const; private: - boost::shared_ptr> ndt_ptr_; + pcl::shared_ptr> ndt_ptr_; }; #include "ndt/impl/omp.hpp" diff --git a/localization/ndt/include/ndt/pcl_generic.hpp b/localization/ndt/include/ndt/pcl_generic.hpp index 8ddb7dcc34006..63928510d54b2 100644 --- a/localization/ndt/include/ndt/pcl_generic.hpp +++ b/localization/ndt/include/ndt/pcl_generic.hpp @@ -17,7 +17,7 @@ #include "ndt/base.hpp" -#include +#include #include #include #include @@ -33,8 +33,8 @@ class NormalDistributionsTransformPCLGeneric ~NormalDistributionsTransformPCLGeneric() = default; void align(pcl::PointCloud & output, const Eigen::Matrix4f & guess) override; - void setInputTarget(const boost::shared_ptr> & map_ptr) override; - void setInputSource(const boost::shared_ptr> & scan_ptr) override; + void setInputTarget(const pcl::shared_ptr> & map_ptr) override; + void setInputSource(const pcl::shared_ptr> & scan_ptr) override; void setMaximumIterations(int max_iter) override; void setResolution(float res) override; @@ -49,15 +49,15 @@ class NormalDistributionsTransformPCLGeneric double getTransformationProbability() const override; double getNearestVoxelTransformationLikelihood() const override; double getFitnessScore() override; - boost::shared_ptr> getInputTarget() const override; - boost::shared_ptr> getInputSource() const override; + pcl::shared_ptr> getInputTarget() const override; + pcl::shared_ptr> getInputSource() const override; Eigen::Matrix4f getFinalTransformation() const override; std::vector> getFinalTransformationArray() const override; Eigen::Matrix getHessian() const override; - boost::shared_ptr> getSearchMethodTarget() const override; + pcl::shared_ptr> getSearchMethodTarget() const override; double calculateTransformationProbability( const pcl::PointCloud & trans_cloud) const override; @@ -65,7 +65,7 @@ class NormalDistributionsTransformPCLGeneric const pcl::PointCloud & trans_cloud) const override; private: - boost::shared_ptr> ndt_ptr_; + pcl::shared_ptr> ndt_ptr_; }; #include "ndt/impl/pcl_generic.hpp" diff --git a/localization/ndt/include/ndt/pcl_modified.hpp b/localization/ndt/include/ndt/pcl_modified.hpp index 34f50cd7553ba..df993566481e5 100644 --- a/localization/ndt/include/ndt/pcl_modified.hpp +++ b/localization/ndt/include/ndt/pcl_modified.hpp @@ -19,7 +19,7 @@ #include -#include +#include #include #include @@ -34,8 +34,8 @@ class NormalDistributionsTransformPCLModified ~NormalDistributionsTransformPCLModified() = default; void align(pcl::PointCloud & output, const Eigen::Matrix4f & guess) override; - void setInputTarget(const boost::shared_ptr> & map_ptr) override; - void setInputSource(const boost::shared_ptr> & scan_ptr) override; + void setInputTarget(const pcl::shared_ptr> & map_ptr) override; + void setInputSource(const pcl::shared_ptr> & scan_ptr) override; void setMaximumIterations(int max_iter) override; void setResolution(float res) override; @@ -50,15 +50,15 @@ class NormalDistributionsTransformPCLModified double getTransformationProbability() const override; double getNearestVoxelTransformationLikelihood() const override; double getFitnessScore() override; - boost::shared_ptr> getInputTarget() const override; - boost::shared_ptr> getInputSource() const override; + pcl::shared_ptr> getInputTarget() const override; + pcl::shared_ptr> getInputSource() const override; Eigen::Matrix4f getFinalTransformation() const override; std::vector> getFinalTransformationArray() const override; Eigen::Matrix getHessian() const override; - boost::shared_ptr> getSearchMethodTarget() const override; + pcl::shared_ptr> getSearchMethodTarget() const override; double calculateTransformationProbability( const pcl::PointCloud & trans_cloud) const override; @@ -66,7 +66,7 @@ class NormalDistributionsTransformPCLModified const pcl::PointCloud & trans_cloud) const override; private: - boost::shared_ptr> ndt_ptr_; + pcl::shared_ptr> ndt_ptr_; }; #include "ndt/impl/pcl_modified.hpp" From 6b341aaeee506d95bf49796b5dba59ff3a03e2c0 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 12:25:49 +0900 Subject: [PATCH 48/92] fix(ndt_scan_matcher): fix build error in Humble (#447) * fix(ndt_scan_matcher): fix build error in Humble * ci(pre-commit): autofix * Update localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../include/ndt_scan_matcher/ndt_scan_matcher_core.hpp | 4 ++-- .../include/ndt_scan_matcher/util_func.hpp | 6 +++--- localization/ndt_scan_matcher/package.xml | 1 + .../ndt_scan_matcher/src/ndt_scan_matcher_core.cpp | 10 ++++------ 4 files changed, 10 insertions(+), 11 deletions(-) diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index e0dd7f4022b15..4ef2f88914831 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -29,6 +29,8 @@ #include #include #include +#include +#include #include #include #include @@ -36,11 +38,9 @@ #include #include -#include #include #include #include -#include #include #include diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp index 85fecf11f060c..1a948b0a00a59 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp @@ -15,14 +15,14 @@ #ifndef NDT_SCAN_MATCHER__UTIL_FUNC_HPP_ #define NDT_SCAN_MATCHER__UTIL_FUNC_HPP_ +#include + #include #include #include +#include #include -#include -#include - #include #include #include diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index 9b6bb71d60426..b42a938089931 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -19,6 +19,7 @@ sensor_msgs std_msgs tf2 + tf2_eigen tf2_geometry_msgs tf2_ros tf2_sensor_msgs diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index ce0c0b804d1ca..caa415f8ff196 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -19,13 +19,11 @@ #include "ndt_scan_matcher/particle.hpp" #include "ndt_scan_matcher/util_func.hpp" +#include #include #include -#include - #include -#include #include #include @@ -410,7 +408,7 @@ void NDTScanMatcher::callbackMapPoints( new_ndt_ptr_->setResolution(resolution); new_ndt_ptr_->setMaximumIterations(max_iterations); - boost::shared_ptr> map_points_ptr(new pcl::PointCloud); + pcl::shared_ptr> map_points_ptr(new pcl::PointCloud); pcl::fromROSMsg(*map_points_msg_ptr, *map_points_ptr); new_ndt_ptr_->setInputTarget(map_points_ptr); // create Thread @@ -434,7 +432,7 @@ void NDTScanMatcher::callbackSensorPoints( const std::string & sensor_frame = sensor_points_sensorTF_msg_ptr->header.frame_id; const rclcpp::Time sensor_ros_time = sensor_points_sensorTF_msg_ptr->header.stamp; - boost::shared_ptr> sensor_points_sensorTF_ptr( + pcl::shared_ptr> sensor_points_sensorTF_ptr( new pcl::PointCloud); pcl::fromROSMsg(*sensor_points_sensorTF_msg_ptr, *sensor_points_sensorTF_ptr); // get TF base to sensor @@ -442,7 +440,7 @@ void NDTScanMatcher::callbackSensorPoints( getTransform(base_frame_, sensor_frame, TF_base_to_sensor_ptr); const Eigen::Affine3d base_to_sensor_affine = tf2::transformToEigen(*TF_base_to_sensor_ptr); const Eigen::Matrix4f base_to_sensor_matrix = base_to_sensor_affine.matrix().cast(); - boost::shared_ptr> sensor_points_baselinkTF_ptr( + pcl::shared_ptr> sensor_points_baselinkTF_ptr( new pcl::PointCloud); pcl::transformPointCloud( *sensor_points_sensorTF_ptr, *sensor_points_baselinkTF_ptr, base_to_sensor_matrix); From 7d3abd1fdd5b400911aabc01754b45cd80196bb6 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 12:52:34 +0900 Subject: [PATCH 49/92] fix(gyro_odometer): fix build error in Humble (#448) * fix(gyro_odometer): fix build error in Humble * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp | 2 +- localization/gyro_odometer/src/gyro_odometer_core.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp index b7dc9ea3414f4..6bb875887d73a 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -20,9 +20,9 @@ #include #include #include +#include #include -#include #include #include diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index 9e57abbf7cedb..ae696385004a6 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -14,7 +14,7 @@ #include "gyro_odometer/gyro_odometer_core.hpp" -#include +#include #include #include From e34ec19e539ca2e546162c079109b727da9eb78e Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 12:52:44 +0900 Subject: [PATCH 50/92] fix(tier4_perception_rviz_plugin): fix build error in Humble (#449) * fix(tier4_perception_rviz_plugin): fix build error in Humble * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/tools/interactive_object.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp index d33a7ea8b4e2c..a9a6ce9e806dd 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp @@ -63,10 +63,10 @@ #include #include +#include #include -#include #include #include From 62737e5c4142a0ebf2accb77bb1ff0a4419c549e Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 12:57:58 +0900 Subject: [PATCH 51/92] fix(tier4_vehicle_rviz_plugin): fix build warning in Humble (#451) --- common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp index ee922e017511b..e0bdd3e350718 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp @@ -130,7 +130,7 @@ void SteeringAngleDisplay::update(float wall_dt, float ros_dt) // ((property_handle_angle_scale_->getFloat() * (msg_ptr->data / M_PI) * -180.0)); int handle_image_width = handle_image_.width(), handle_image_height = handle_image_.height(); QPixmap rotate_handle_image; - rotate_handle_image = handle_image_.transformed(rotation_matrix); + rotate_handle_image = handle_image_.transformed(QTransform(rotation_matrix)); rotate_handle_image = rotate_handle_image.copy( (rotate_handle_image.width() - handle_image_width) / 2, (rotate_handle_image.height() - handle_image_height) / 2, handle_image_width, From 36358a2798192f1aa160382c652e336d41a5ec2d Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 12:58:08 +0900 Subject: [PATCH 52/92] fix(tier4_planning_rviz_plugin): fix build error in Humble (#450) * fix(tier4_planning_rviz_plugin): fix build error in Humble * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/mission_checkpoint/mission_checkpoint.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/common/tier4_planning_rviz_plugin/src/mission_checkpoint/mission_checkpoint.cpp b/common/tier4_planning_rviz_plugin/src/mission_checkpoint/mission_checkpoint.cpp index a3678158c32aa..cb6f23b38d131 100644 --- a/common/tier4_planning_rviz_plugin/src/mission_checkpoint/mission_checkpoint.cpp +++ b/common/tier4_planning_rviz_plugin/src/mission_checkpoint/mission_checkpoint.cpp @@ -47,7 +47,8 @@ #include -#include +#include + #include #include From ce343ab451f73a9e63b915336d92b05f5ccdf982 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 14:07:38 +0900 Subject: [PATCH 53/92] fix(localization_error_monitor): fix build warning in Humble (#452) * fix(localization_error_monitor): fix build warning in Humble * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/localization_error_monitor/src/node.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/localization/localization_error_monitor/src/node.cpp b/localization/localization_error_monitor/src/node.cpp index d3cbdbeb4347c..82647c14202e7 100644 --- a/localization/localization_error_monitor/src/node.cpp +++ b/localization/localization_error_monitor/src/node.cpp @@ -16,8 +16,9 @@ #include +#include + #include -#include #include #include From 39ba4f1ce908663b1bbc64fab7c0e0494f20dc30 Mon Sep 17 00:00:00 2001 From: v-sugahara7993-esol <109567391+v-sugahara7993-esol@users.noreply.github.com> Date: Mon, 15 May 2023 14:11:12 +0900 Subject: [PATCH 54/92] fix(interpolation): modify implementation of template function (#458) Signed-off-by: v-sugahara7993-esol --- .../spline_interpolation_points_2d.hpp | 10 +++++- .../src/spline_interpolation_points_2d.cpp | 36 +++++++++---------- 2 files changed, 27 insertions(+), 19 deletions(-) diff --git a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp index 7d58213e115a5..c1f08a6d937ae 100644 --- a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp +++ b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp @@ -46,7 +46,14 @@ class SplineInterpolationPoints2d SplineInterpolationPoints2d() = default; template - void calcSplineCoefficients(const std::vector & points); + void calcSplineCoefficients(const std::vector & points) + { + std::vector points_inner; + for (const auto & p : points) { + points_inner.push_back(tier4_autoware_utils::getPoint(p)); + } + calcSplineCoefficientsInner(points_inner); + } // TODO(murooka) implement these functions // std::vector getSplineInterpolatedPoints(const double width); @@ -58,6 +65,7 @@ class SplineInterpolationPoints2d double getAccumulatedLength(const size_t idx) const; private: + void calcSplineCoefficientsInner(const std::vector & points); SplineInterpolation slerp_x_; SplineInterpolation slerp_y_; diff --git a/common/interpolation/src/spline_interpolation_points_2d.cpp b/common/interpolation/src/spline_interpolation_points_2d.cpp index e0c0caf27d0c0..2a34e53fc884e 100644 --- a/common/interpolation/src/spline_interpolation_points_2d.cpp +++ b/common/interpolation/src/spline_interpolation_points_2d.cpp @@ -35,16 +35,16 @@ std::vector calcEuclidDist(const std::vector & x, const std::vec return dist_v; } -template -std::array, 3> getBaseValues(const std::vector & points) +std::array, 3> getBaseValues( + const std::vector & points) { // calculate x, y std::vector base_x; std::vector base_y; for (size_t i = 0; i < points.size(); i++) { - const auto & current_pos = tier4_autoware_utils::getPoint(points.at(i)); + const auto & current_pos = points.at(i); if (i > 0) { - const auto & prev_pos = tier4_autoware_utils::getPoint(points.at(i - 1)); + const auto & prev_pos = points.at(i - 1); if ( std::fabs(current_pos.x - prev_pos.x) < 1e-6 && std::fabs(current_pos.y - prev_pos.y) < 1e-6) { @@ -88,20 +88,6 @@ template std::vector slerpYawFromPoints( const std::vector & points); } // namespace interpolation -template -void SplineInterpolationPoints2d::calcSplineCoefficients(const std::vector & points) -{ - const auto base = getBaseValues(points); - - base_s_vec_ = base.at(0); - const auto & base_x_vec = base.at(1); - const auto & base_y_vec = base.at(2); - - // calculate spline coefficients - slerp_x_.calcSplineCoefficients(base_s_vec_, base_x_vec); - slerp_y_.calcSplineCoefficients(base_s_vec_, base_y_vec); -} - geometry_msgs::msg::Point SplineInterpolationPoints2d::getSplineInterpolatedPoint( const size_t idx, const double s) const { @@ -153,3 +139,17 @@ double SplineInterpolationPoints2d::getAccumulatedLength(const size_t idx) const } return base_s_vec_.at(idx); } + +void SplineInterpolationPoints2d::calcSplineCoefficientsInner( + const std::vector & points) +{ + const auto base = getBaseValues(points); + + base_s_vec_ = base.at(0); + const auto & base_x_vec = base.at(1); + const auto & base_y_vec = base.at(2); + + // calculate spline coefficients + slerp_x_.calcSplineCoefficients(base_s_vec_, base_x_vec); + slerp_y_.calcSplineCoefficients(base_s_vec_, base_y_vec); +} From a5c8a7179f9eac8bee21ce8bbbcf15e22fb219a1 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 14:12:44 +0900 Subject: [PATCH 55/92] fix(stop_filter): modify build error in rolling (#805) (#455) * fix(stop_filter): modify build error in rolling * ci(pre-commit): autofix Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/stop_filter/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/localization/stop_filter/package.xml b/localization/stop_filter/package.xml index 6e6cbf152e385..3ae3aff3e980a 100644 --- a/localization/stop_filter/package.xml +++ b/localization/stop_filter/package.xml @@ -12,7 +12,9 @@ geometry_msgs nav_msgs rclcpp + tf2 tier4_debug_msgs + ament_cmake_gtest ament_lint_auto autoware_lint_common From 08f606b51d3fd8bb9d1293228f030c33fb98a326 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 14:13:06 +0900 Subject: [PATCH 56/92] fix(map_loader): modify build error in rolling (#777) (#456) Signed-off-by: wep21 Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> --- map/map_loader/CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index b27d3933f0d55..11373def78d80 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -21,6 +21,11 @@ ament_auto_add_library(pointcloud_map_loader_node SHARED ) target_link_libraries(pointcloud_map_loader_node ${PCL_LIBRARIES}) +target_include_directories(pointcloud_map_loader_node + SYSTEM PUBLIC + ${PCL_INCLUDE_DIRS} +) + rclcpp_components_register_node(pointcloud_map_loader_node PLUGIN "PointCloudMapLoaderNode" EXECUTABLE pointcloud_map_loader From 92ad16e8d568ec8eed8ef3a4eb103025d78b0e46 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 14:13:28 +0900 Subject: [PATCH 57/92] fix(pose_initializer): fix build warning in Humble (#453) * fix(pose_initializer): fix build warning in Humble * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/pose_initializer/src/pose_initializer_core.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/localization/pose_initializer/src/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer_core.cpp index 777206a85ff04..3ca67d19d5104 100644 --- a/localization/pose_initializer/src/pose_initializer_core.cpp +++ b/localization/pose_initializer/src/pose_initializer_core.cpp @@ -14,8 +14,9 @@ #include "pose_initializer/pose_initializer_core.hpp" +#include + #include -#include #include #include @@ -190,7 +191,7 @@ bool PoseInitializer::getHeight( input_pose_msg.pose.pose.position.z); if (map_ptr_) { - tf2::Transform transform; + tf2::Transform transform{tf2::Quaternion{}, tf2::Vector3{}}; try { const auto stamped = tf2_buffer_.lookupTransform(map_frame_, fixed_frame, tf2::TimePointZero); tf2::fromMsg(stamped.transform, transform); From 579c4f71eef22f5f8cae10c17592cec56573b582 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 14:13:35 +0900 Subject: [PATCH 58/92] fix(pose2twist): fix build warning in Humble (#454) --- localization/pose2twist/src/pose2twist_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/pose2twist/src/pose2twist_core.cpp b/localization/pose2twist/src/pose2twist_core.cpp index 9eeb5f359695d..7da512a853a0a 100644 --- a/localization/pose2twist/src/pose2twist_core.cpp +++ b/localization/pose2twist/src/pose2twist_core.cpp @@ -14,7 +14,7 @@ #include "pose2twist/pose2twist_core.hpp" -#include +#include #include #include From fda8d4caa2353bd70d64de69bc7d965fa3f93852 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 14:14:23 +0900 Subject: [PATCH 59/92] fix(map_tf_generator): modify build error in rolling (#778) (#457) Signed-off-by: wep21 Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> --- map/map_tf_generator/package.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/map/map_tf_generator/package.xml b/map/map_tf_generator/package.xml index cd1ef98eb8014..e1f74fd40511d 100644 --- a/map/map_tf_generator/package.xml +++ b/map/map_tf_generator/package.xml @@ -12,7 +12,8 @@ libpcl-all-dev pcl_conversions rclcpp - std_msgs + rclcpp_components + sensor_msgs tf2 tf2_ros From af63bb6d7eb5766198290b46341a8fed6b20fd1a Mon Sep 17 00:00:00 2001 From: v-nojiri7841-esol <109567032+v-nojiri7841-esol@users.noreply.github.com> Date: Mon, 15 May 2023 14:15:34 +0900 Subject: [PATCH 60/92] fix(ad_service_state_monitor): fix build error in Humble (#459) * fix(ad_service_state_monitor): fix build error in Humble Signed-off-by: v-nojiri7841-esol * ci(pre-commit): autofix --------- Signed-off-by: v-nojiri7841-esol Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../include/ad_service_state_monitor/state_machine.hpp | 2 -- .../ad_service_state_monitor_node.cpp | 2 -- .../src/ad_service_state_monitor_node/diagnostics.cpp | 2 -- .../src/ad_service_state_monitor_node/state_machine.cpp | 7 +++++-- 4 files changed, 5 insertions(+), 8 deletions(-) diff --git a/system/ad_service_state_monitor/include/ad_service_state_monitor/state_machine.hpp b/system/ad_service_state_monitor/include/ad_service_state_monitor/state_machine.hpp index 119e3246e9431..9dee8824ef715 100644 --- a/system/ad_service_state_monitor/include/ad_service_state_monitor/state_machine.hpp +++ b/system/ad_service_state_monitor/include/ad_service_state_monitor/state_machine.hpp @@ -31,8 +31,6 @@ #include #include -#include - #include #include #include diff --git a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp index 3d1c414551a13..91116658dfd67 100644 --- a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp +++ b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/ad_service_state_monitor_node.cpp @@ -14,8 +14,6 @@ #include "ad_service_state_monitor/ad_service_state_monitor_node.hpp" -#include - #include #include #include diff --git a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/diagnostics.cpp b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/diagnostics.cpp index ee0fa94545422..503547370029b 100644 --- a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/diagnostics.cpp +++ b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/diagnostics.cpp @@ -14,8 +14,6 @@ #include "ad_service_state_monitor/ad_service_state_monitor_node.hpp" -#include - #include #include diff --git a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/state_machine.cpp b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/state_machine.cpp index 922869af99e56..aaae2817baac1 100644 --- a/system/ad_service_state_monitor/src/ad_service_state_monitor_node/state_machine.cpp +++ b/system/ad_service_state_monitor/src/ad_service_state_monitor_node/state_machine.cpp @@ -12,13 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "ad_service_state_monitor/state_machine.hpp" + #include #include -#define FMT_HEADER_ONLY -#include "ad_service_state_monitor/state_machine.hpp" +#define FMT_HEADER_ONLY // NOLINT +#include #include +#include namespace { From 3406a58ca232a178f0cfa24807c2fa28962383ac Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 14:38:44 +0900 Subject: [PATCH 61/92] fix(shape_estimation): fix build error in Humble (#460) * fix(shape_estimation): fix build error in Humble * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- perception/shape_estimation/CMakeLists.txt | 5 +++++ perception/shape_estimation/lib/corrector/utils.cpp | 6 ++++-- perception/shape_estimation/lib/model/bounding_box.cpp | 2 +- perception/shape_estimation/src/node.cpp | 2 +- 4 files changed, 11 insertions(+), 4 deletions(-) diff --git a/perception/shape_estimation/CMakeLists.txt b/perception/shape_estimation/CMakeLists.txt index d8c1c8df49d6d..080c5b6871c85 100644 --- a/perception/shape_estimation/CMakeLists.txt +++ b/perception/shape_estimation/CMakeLists.txt @@ -45,6 +45,11 @@ ament_auto_add_library(shape_estimation_lib SHARED ament_target_dependencies(shape_estimation_lib ${SHAPE_ESTIMATION_DEPENDENCIES}) +target_include_directories(shape_estimation_lib + SYSTEM PUBLIC + "${PCL_INCLUDE_DIRS}" +) + ament_auto_add_library(shape_estimation_node SHARED src/node.cpp ) diff --git a/perception/shape_estimation/lib/corrector/utils.cpp b/perception/shape_estimation/lib/corrector/utils.cpp index 82ce5e1b587b9..05a27a072d0e9 100644 --- a/perception/shape_estimation/lib/corrector/utils.cpp +++ b/perception/shape_estimation/lib/corrector/utils.cpp @@ -14,11 +14,13 @@ #include "shape_estimation/corrector/utils.hpp" +#include + +#include + #include #include #include -#include -#include #include #include diff --git a/perception/shape_estimation/lib/model/bounding_box.cpp b/perception/shape_estimation/lib/model/bounding_box.cpp index 71f9a8d48b7d6..f61dd22dd5acd 100644 --- a/perception/shape_estimation/lib/model/bounding_box.cpp +++ b/perception/shape_estimation/lib/model/bounding_box.cpp @@ -19,12 +19,12 @@ #include #include +#include #include #include #include #include -#include #include #include diff --git a/perception/shape_estimation/src/node.cpp b/perception/shape_estimation/src/node.cpp index c2db0b532333f..baea36d95a11f 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/perception/shape_estimation/src/node.cpp @@ -18,11 +18,11 @@ #include #include +#include #include #include #include -#include #include From c2f912f3e718086436fd6596828c2d4420e3a2ea Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 14:38:51 +0900 Subject: [PATCH 62/92] fix(mission_planner): fix build warning in Humble (#461) * fix(mission_planner): fix build warning in Humble * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- planning/mission_planner/lib/mission_planner_base.cpp | 3 ++- .../src/mission_planner_lanelet2/mission_planner_lanelet2.cpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/planning/mission_planner/lib/mission_planner_base.cpp b/planning/mission_planner/lib/mission_planner_base.cpp index 2d29b655602a6..5a1551a156784 100644 --- a/planning/mission_planner/lib/mission_planner_base.cpp +++ b/planning/mission_planner/lib/mission_planner_base.cpp @@ -18,8 +18,9 @@ #include #include +#include + #include -#include #include #include diff --git a/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp b/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp index 271b4a0344115..86dce77254879 100644 --- a/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp +++ b/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp @@ -21,11 +21,12 @@ #include #include +#include + #include #include #include #include -#include #include #include From 0ae551ead358f722b88e46661a528c7a52904720 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 14:50:01 +0900 Subject: [PATCH 63/92] fix(obstacle_stop_planner): fix build error in Humble (#462) * fix(obstacle_stop_planner): fix build error in Humble * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../obstacle_stop_planner/src/adaptive_cruise_control.cpp | 4 +++- planning/obstacle_stop_planner/src/debug_marker.cpp | 2 +- planning/obstacle_stop_planner/src/node.cpp | 3 ++- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp index df4209c6d1c99..d5ebf8959480d 100644 --- a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -14,6 +14,8 @@ #include "obstacle_stop_planner/adaptive_cruise_control.hpp" +#include + #include #include #include @@ -665,7 +667,7 @@ double AdaptiveCruiseController::getMedianVel(const std::vector raw_vel_que; - for (const auto vel : vel_que) { + for (const auto & vel : vel_que) { raw_vel_que.emplace_back(vel.twist.twist.linear.x); } diff --git a/planning/obstacle_stop_planner/src/debug_marker.cpp b/planning/obstacle_stop_planner/src/debug_marker.cpp index 491d2b3dfd904..3014f7da76231 100644 --- a/planning/obstacle_stop_planner/src/debug_marker.cpp +++ b/planning/obstacle_stop_planner/src/debug_marker.cpp @@ -16,7 +16,7 @@ #include -#include +#include #include #include diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 56fc621e0f002..324a37bfa9559 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -25,12 +25,13 @@ #include #include +#include #include +#include #include #include -#include namespace motion_planning { From c498b645c1d409c2118b5c13d1f35d54d2605182 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 15:36:30 +0900 Subject: [PATCH 64/92] fix(autonomous_emergency_braking): fix build error in Humble (#463) --- .../autonomous_emergency_braking/src/node.cpp | 30 ------------------- 1 file changed, 30 deletions(-) diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index f4d48380abfed..a66935d3f7915 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -26,36 +26,6 @@ #include #endif -namespace tf2 -{ -template <> -inline void doTransform( - const geometry_msgs::msg::Pose & t_in, geometry_msgs::msg::Pose & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - KDL::Vector v(t_in.position.x, t_in.position.y, t_in.position.z); - KDL::Rotation r = KDL::Rotation::Quaternion( - t_in.orientation.x, t_in.orientation.y, t_in.orientation.z, t_in.orientation.w); - - KDL::Frame v_out = gmTransformToKDL(transform) * KDL::Frame(r, v); - t_out.position.x = v_out.p[0]; - t_out.position.y = v_out.p[1]; - t_out.position.z = v_out.p[2]; - v_out.M.GetQuaternion( - t_out.orientation.x, t_out.orientation.y, t_out.orientation.z, t_out.orientation.w); -} - -template <> -inline void doTransform( - const geometry_msgs::msg::Point & t_in, geometry_msgs::msg::Point & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - KDL::Vector v_out = gmTransformToKDL(transform) * KDL::Vector(t_in.x, t_in.y, t_in.z); - t_out.x = v_out[0]; - t_out.y = v_out[1]; - t_out.z = v_out[2]; -} -} // namespace tf2 namespace autoware::motion::control::autonomous_emergency_braking { using diagnostic_msgs::msg::DiagnosticStatus; From 06de19ca6906c072b7eeabd038c7d1cb562ea923 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 15:41:59 +0900 Subject: [PATCH 65/92] fix(trajectory_follower): fix build error in Humble (#464) --- .../include/trajectory_follower/mpc_utils.hpp | 2 +- .../trajectory_follower/src/longitudinal_controller_utils.cpp | 2 +- control/trajectory_follower/src/pid.cpp | 1 + .../test/test_longitudinal_controller_utils.cpp | 2 +- control/trajectory_follower/test/test_mpc.cpp | 2 +- 5 files changed, 5 insertions(+), 4 deletions(-) diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp index ec00273225e3a..259d08c4ccc67 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp @@ -36,7 +36,7 @@ #include "motion_common/motion_common.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace autoware diff --git a/control/trajectory_follower/src/longitudinal_controller_utils.cpp b/control/trajectory_follower/src/longitudinal_controller_utils.cpp index 54cb05d3448d1..875887556ce8f 100644 --- a/control/trajectory_follower/src/longitudinal_controller_utils.cpp +++ b/control/trajectory_follower/src/longitudinal_controller_utils.cpp @@ -21,7 +21,7 @@ #include "motion_common/motion_common.hpp" #include "tf2/LinearMath/Matrix3x3.h" #include "tf2/LinearMath/Quaternion.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace autoware diff --git a/control/trajectory_follower/src/pid.cpp b/control/trajectory_follower/src/pid.cpp index 1d61114b05972..053b4373bb0e3 100644 --- a/control/trajectory_follower/src/pid.cpp +++ b/control/trajectory_follower/src/pid.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include diff --git a/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp b/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp index 4256bf9d0f74e..47151438ea928 100644 --- a/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp +++ b/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp @@ -22,7 +22,7 @@ #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/quaternion.hpp" #include "tf2/LinearMath/Quaternion.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace longitudinal_utils = ::autoware::motion::control::trajectory_follower::longitudinal_utils; diff --git a/control/trajectory_follower/test/test_mpc.cpp b/control/trajectory_follower/test/test_mpc.cpp index 52c9b60613082..18f8f313edc05 100644 --- a/control/trajectory_follower/test/test_mpc.cpp +++ b/control/trajectory_follower/test/test_mpc.cpp @@ -30,7 +30,7 @@ #include "common/types.hpp" #include "geometry_msgs/msg/pose.hpp" #include "gtest/gtest.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace { From 91889c13378cc28f95cb78e6f4c0b51d63e14b04 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 15:43:25 +0900 Subject: [PATCH 66/92] fix(compare_map_segmentation): fix build error in Humble (#465) --- .../src/distance_based_compare_map_filter_nodelet.cpp | 2 +- .../src/voxel_based_approximate_compare_map_filter_nodelet.cpp | 2 +- .../src/voxel_based_compare_map_filter_nodelet.cpp | 2 +- .../src/voxel_distance_based_compare_map_filter_nodelet.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp index a3f216fef5952..ab51a71627567 100644 --- a/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/distance_based_compare_map_filter_nodelet.cpp @@ -63,7 +63,7 @@ void DistanceBasedCompareMapFilterComponent::input_target_callback(const PointCl { pcl::PointCloud map_pcl; pcl::fromROSMsg(*map, map_pcl); - const auto map_pcl_ptr = boost::make_shared>(map_pcl); + const auto map_pcl_ptr = pcl::make_shared>(map_pcl); boost::mutex::scoped_lock lock(mutex_); map_ptr_ = map_pcl_ptr; diff --git a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp index 78bdf191f3d8a..70f90bed924a0 100644 --- a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp @@ -72,7 +72,7 @@ void VoxelBasedApproximateCompareMapFilterComponent::input_target_callback( { pcl::PointCloud map_pcl; pcl::fromROSMsg(*map, map_pcl); - const auto map_pcl_ptr = boost::make_shared>(map_pcl); + const auto map_pcl_ptr = pcl::make_shared>(map_pcl); boost::mutex::scoped_lock lock(mutex_); set_map_in_voxel_grid_ = true; diff --git a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp index 7afce3d35bc69..3c206f7cec7b4 100644 --- a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp @@ -239,7 +239,7 @@ void VoxelBasedCompareMapFilterComponent::input_target_callback(const PointCloud { pcl::PointCloud map_pcl; pcl::fromROSMsg(*map, map_pcl); - const auto map_pcl_ptr = boost::make_shared>(map_pcl); + const auto map_pcl_ptr = pcl::make_shared>(map_pcl); boost::mutex::scoped_lock lock(mutex_); set_map_in_voxel_grid_ = true; diff --git a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp index b9d001b5dc439..d7997e03f03b8 100644 --- a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp @@ -74,7 +74,7 @@ void VoxelDistanceBasedCompareMapFilterComponent::input_target_callback( { pcl::PointCloud map_pcl; pcl::fromROSMsg(*map, map_pcl); - const auto map_pcl_ptr = boost::make_shared>(map_pcl); + const auto map_pcl_ptr = pcl::make_shared>(map_pcl); boost::mutex::scoped_lock lock(mutex_); tf_input_frame_ = map_pcl_ptr->header.frame_id; From 107a6d7a332a059a73304707d1812b0fe4deaaec Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 16:17:07 +0900 Subject: [PATCH 67/92] fix(ground_segmentation): fix build error in Humble (#466) * fix(ground_segmentation): fix build error in Humble * fix: CMakeLists * delete unused * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- perception/ground_segmentation/CMakeLists.txt | 11 ++++++----- .../ransac_ground_filter_nodelet.hpp | 3 ++- .../ground_segmentation/ray_ground_filter_nodelet.hpp | 3 ++- .../scan_ground_filter_nodelet.hpp | 2 +- .../src/ransac_ground_filter_nodelet.cpp | 3 +-- .../src/ray_ground_filter_nodelet.cpp | 2 +- 6 files changed, 13 insertions(+), 11 deletions(-) diff --git a/perception/ground_segmentation/CMakeLists.txt b/perception/ground_segmentation/CMakeLists.txt index 076f9cbc01cb4..1db192cb4bc39 100644 --- a/perception/ground_segmentation/CMakeLists.txt +++ b/perception/ground_segmentation/CMakeLists.txt @@ -32,11 +32,12 @@ ament_auto_find_build_dependencies() include_directories( include - ${Boost_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${GRID_MAP_INCLUDE_DIR} + SYSTEM + ${Boost_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${GRID_MAP_INCLUDE_DIR} ) ament_auto_add_library(ground_segmentation SHARED diff --git a/perception/ground_segmentation/include/ground_segmentation/ransac_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/ransac_ground_filter_nodelet.hpp index 6e46f2a376bf5..498fcca6a302a 100644 --- a/perception/ground_segmentation/include/ground_segmentation/ransac_ground_filter_nodelet.hpp +++ b/perception/ground_segmentation/include/ground_segmentation/ransac_ground_filter_nodelet.hpp @@ -17,6 +17,8 @@ #include "pointcloud_preprocessor/filter.hpp" +#include + #include #include @@ -25,7 +27,6 @@ #include #include #include -#include #include #include diff --git a/perception/ground_segmentation/include/ground_segmentation/ray_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/ray_ground_filter_nodelet.hpp index cbef2564920ac..44af8b766df62 100644 --- a/perception/ground_segmentation/include/ground_segmentation/ray_ground_filter_nodelet.hpp +++ b/perception/ground_segmentation/include/ground_segmentation/ray_ground_filter_nodelet.hpp @@ -45,13 +45,14 @@ #ifndef GROUND_SEGMENTATION__RAY_GROUND_FILTER_NODELET_HPP_ #define GROUND_SEGMENTATION__RAY_GROUND_FILTER_NODELET_HPP_ +#include + #include #include #include #include #include -#include #include #include diff --git a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp index 8ec423cd9be98..6680891bfe0cf 100644 --- a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp +++ b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp @@ -17,6 +17,7 @@ #include "pointcloud_preprocessor/filter.hpp" +#include #include #include @@ -25,7 +26,6 @@ #include #include #include -#include #include #include diff --git a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp index a59d8b8b8bbb5..45aeedabe01cb 100644 --- a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp @@ -17,7 +17,6 @@ #include #include -#include #include #include @@ -187,7 +186,7 @@ void RANSACGroundFilterComponent::extractPointsIndices( { pcl::ExtractIndices extract_ground; extract_ground.setInputCloud(in_cloud_ptr); - extract_ground.setIndices(boost::make_shared(in_indices)); + extract_ground.setIndices(pcl::make_shared(in_indices)); extract_ground.setNegative(false); // true removes the indices, false leaves only the indices extract_ground.filter(*out_only_indices_cloud_ptr); diff --git a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp index a452512e36bdf..51410771edb9f 100644 --- a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp @@ -297,7 +297,7 @@ void RayGroundFilterComponent::ExtractPointsIndices( { pcl::ExtractIndices extract_ground; extract_ground.setInputCloud(in_cloud_ptr); - extract_ground.setIndices(boost::make_shared(in_indices)); + extract_ground.setIndices(pcl::make_shared(in_indices)); extract_ground.setNegative(false); // true removes the indices, false leaves only the indices extract_ground.filter(*out_only_indices_cloud_ptr); From 8553fd3185d7f5276cfe474cedd49d3db7d3bcc1 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 16:22:47 +0900 Subject: [PATCH 68/92] fix(occupancy_grid_map_outlier_filter): fix build error in Humble (#467) * fix(occupancy_grid_map_outlier_filter): fix build error in Humble * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../occupancy_grid_map_outlier_filter_nodelet.hpp | 1 + .../src/occupancy_grid_map_outlier_filter_nodelet.cpp | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp index 4ae0fde705e18..a79d5bb35a575 100644 --- a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp +++ b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp @@ -31,6 +31,7 @@ #include #include #include +#include #include #include #include diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index 8b47cee943138..b8c605033f4d8 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -15,13 +15,13 @@ #include "occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp" #include +#include #include #include #include #include -#include #include #include @@ -103,7 +103,7 @@ RadiusSearch2dfilter::RadiusSearch2dfilter(rclcpp::Node & node) node.declare_parameter("radius_search_2d_filter.min_points_and_distance_ratio", 400.0f); min_points_ = node.declare_parameter("radius_search_2d_filter.min_points", 4); max_points_ = node.declare_parameter("radius_search_2d_filter.max_points", 70); - kd_tree_ = boost::make_shared>(false); + kd_tree_ = pcl::make_shared>(false); } void RadiusSearch2dfilter::filter( From 75cc7bfcbc6232b40ad7c9f0e83a282ea1b4b0ad Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 17:21:26 +0900 Subject: [PATCH 69/92] fix(behavior_path_planner): fix build error in Humble (#468) * fix(behavior_path_planner): fix build error in Humble * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../behavior_path_planner/utilities.hpp | 32 +------------------ .../avoidance/avoidance_module.cpp | 2 +- 2 files changed, 2 insertions(+), 32 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 08d9808997011..8e3eaeb4bb455 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -34,6 +34,7 @@ #include #include #include +#include #include #include @@ -45,7 +46,6 @@ #include #include #include -#include #include #include @@ -79,36 +79,6 @@ inline void fromMsg(const geometry_msgs::msg::PoseStamped & msg, tf2::Stamped -inline void doTransform( - const geometry_msgs::msg::Point & t_in, geometry_msgs::msg::Point & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - tf2::Transform t; - fromMsg(transform.transform, t); - tf2::Vector3 v_in; - fromMsg(t_in, v_in); - tf2::Vector3 v_out = t * v_in; - toMsg(v_out, t_out); -} } // namespace tf2 namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 0bf4cf4d90b42..c306c22e88c64 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -2449,7 +2449,7 @@ void AvoidanceModule::updateRegisteredObject(const ObjectDataArray & now_objects }; // -- check now_objects, add it if it has new object id -- - for (const auto now_obj : now_objects) { + for (const auto & now_obj : now_objects) { if (!isAlreadyRegistered(now_obj.object.object_id)) { registered_objects_.push_back(now_obj); } From f3fe82ed05b4c84aa15e2abf05cbaef371cfd0c7 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 17:21:34 +0900 Subject: [PATCH 70/92] fix(surround_obstacle_checker): fix build warning in Humble (#471) * fix(surround_obstacle_checker): fix build warning in Humble * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- planning/surround_obstacle_checker/src/debug_marker.cpp | 2 +- planning/surround_obstacle_checker/src/node.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/planning/surround_obstacle_checker/src/debug_marker.cpp b/planning/surround_obstacle_checker/src/debug_marker.cpp index 0deea0ce6d912..cddaf06565f02 100644 --- a/planning/surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/surround_obstacle_checker/src/debug_marker.cpp @@ -14,7 +14,7 @@ #include "surround_obstacle_checker/debug_marker.hpp" -#include +#include #include diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/surround_obstacle_checker/src/node.cpp index fba6f2d0e99ef..4e2f0d152f735 100644 --- a/planning/surround_obstacle_checker/src/node.cpp +++ b/planning/surround_obstacle_checker/src/node.cpp @@ -14,11 +14,12 @@ #include "surround_obstacle_checker/node.hpp" +#include + #include #include #include #include -#include #include #include From 443c8486755e1a83079fbdb7cf3a41bec4f80698 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 17:21:40 +0900 Subject: [PATCH 71/92] fix(dummy_perception_publisher): fix build warning in Humble (#472) * fix(dummy_perception_publisher): fix build warning in Humble * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../include/dummy_perception_publisher/node.hpp | 2 +- simulator/dummy_perception_publisher/src/node.cpp | 3 ++- .../src/signed_distance_function.cpp | 2 +- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp index eef3eb92f7314..2577060c78ce5 100644 --- a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp +++ b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -29,7 +30,6 @@ #include #include #include -#include #include #include diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index 87a38ffadb7f6..b6c56d01dc7b2 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/simulator/dummy_perception_publisher/src/node.cpp @@ -14,11 +14,12 @@ #include "dummy_perception_publisher/node.hpp" +#include + #include #include #include #include -#include #include #include diff --git a/simulator/dummy_perception_publisher/src/signed_distance_function.cpp b/simulator/dummy_perception_publisher/src/signed_distance_function.cpp index c23252e58dc06..0e61f2d1122dc 100644 --- a/simulator/dummy_perception_publisher/src/signed_distance_function.cpp +++ b/simulator/dummy_perception_publisher/src/signed_distance_function.cpp @@ -77,7 +77,7 @@ double CompositeSDF::operator()(double x, double y) const size_t CompositeSDF::nearest_sdf_index(double x, double y) const { double min_value = std::numeric_limits::infinity(); - size_t idx_min; + size_t idx_min{}; for (size_t i = 0; i < sdf_ptrs_.size(); ++i) { const auto value = sdf_ptrs_.at(i)->operator()(x, y); if (value < min_value) { From d397e1f83df2d0ddb9c367004391c986b9031c45 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 17:32:10 +0900 Subject: [PATCH 72/92] fix(behavior_velocity_planner): fix build error in Humble (#469) * fix(behavior_velocity_planner): fix build error in Humble * ci(pre-commit): autofix * fix typo --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- planning/behavior_velocity_planner/CMakeLists.txt | 5 +++++ .../include/scene_module/intersection/util.hpp | 3 +-- .../scene_module/occlusion_spot/occlusion_spot_utils.hpp | 2 +- .../include/utilization/arc_lane_util.hpp | 5 ++--- planning/behavior_velocity_planner/src/node.cpp | 2 +- .../src/scene_module/detection_area/debug.cpp | 2 +- .../src/scene_module/detection_area/scene.cpp | 3 +-- .../src/scene_module/no_stopping_area/debug.cpp | 2 +- .../scene_module/no_stopping_area/scene_no_stopping_area.cpp | 2 +- .../src/scene_module/stop_line/debug.cpp | 2 +- .../src/scene_module/traffic_light/debug.cpp | 2 +- .../src/scene_module/traffic_light/scene.cpp | 2 +- .../src/utilization/path_utilization.cpp | 3 ++- .../test/src/test_occlusion_spot_utils.cpp | 2 +- planning/behavior_velocity_planner/test/src/utils.hpp | 2 -- 15 files changed, 20 insertions(+), 19 deletions(-) diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/CMakeLists.txt index 8cb2e1b224786..189b16a1e0517 100644 --- a/planning/behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/CMakeLists.txt @@ -67,6 +67,11 @@ target_include_directories(scene_module_lib $ ) +target_include_directories(scene_module_lib + SYSTEM PUBLIC + ${PCL_INCLUDE_DIRS} +) + ament_target_dependencies(scene_module_lib ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp index dc647df54f533..9382c456a6c48 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -19,8 +19,7 @@ #include #include - -#include +#include #include #include diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp index 356212191c7c5..e254e644ae5c9 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include @@ -34,7 +35,6 @@ #include #include #include -#include #include #include diff --git a/planning/behavior_velocity_planner/include/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner/include/utilization/arc_lane_util.hpp index b03362dd60575..e4089207b66d3 100644 --- a/planning/behavior_velocity_planner/include/utilization/arc_lane_util.hpp +++ b/planning/behavior_velocity_planner/include/utilization/arc_lane_util.hpp @@ -16,16 +16,15 @@ #define UTILIZATION__ARC_LANE_UTIL_HPP_ #include +#include #include #include #include +#include #include -#include -#include - #include #include #include diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 5d4d36bffa78a..052dad17eeef6 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -15,6 +15,7 @@ #include "behavior_velocity_planner/node.hpp" #include +#include #include #include @@ -23,7 +24,6 @@ #include #include -#include #include #include diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp index e0f7745ca0407..f4ba267c5538e 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp @@ -16,7 +16,7 @@ #include #include -#include +#include #include diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp index b3d89c333144a..8d0454d2609dd 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp @@ -13,11 +13,10 @@ // limitations under the License. #include +#include #include #include -#include - #include #include #include diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp index a467bedec8aa0..1ff4708ad7a01 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp @@ -18,7 +18,7 @@ #include -#include +#include #include #include diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp index deea9e26c160c..94c06061eaa36 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp @@ -19,10 +19,10 @@ #include "utilization/util.hpp" #include +#include #include #include -#include #include #include diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp index 56fb110283591..3c4a6307507b6 100644 --- a/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp @@ -16,7 +16,7 @@ #include #include -#include +#include namespace behavior_velocity_planner { diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/debug.cpp index 3e6677702b096..6d9e81d0072f0 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/traffic_light/debug.cpp @@ -16,7 +16,7 @@ #include #include -#include +#include namespace behavior_velocity_planner { diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp index e509b80b3464a..ccbe641070e08 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp @@ -13,13 +13,13 @@ // limitations under the License. #include +#include #include #include #include // To be replaced by std::optional in C++17 #include -#include #include #include diff --git a/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp b/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp index 4d5d4d23392ca..7eb944502f9e5 100644 --- a/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp +++ b/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp @@ -16,8 +16,9 @@ #include #include +#include + #include -#include #include #include diff --git a/planning/behavior_velocity_planner/test/src/test_occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/test/src/test_occlusion_spot_utils.cpp index 4540d534350d1..2fc6f2d34b90e 100644 --- a/planning/behavior_velocity_planner/test/src/test_occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/test/src/test_occlusion_spot_utils.cpp @@ -98,7 +98,7 @@ TEST(calcSlowDownPointsForPossibleCollision, ConsiderSignedOffset) std::cout << "v : " << path.points[i].point.longitudinal_velocity_mps << "\t"; } std::cout << std::endl; - for (const auto pc : pcs) { + for (const auto & pc : pcs) { std::cout << "len : " << pc.arc_lane_dist_at_collision.length << "\t"; } std::cout << std::endl; diff --git a/planning/behavior_velocity_planner/test/src/utils.hpp b/planning/behavior_velocity_planner/test/src/utils.hpp index 8054dfcadaca1..91574b02c65e2 100644 --- a/planning/behavior_velocity_planner/test/src/utils.hpp +++ b/planning/behavior_velocity_planner/test/src/utils.hpp @@ -202,8 +202,6 @@ inline autoware_auto_perception_msgs::msg::PredictedObject generatePredictedObje autoware_auto_perception_msgs::msg::PredictedObject obj; obj.shape.dimensions.x = 5.0; obj.shape.dimensions.y = 2.0; - tf2::Quaternion q; - obj.kinematics.initial_pose_with_covariance.pose.orientation = tf2::toMsg(q); obj.kinematics.initial_twist_with_covariance.twist.linear.x = 0; obj.kinematics.initial_pose_with_covariance.pose.position.x = x; obj.kinematics.initial_pose_with_covariance.pose.position.y = 0; From 540375b42efdd799c69b31e69b62780ae05a190d Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 17:38:18 +0900 Subject: [PATCH 73/92] fix(scenario_selector): fix build warning in Humble (#470) * fix(scenario_selector): fix build warning in Humble * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../include/scenario_selector/scenario_selector_node.hpp | 2 +- .../src/scenario_selector_node/scenario_selector_node.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp b/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp index e0f475d576260..3a47a8e196e44 100644 --- a/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp +++ b/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp @@ -22,12 +22,12 @@ #include #include #include +#include #include #include #include #include -#include #include #include diff --git a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp index 75c68da8f8f79..447724a2f60f7 100644 --- a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp +++ b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp @@ -186,7 +186,7 @@ std::string ScenarioSelectorNode::selectScenarioByPosition() } if (current_scenario_ == tier4_planning_msgs::msg::Scenario::PARKING) { - bool is_parking_completed; + bool is_parking_completed{}; this->get_parameter("is_parking_completed", is_parking_completed); if (is_parking_completed && is_in_lane) { this->set_parameter(rclcpp::Parameter("is_parking_completed", false)); From 475aa3c710b8c05e0aeac8bcbe9acf7218d8f336 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 17:38:26 +0900 Subject: [PATCH 74/92] fix(fault_injection): fix build error in Humble (#473) * fix(fault_injection): fix build error in Humble * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/fault_injection_node/fault_injection_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulator/fault_injection/src/fault_injection_node/fault_injection_node.cpp b/simulator/fault_injection/src/fault_injection_node/fault_injection_node.cpp index fcbf69d5032e5..68f6965809785 100644 --- a/simulator/fault_injection/src/fault_injection_node/fault_injection_node.cpp +++ b/simulator/fault_injection/src/fault_injection_node/fault_injection_node.cpp @@ -65,7 +65,7 @@ FaultInjectionNode::FaultInjectionNode(rclcpp::NodeOptions node_options) void FaultInjectionNode::onSimulationEvents(const SimulationEvents::ConstSharedPtr msg) { RCLCPP_DEBUG( - this->get_logger(), "Received data: %s", rosidl_generator_traits::to_yaml(*msg).c_str()); + this->get_logger(), "Received data: %s", tier4_simulation_msgs::msg::to_yaml(*msg).c_str()); for (const auto & event : msg->fault_injection_events) { if (diagnostic_storage_.isEventRegistered(event.name)) { diagnostic_storage_.updateLevel(event.name, event.level); From 593e147670462dc684e4561d820e353e09026491 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 17:38:32 +0900 Subject: [PATCH 75/92] fix(simple_planning_simulator): fix build error in Humble (#474) --- simulator/simple_planning_simulator/package.xml | 2 ++ .../test/test_simple_planning_simulator.cpp | 4 +++- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 619694b3de0dc..ba2d4019940ec 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -21,6 +21,8 @@ nav_msgs vehicle_info_util tier4_autoware_utils + rclcpp_components + tf2_geometry_msgs motion_common diff --git a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp index ad872909740b3..cad1b295209a8 100644 --- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp +++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp @@ -16,6 +16,8 @@ #include "simple_planning_simulator/simple_planning_simulator_core.hpp" #include "tf2/utils.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + #include using autoware_auto_control_msgs::msg::AckermannControlCommand; @@ -272,5 +274,5 @@ const std::string VEHICLE_MODEL_LIST[] = { "DELAY_STEER_ACC_GEARED", }; -INSTANTIATE_TEST_CASE_P( +INSTANTIATE_TEST_SUITE_P( TestForEachVehicleModel, TestSimplePlanningSimulator, ::testing::ValuesIn(VEHICLE_MODEL_LIST)); From 3f32d675cb3bd6cba34ea54ff2a4c99fe486dcde Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 17:38:38 +0900 Subject: [PATCH 76/92] fix(dummy_diag_publisher): modify build error in rolling (#760) (#475) Signed-off-by: wep21 Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> --- system/dummy_diag_publisher/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/system/dummy_diag_publisher/package.xml b/system/dummy_diag_publisher/package.xml index d6cd9847a84a9..9f9646feb8f9f 100644 --- a/system/dummy_diag_publisher/package.xml +++ b/system/dummy_diag_publisher/package.xml @@ -11,6 +11,7 @@ diagnostic_updater rclcpp + rclcpp_components tier4_autoware_utils rqt_reconfigure From 79d24bfbaad4e949948621620ace68dde03c6dbb Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 15 May 2023 17:59:48 +0900 Subject: [PATCH 77/92] fix(system_monitor): fix build error in Humble (#476) --- system/system_monitor/CMakeLists.txt | 2 +- system/system_monitor/launch/system_monitor.launch.py | 2 +- .../system_monitor/src/gpu_monitor/nvml_gpu_monitor.cpp | 8 ++++---- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/system/system_monitor/CMakeLists.txt b/system/system_monitor/CMakeLists.txt index 1ff6f95b28ccc..e7b08ad33f75b 100644 --- a/system/system_monitor/CMakeLists.txt +++ b/system/system_monitor/CMakeLists.txt @@ -8,7 +8,7 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_EXTENSIONS OFF) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) + add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wno-stringop-truncation) endif() find_package(ament_cmake_auto REQUIRED) diff --git a/system/system_monitor/launch/system_monitor.launch.py b/system/system_monitor/launch/system_monitor.launch.py index 463fdb6bab0a8..ad4aa53beca30 100644 --- a/system/system_monitor/launch/system_monitor.launch.py +++ b/system/system_monitor/launch/system_monitor.launch.py @@ -119,7 +119,7 @@ def launch_setup(context, *args, **kwargs): def generate_launch_description(): system_monitor_path = os.path.join( - get_package_share_directory("tier4_system_launch"), "config", "system_monitor" + get_package_share_directory("system_monitor"), "config", "system_monitor" ) return launch.LaunchDescription( [ diff --git a/system/system_monitor/src/gpu_monitor/nvml_gpu_monitor.cpp b/system/system_monitor/src/gpu_monitor/nvml_gpu_monitor.cpp index 19026717bc77b..4cf035dffd556 100644 --- a/system/system_monitor/src/gpu_monitor/nvml_gpu_monitor.cpp +++ b/system/system_monitor/src/gpu_monitor/nvml_gpu_monitor.cpp @@ -183,10 +183,10 @@ void GPUMonitor::addProcessUsage( uint32_t info_count = MAX_ARRAY_SIZE; std::unique_ptr infos; infos = std::make_unique(MAX_ARRAY_SIZE); - ret = nvmlDeviceGetComputeRunningProcesses_v2(device, &info_count, infos.get()); + ret = nvmlDeviceGetComputeRunningProcesses(device, &info_count, infos.get()); if (ret != NVML_SUCCESS) { RCLCPP_WARN( - this->get_logger(), "Failed to nvmlDeviceGetComputeRunningProcesses_v2 NVML: %s", + this->get_logger(), "Failed to nvmlDeviceGetComputeRunningProcesses NVML: %s", nvmlErrorString(ret)); return; } @@ -197,10 +197,10 @@ void GPUMonitor::addProcessUsage( // Get Graphics Process ID info_count = MAX_ARRAY_SIZE; infos = std::make_unique(MAX_ARRAY_SIZE); - ret = nvmlDeviceGetGraphicsRunningProcesses_v2(device, &info_count, infos.get()); + ret = nvmlDeviceGetGraphicsRunningProcesses(device, &info_count, infos.get()); if (ret != NVML_SUCCESS) { RCLCPP_WARN( - this->get_logger(), "Failed to nvmlDeviceGetGraphicsRunningProcesses_v2 NVML: %s", + this->get_logger(), "Failed to nvmlDeviceGetGraphicsRunningProcesses NVML: %s", nvmlErrorString(ret)); return; } From 79db463aeea3dbecf5ec3335babb4f6d6c06d5e5 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Thu, 18 May 2023 15:58:27 +0900 Subject: [PATCH 78/92] fix(probabilistic_occupancy_grid_map): fix build error in Humble (#495) * fix(probabilistic_occupancy_grid_map): fix build error in Humble * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- sensing/probabilistic_occupancy_grid_map/package.xml | 1 + .../laserscan_based_occupancy_grid_map_node.cpp | 2 +- .../pointcloud_based_occupancy_grid_map/occupancy_grid_map.cpp | 2 +- .../pointcloud_based_occupancy_grid_map_node.cpp | 2 +- 4 files changed, 4 insertions(+), 3 deletions(-) diff --git a/sensing/probabilistic_occupancy_grid_map/package.xml b/sensing/probabilistic_occupancy_grid_map/package.xml index 7c4d01e27df6c..873639a5e873c 100644 --- a/sensing/probabilistic_occupancy_grid_map/package.xml +++ b/sensing/probabilistic_occupancy_grid_map/package.xml @@ -19,6 +19,7 @@ rclcpp_components sensor_msgs tf2 + tf2_eigen tf2_geometry_msgs tf2_ros tf2_sensor_msgs diff --git a/sensing/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp b/sensing/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp index 34d7ae257bbf7..b41ae16870136 100644 --- a/sensing/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp +++ b/sensing/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp @@ -17,13 +17,13 @@ #include "cost_value.hpp" #include +#include #include #include #include #include -#include #include #include diff --git a/sensing/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map.cpp b/sensing/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map.cpp index b779334d9893d..ed3732fa2b38e 100644 --- a/sensing/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map.cpp +++ b/sensing/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map.cpp @@ -54,11 +54,11 @@ #include "cost_value.hpp" #include +#include #include #include -#include #include #include diff --git a/sensing/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/sensing/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index ce92f15d4649d..e286a0ca52236 100644 --- a/sensing/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/sensing/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -17,13 +17,13 @@ #include "cost_value.hpp" #include +#include #include #include #include #include -#include #include #include From b7c45b6d1effef55c8d5ec1b8edc758b8db4cfbd Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Wed, 24 May 2023 12:48:56 +0900 Subject: [PATCH 79/92] feat: update trajectory visualizer (backport #737 #745) (#515) * feat: update trajectory visualizer (#737) * feat: update callback group of trajectory visualizer Signed-off-by: tomoya.kimura * add velocity limit to trajectory visualizer Signed-off-by: tomoya.kimura * ci(pre-commit): autofix * rename behavior _path to behavior_velocity_path Signed-off-by: tomoya.kimura * rename lane_change_path to behavior_path_path Signed-off-by: tomoya.kimura * misc Signed-off-by: tomoya.kimura * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix: trajectory visualizer (#745) Signed-off-by: tomoya.kimura --------- Signed-off-by: tomoya.kimura Co-authored-by: Tomoya Kimura Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../scripts/trajectory_visualizer.py | 139 ++++++++++++------ 1 file changed, 95 insertions(+), 44 deletions(-) diff --git a/planning/motion_velocity_smoother/scripts/trajectory_visualizer.py b/planning/motion_velocity_smoother/scripts/trajectory_visualizer.py index 711c35ae0c2d1..dfd9a9dec757a 100755 --- a/planning/motion_velocity_smoother/scripts/trajectory_visualizer.py +++ b/planning/motion_velocity_smoother/scripts/trajectory_visualizer.py @@ -31,6 +31,7 @@ import tf2_ros from tf2_ros.buffer import Buffer from tf2_ros.transform_listener import TransformListener +from tier4_planning_msgs.msg import VelocityLimit parser = argparse.ArgumentParser() parser.add_argument("-l", "--length", help="max arclength in plot") @@ -42,6 +43,8 @@ args = parser.parse_args() +PLOT_MIN_ARCLENGTH = -5 + if args.length is None: PLOT_MAX_ARCLENGTH = 200 else: @@ -83,8 +86,8 @@ def __init__(self): self.update_traj_raw = False self.update_traj_resample = False self.update_traj_final = False - self.update_lanechange_path = False - self.update_behavior_path = False + self.update_behavior_path_planner_path = False + self.update_behavior_velocity_planner_path = False self.update_traj_ob_avoid = False self.update_traj_ob_stop = False @@ -95,6 +98,7 @@ def __init__(self): self.self_pose_received = False self.localization_vx = 0.0 self.vehicle_vx = 0.0 + self.velocity_limit = None self.trajectory_external_velocity_limited = Trajectory() self.trajectory_lateral_acc_filtered = Trajectory() @@ -102,8 +106,8 @@ def __init__(self): self.trajectory_time_resampled = Trajectory() self.trajectory_final = Trajectory() - self.lane_change_path = PathWithLaneId() - self.behavior_path = Path() + self.behavior_path_planner_path = PathWithLaneId() + self.behavior_velocity_planner_path = Path() self.obstacle_avoid_traj = Trajectory() self.obstacle_stop_traj = Trajectory() @@ -115,6 +119,10 @@ def __init__(self): VelocityReport, "/vehicle/status/velocity_status", self.CallbackVehicleTwist, 1 ) + self.sub_external_velocity_limit = self.create_subscription( + VelocityLimit, "/planning/scenario_planning/max_velocity", self.CallbackVelocityLimit, 1 + ) + # BUFFER_SIZE = 65536*100 optimizer_debug = "/planning/scenario_planning/motion_velocity_smoother/debug/" self.sub1 = message_filters.Subscriber( @@ -144,12 +152,12 @@ def __init__(self): self.sub9 = message_filters.Subscriber(self, Trajectory, lane_driving + "/trajectory") self.ts1 = message_filters.ApproximateTimeSynchronizer( - [self.sub1, self.sub2, self.sub3, self.sub4, self.sub5], 30, 0.5 + [self.sub1, self.sub2, self.sub3, self.sub4], 30, 0.5 ) self.ts1.registerCallback(self.CallbackMotionVelOptTraj) self.ts2 = message_filters.ApproximateTimeSynchronizer( - [self.sub6, self.sub7, self.sub8, self.sub9], 30, 1, 0 + [self.sub5, self.sub6, self.sub7, self.sub8, self.sub9], 30, 1, 0 ) self.ts2.registerCallback(self.CallBackLaneDrivingTraj) @@ -178,13 +186,15 @@ def CallbackLocalizationTwist(self, cmd): def CallbackVehicleTwist(self, cmd): self.vehicle_vx = cmd.longitudinal_velocity - def CallbackMotionVelOptTraj(self, cmd1, cmd2, cmd3, cmd4, cmd5): + def CallbackVelocityLimit(self, cmd): + self.velocity_limit = cmd.max_velocity + + def CallbackMotionVelOptTraj(self, cmd1, cmd2, cmd3, cmd4): print("CallbackMotionVelOptTraj called") self.CallBackTrajExVelLim(cmd1) self.CallBackTrajLatAccFiltered(cmd2) self.CallBackTrajRaw(cmd3) self.CallBackTrajTimeResampled(cmd4) - self.CallBackTrajFinal(cmd5) def CallBackTrajExVelLim(self, cmd): self.trajectory_external_velocity_limited = cmd @@ -206,20 +216,21 @@ def CallBackTrajFinal(self, cmd): self.trajectory_final = cmd self.update_traj_final = True - def CallBackLaneDrivingTraj(self, cmd6, cmd7, cmd8, cmd9): + def CallBackLaneDrivingTraj(self, cmd5, cmd6, cmd7, cmd8, cmd9): print("CallBackLaneDrivingTraj called") - self.CallBackLaneChangePath(cmd6) - self.CallBackBehaviorPath(cmd7) + self.CallBackTrajFinal(cmd5) + self.CallBackLBehaviorPathPlannerPath(cmd6) + self.CallBackBehaviorVelocityPlannerPath(cmd7) self.CallbackObstacleAvoidTraj(cmd8) self.CallbackObstacleStopTraj(cmd9) - def CallBackLaneChangePath(self, cmd): - self.lane_change_path = cmd - self.update_lanechange_path = True + def CallBackLBehaviorPathPlannerPath(self, cmd): + self.behavior_path_planner_path = cmd + self.update_behavior_path_planner_path = True - def CallBackBehaviorPath(self, cmd): - self.behavior_path = cmd - self.update_behavior_path = True + def CallBackBehaviorVelocityPlannerPath(self, cmd): + self.behavior_velocity_planner_path = cmd + self.update_behavior_velocity_planner_path = True def CallbackObstacleAvoidTraj(self, cmd): self.obstacle_avoid_traj = cmd @@ -231,8 +242,10 @@ def CallbackObstacleStopTraj(self, cmd): def setPlotTrajectoryVelocity(self): self.ax1 = plt.subplot(1, 1, 1) # row, col, index(= 0: - x_closest = x[closest] - self.im10.set_data(x_closest, self.localization_vx) - self.im11.set_data(x_closest, self.vehicle_vx) + self.im10.set_data(0, self.localization_vx) + self.im11.set_data(0, self.vehicle_vx) + + if self.velocity_limit is not None: + x = [PLOT_MIN_ARCLENGTH, PLOT_MAX_ARCLENGTH] + y = [self.velocity_limit, self.velocity_limit] + self.im12.set_data(x, y) # change y-range self.ax1.set_ylim([self.min_vel - 1.0, self.max_vel + 1.0]) @@ -375,16 +395,27 @@ def plotTrajectoryVelocity(self, data): self.im9, self.im10, self.im11, + self.im12, ) def CalcArcLength(self, traj): + if len(traj.points) == 0: + return + s_arr = [] ds = 0.0 s_sum = 0.0 - if len(traj.points) > 0: - s_arr.append(s_sum) + closest_id = self.calcClosestTrajectory(traj) + for i in range(1, closest_id): + p0 = traj.points[i - 1] + p1 = traj.points[i] + dx = p1.pose.position.x - p0.pose.position.x + dy = p1.pose.position.y - p0.pose.position.y + ds = np.sqrt(dx**2 + dy**2) + s_sum -= ds + s_arr.append(s_sum) for i in range(1, len(traj.points)): p0 = traj.points[i - 1] p1 = traj.points[i] @@ -396,13 +427,23 @@ def CalcArcLength(self, traj): return s_arr def CalcArcLengthPathWLid(self, traj): + if len(traj.points) == 0: + return + s_arr = [] ds = 0.0 s_sum = 0.0 - if len(traj.points) > 0: - s_arr.append(s_sum) + closest_id = self.calcClosestPathWLid(traj) + for i in range(1, closest_id): + p0 = traj.points[i - 1].point + p1 = traj.points[i].point + dx = p1.pose.position.x - p0.pose.position.x + dy = p1.pose.position.y - p0.pose.position.y + ds = np.sqrt(dx**2 + dy**2) + s_sum -= ds + s_arr.append(s_sum) for i in range(1, len(traj.points)): p0 = traj.points[i - 1].point p1 = traj.points[i].point @@ -414,13 +455,23 @@ def CalcArcLengthPathWLid(self, traj): return s_arr def CalcArcLengthPath(self, traj): + if len(traj.points) == 0: + return + s_arr = [] ds = 0.0 s_sum = 0.0 - if len(traj.points) > 0: - s_arr.append(s_sum) + closest_id = self.calcClosestPath(traj) + for i in range(1, closest_id): + p0 = traj.points[i - 1] + p1 = traj.points[i] + dx = p1.pose.position.x - p0.pose.position.x + dy = p1.pose.position.y - p0.pose.position.y + ds = np.sqrt(dx**2 + dy**2) + s_sum -= ds + s_arr.append(s_sum) for i in range(1, len(traj.points)): p0 = traj.points[i - 1] p1 = traj.points[i] @@ -506,17 +557,17 @@ def setPlotTrajectory(self): (self.im2,) = self.ax1.plot([], [], label="4: final velocity", marker="") self.ax1.set_title("trajectory's velocity") self.ax1.legend() - self.ax1.set_xlim([0, PLOT_MAX_ARCLENGTH]) + self.ax1.set_xlim([PLOT_MIN_ARCLENGTH, PLOT_MAX_ARCLENGTH]) self.ax1.set_ylabel("vel [m/s]") self.ax2 = plt.subplot(3, 1, 2) - self.ax2.set_xlim([0, PLOT_MAX_ARCLENGTH]) + self.ax2.set_xlim([PLOT_MIN_ARCLENGTH, PLOT_MAX_ARCLENGTH]) self.ax2.set_ylim([-1, 1]) self.ax2.set_ylabel("acc [m/ss]") (self.im3,) = self.ax2.plot([], [], label="final accel") self.ax3 = plt.subplot(3, 1, 3) - self.ax3.set_xlim([0, PLOT_MAX_ARCLENGTH]) + self.ax3.set_xlim([PLOT_MIN_ARCLENGTH, PLOT_MAX_ARCLENGTH]) self.ax3.set_ylim([-2, 2]) self.ax3.set_xlabel("arclength [m]") self.ax3.set_ylabel("jerk [m/sss]") From f70128b958f7e3bb7b0d3a17b14c07c089e6f747 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Wed, 24 May 2023 15:06:53 +0900 Subject: [PATCH 80/92] fix(transformPointCloud): use tier4_autoware_utils instead of pcl (#497) * fix(transformPointCloud): use tier4_autoware_utils instead of pcl * Update ndt_scan_matcher_core.cpp * comment out --- .../src/ndt_scan_matcher_core.cpp | 33 +++++++++++++++++-- .../behavior_velocity_planner/src/node.cpp | 29 +++++++++++++++- .../surround_obstacle_checker/src/node.cpp | 29 +++++++++++++++- 3 files changed, 86 insertions(+), 5 deletions(-) diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index caa415f8ff196..f30270356688b 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -31,6 +31,33 @@ #include #include +namespace tier4_autoware_utils +{ +template +void transformPointCloud( + const pcl::PointCloud & cloud_in, pcl::PointCloud & cloud_out, + const Eigen::Matrix & transform) +{ + if (cloud_in.empty() || cloud_in.width == 0) { + // RCLCPP_WARN(rclcpp::get_logger("transformPointCloud"), "input point cloud is empty!"); + } else { + pcl::transformPointCloud(cloud_in, cloud_out, transform); + } +} + +template +void transformPointCloud( + const pcl::PointCloud & cloud_in, pcl::PointCloud & cloud_out, + const Eigen::Affine3f & transform) +{ + if (cloud_in.empty() || cloud_in.width == 0) { + // RCLCPP_WARN(rclcpp::get_logger("transformPointCloud"), "input point cloud is empty!"); + } else { + pcl::transformPointCloud(cloud_in, cloud_out, transform); + } +} +} // namespace tier4_autoware_utils + tier4_debug_msgs::msg::Float32Stamped makeFloat32Stamped( const builtin_interfaces::msg::Time & stamp, const float data) { @@ -442,7 +469,7 @@ void NDTScanMatcher::callbackSensorPoints( const Eigen::Matrix4f base_to_sensor_matrix = base_to_sensor_affine.matrix().cast(); pcl::shared_ptr> sensor_points_baselinkTF_ptr( new pcl::PointCloud); - pcl::transformPointCloud( + tier4_autoware_utils::transformPointCloud( *sensor_points_sensorTF_ptr, *sensor_points_baselinkTF_ptr, base_to_sensor_matrix); ndt_ptr_->setInputSource(sensor_points_baselinkTF_ptr); @@ -609,7 +636,7 @@ void NDTScanMatcher::callbackSensorPoints( publishTF(ndt_base_frame_, result_pose_stamped_msg); auto sensor_points_mapTF_ptr = std::make_shared>(); - pcl::transformPointCloud( + tier4_autoware_utils::transformPointCloud( *sensor_points_baselinkTF_ptr, *sensor_points_mapTF_ptr, result_pose_matrix); sensor_msgs::msg::PointCloud2 sensor_points_mapTF_msg; pcl::toROSMsg(*sensor_points_mapTF_ptr, sensor_points_mapTF_msg); @@ -719,7 +746,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::alignUsingMonteCar auto sensor_points_mapTF_ptr = std::make_shared>(); const auto sensor_points_baselinkTF_ptr = ndt_ptr->getInputSource(); - pcl::transformPointCloud( + tier4_autoware_utils::transformPointCloud( *sensor_points_baselinkTF_ptr, *sensor_points_mapTF_ptr, result_pose_matrix); sensor_msgs::msg::PointCloud2 sensor_points_mapTF_msg; pcl::toROSMsg(*sensor_points_mapTF_ptr, sensor_points_mapTF_msg); diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 052dad17eeef6..ad30db2917ecd 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -39,6 +39,33 @@ #include #include +namespace tier4_autoware_utils +{ +template +void transformPointCloud( + const pcl::PointCloud & cloud_in, pcl::PointCloud & cloud_out, + const Eigen::Matrix & transform) +{ + if (cloud_in.empty() || cloud_in.width == 0) { + // RCLCPP_WARN(rclcpp::get_logger("transformPointCloud"), "input point cloud is empty!"); + } else { + pcl::transformPointCloud(cloud_in, cloud_out, transform); + } +} + +template +void transformPointCloud( + const pcl::PointCloud & cloud_in, pcl::PointCloud & cloud_out, + const Eigen::Affine3f & transform) +{ + if (cloud_in.empty() || cloud_in.width == 0) { + // RCLCPP_WARN(rclcpp::get_logger("transformPointCloud"), "input point cloud is empty!"); + } else { + pcl::transformPointCloud(cloud_in, cloud_out, transform); + } +} +} // namespace tier4_autoware_utils + namespace { rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) @@ -258,7 +285,7 @@ void BehaviorVelocityPlannerNode::onNoGroundPointCloud( Eigen::Affine3f affine = tf2::transformToEigen(transform.transform).cast(); pcl::PointCloud::Ptr pc_transformed(new pcl::PointCloud); - pcl::transformPointCloud(pc, *pc_transformed, affine); + tier4_autoware_utils::transformPointCloud(pc, *pc_transformed, affine); { std::lock_guard lock(mutex_); diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/surround_obstacle_checker/src/node.cpp index 4e2f0d152f735..80d2bf0230830 100644 --- a/planning/surround_obstacle_checker/src/node.cpp +++ b/planning/surround_obstacle_checker/src/node.cpp @@ -33,6 +33,33 @@ #include #include +namespace tier4_autoware_utils +{ +template +void transformPointCloud( + const pcl::PointCloud & cloud_in, pcl::PointCloud & cloud_out, + const Eigen::Matrix & transform) +{ + if (cloud_in.empty() || cloud_in.width == 0) { + // RCLCPP_WARN(rclcpp::get_logger("transformPointCloud"), "input point cloud is empty!"); + } else { + pcl::transformPointCloud(cloud_in, cloud_out, transform); + } +} + +template +void transformPointCloud( + const pcl::PointCloud & cloud_in, pcl::PointCloud & cloud_out, + const Eigen::Affine3f & transform) +{ + if (cloud_in.empty() || cloud_in.width == 0) { + // RCLCPP_WARN(rclcpp::get_logger("transformPointCloud"), "input point cloud is empty!"); + } else { + pcl::transformPointCloud(cloud_in, cloud_out, transform); + } +} +} // namespace tier4_autoware_utils + SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptions & node_options) : Node("surround_obstacle_checker_node", node_options), tf_buffer_(this->get_clock()), @@ -269,7 +296,7 @@ void SurroundObstacleCheckerNode::getNearestObstacleByPointCloud( Eigen::Affine3f isometry = tf2::transformToEigen(transform_stamped.transform).cast(); pcl::PointCloud pcl; pcl::fromROSMsg(*pointcloud_ptr_, pcl); - pcl::transformPointCloud(pcl, pcl, isometry); + tier4_autoware_utils::transformPointCloud(pcl, pcl, isometry); for (const auto & p : pcl) { // create boost point Point2d boost_p(p.x, p.y); From afe5edb88d1fc2c3d49547ab01c78ac8d6f4b3b7 Mon Sep 17 00:00:00 2001 From: v-nojiri7841-esol <109567032+v-nojiri7841-esol@users.noreply.github.com> Date: Wed, 24 May 2023 15:51:27 +0900 Subject: [PATCH 81/92] fix(dummy_perception_publisher): use rosidl_get_typesupport_target instead of rosidl_target_interfaces (#516) Signed-off-by: v-nojiri7841-esol --- simulator/dummy_perception_publisher/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simulator/dummy_perception_publisher/CMakeLists.txt b/simulator/dummy_perception_publisher/CMakeLists.txt index bd237fecece1d..1c8a487984a7d 100644 --- a/simulator/dummy_perception_publisher/CMakeLists.txt +++ b/simulator/dummy_perception_publisher/CMakeLists.txt @@ -65,8 +65,8 @@ target_include_directories(dummy_perception_publisher_node $) # For using message definitions from the same package -rosidl_target_interfaces(dummy_perception_publisher_node - ${PROJECT_NAME} "rosidl_typesupport_cpp") +rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") +target_link_libraries(dummy_perception_publisher_node "${cpp_typesupport_target}") # PCL dependencies – `ament_target_dependencies` doesn't respect the # components/modules selected above and only links in `common` ,so we need From ee0f0d25c878996f32f617b83d81c5a8c446cc51 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Thu, 25 May 2023 11:04:54 +0000 Subject: [PATCH 82/92] fix(autonomous_emergency_braking): fix imu tranform to base_link (#3812) (backport #522) (#524) fix(autonomous_emergency_braking): fix imu tranform to base_link (#3812) (#522) (cherry picked from commit 156d52ffcd211d34f8ad87e6adeca0d89eefe917) Co-authored-by: Hiroki OTA --- .../autonomous_emergency_braking/node.hpp | 4 +++- .../autonomous_emergency_braking/src/node.cpp | 23 ++++++++++++++++--- 2 files changed, 23 insertions(+), 4 deletions(-) diff --git a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp index 81d64e6e354e3..5865530ec1e3b 100644 --- a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp +++ b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -61,6 +62,7 @@ using vehicle_info_util::VehicleInfo; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; using Path = std::vector; +using Vector3 = geometry_msgs::msg::Vector3; struct ObjectData { @@ -156,7 +158,7 @@ class AEB : public rclcpp::Node PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr}; VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr}; - Imu::ConstSharedPtr imu_ptr_{nullptr}; + Vector3::SharedPtr angular_velocity_ptr_{nullptr}; Trajectory::ConstSharedPtr predicted_traj_ptr_{nullptr}; AutowareState::ConstSharedPtr autoware_state_{nullptr}; diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index a66935d3f7915..39419eeb6ae53 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -146,7 +146,24 @@ void AEB::onVelocity(const VelocityReport::ConstSharedPtr input_msg) current_velocity_ptr_ = input_msg; } -void AEB::onImu(const Imu::ConstSharedPtr input_msg) { imu_ptr_ = input_msg; } +void AEB::onImu(const Imu::ConstSharedPtr input_msg) +{ + // transform imu + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_.lookupTransform( + "base_link", input_msg->header.frame_id, input_msg->header.stamp, + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + get_logger(), + "[AEB] Failed to look up transform from base_link to" << input_msg->header.frame_id); + return; + } + + angular_velocity_ptr_ = std::make_shared(); + tf2::doTransform(input_msg->angular_velocity, *angular_velocity_ptr_, transform_stamped); +} void AEB::onPredictedTrajectory( const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg) @@ -216,7 +233,7 @@ bool AEB::isDataReady() return missing("object pointcloud"); } - if (use_imu_path_ && !imu_ptr_) { + if (use_imu_path_ && !angular_velocity_ptr_) { return missing("imu"); } @@ -277,7 +294,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers) if (use_imu_path_) { Path ego_path; std::vector ego_polys; - const double current_w = imu_ptr_->angular_velocity.z; + const double current_w = angular_velocity_ptr_->z; constexpr double color_r = 0.0 / 256.0; constexpr double color_g = 148.0 / 256.0; constexpr double color_b = 205.0 / 256.0; From a9a17b56a38514211d67c62db2c949ff5f66125a Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 26 May 2023 13:44:29 +0900 Subject: [PATCH 83/92] feat(common): use autoware_cmake to avoid Boost warning (#528) feat(common): use autoware_cmake --- .../path_distance_calculator/CMakeLists.txt | 19 ++----------------- common/path_distance_calculator/package.xml | 2 ++ common/tier4_autoware_utils/CMakeLists.txt | 15 ++------------- common/tier4_autoware_utils/package.xml | 1 + .../tier4_vehicle_rviz_plugin/CMakeLists.txt | 16 ++-------------- common/tier4_vehicle_rviz_plugin/package.xml | 1 + 6 files changed, 10 insertions(+), 44 deletions(-) diff --git a/common/path_distance_calculator/CMakeLists.txt b/common/path_distance_calculator/CMakeLists.txt index c70e20601a3f7..b1c71028b6ad1 100644 --- a/common/path_distance_calculator/CMakeLists.txt +++ b/common/path_distance_calculator/CMakeLists.txt @@ -1,18 +1,8 @@ cmake_minimum_required(VERSION 3.5) project(path_distance_calculator) -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() +find_package(autoware_cmake REQUIRED) +autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED src/path_distance_calculator.cpp @@ -23,9 +13,4 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE ${PROJECT_NAME}_node ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/common/path_distance_calculator/package.xml b/common/path_distance_calculator/package.xml index e85c8ccd93850..a28d7d7cd9213 100644 --- a/common/path_distance_calculator/package.xml +++ b/common/path_distance_calculator/package.xml @@ -8,6 +8,8 @@ Apache License 2.0 ament_cmake + autoware_cmake + autoware_auto_planning_msgs rclcpp rclcpp_components diff --git a/common/tier4_autoware_utils/CMakeLists.txt b/common/tier4_autoware_utils/CMakeLists.txt index b81dedd893697..56831cada9209 100644 --- a/common/tier4_autoware_utils/CMakeLists.txt +++ b/common/tier4_autoware_utils/CMakeLists.txt @@ -1,16 +1,8 @@ cmake_minimum_required(VERSION 3.5) project(tier4_autoware_utils) -### Compile options -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) -endif() - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() +find_package(autoware_cmake REQUIRED) +autoware_package() find_package(Boost REQUIRED) @@ -20,10 +12,7 @@ ament_auto_add_library(tier4_autoware_utils SHARED src/geometry/boost_polygon_utils.cpp ) -# Test if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) diff --git a/common/tier4_autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml index 85a476331d529..36aa504c2033d 100644 --- a/common/tier4_autoware_utils/package.xml +++ b/common/tier4_autoware_utils/package.xml @@ -8,6 +8,7 @@ Apache License 2.0 ament_cmake_auto + autoware_cmake autoware_auto_planning_msgs builtin_interfaces diff --git a/common/tier4_vehicle_rviz_plugin/CMakeLists.txt b/common/tier4_vehicle_rviz_plugin/CMakeLists.txt index 6996b2adf4b42..3906210a4625d 100644 --- a/common/tier4_vehicle_rviz_plugin/CMakeLists.txt +++ b/common/tier4_vehicle_rviz_plugin/CMakeLists.txt @@ -1,15 +1,8 @@ cmake_minimum_required(VERSION 3.5) project(tier4_vehicle_rviz_plugin) -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() +find_package(autoware_cmake REQUIRED) +autoware_package() find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) set(QT_LIBRARIES Qt5::Widgets) @@ -42,11 +35,6 @@ target_link_libraries(tier4_vehicle_rviz_plugin # Export the plugin to be imported by rviz2 pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - ament_auto_package( INSTALL_TO_SHARE icons diff --git a/common/tier4_vehicle_rviz_plugin/package.xml b/common/tier4_vehicle_rviz_plugin/package.xml index a63351ea65538..cd23786ae0d7b 100644 --- a/common/tier4_vehicle_rviz_plugin/package.xml +++ b/common/tier4_vehicle_rviz_plugin/package.xml @@ -6,6 +6,7 @@ Yukihiro Saito Apache License 2.0 ament_cmake_auto + autoware_cmake autoware_auto_vehicle_msgs libqt5-core From 9d8a1bc8a9fe4eed1d08fbf8c28445d7de1d2226 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 26 May 2023 14:26:18 +0900 Subject: [PATCH 84/92] fix: remove unused check of rviz plugin version (#1474) (#529) Signed-off-by: Takagi, Isamu Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> --- common/polar_grid/CMakeLists.txt | 2 +- common/tier4_calibration_rviz_plugin/CMakeLists.txt | 2 +- common/tier4_datetime_rviz_plugin/CMakeLists.txt | 2 +- common/tier4_localization_rviz_plugin/CMakeLists.txt | 2 +- common/tier4_perception_rviz_plugin/CMakeLists.txt | 2 +- common/tier4_planning_rviz_plugin/CMakeLists.txt | 2 +- common/tier4_simulated_clock_rviz_plugin/CMakeLists.txt | 2 +- common/tier4_state_rviz_plugin/CMakeLists.txt | 2 +- common/tier4_traffic_light_rviz_plugin/CMakeLists.txt | 2 +- common/tier4_vehicle_rviz_plugin/CMakeLists.txt | 2 +- localization/initial_pose_button_panel/CMakeLists.txt | 2 +- 11 files changed, 11 insertions(+), 11 deletions(-) diff --git a/common/polar_grid/CMakeLists.txt b/common/polar_grid/CMakeLists.txt index 8d9b69fe0e8c1..14e94fe7da8d8 100644 --- a/common/polar_grid/CMakeLists.txt +++ b/common/polar_grid/CMakeLists.txt @@ -13,7 +13,7 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) +find_package(Qt5 REQUIRED Core Widgets) set(QT_LIBRARIES Qt5::Widgets) set(CMAKE_AUTOMOC ON) set(CMAKE_INCLUDE_CURRENT_DIR ON) diff --git a/common/tier4_calibration_rviz_plugin/CMakeLists.txt b/common/tier4_calibration_rviz_plugin/CMakeLists.txt index 5354c44a434c0..65d033f575c69 100644 --- a/common/tier4_calibration_rviz_plugin/CMakeLists.txt +++ b/common/tier4_calibration_rviz_plugin/CMakeLists.txt @@ -13,7 +13,7 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets) +find_package(Qt5 REQUIRED Core Widgets) set(QT_LIBRARIES Qt5::Widgets) set(CMAKE_AUTOMOC ON) set(CMAKE_INCLUDE_CURRENT_DIR ON) diff --git a/common/tier4_datetime_rviz_plugin/CMakeLists.txt b/common/tier4_datetime_rviz_plugin/CMakeLists.txt index b35caa4d5a2cb..dba4bef12773f 100644 --- a/common/tier4_datetime_rviz_plugin/CMakeLists.txt +++ b/common/tier4_datetime_rviz_plugin/CMakeLists.txt @@ -13,7 +13,7 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) +find_package(Qt5 REQUIRED Core Widgets) set(QT_LIBRARIES Qt5::Widgets) set(CMAKE_AUTOMOC ON) set(CMAKE_INCLUDE_CURRENT_DIR ON) diff --git a/common/tier4_localization_rviz_plugin/CMakeLists.txt b/common/tier4_localization_rviz_plugin/CMakeLists.txt index eb3b1605c7ca6..5303f762cbfa9 100644 --- a/common/tier4_localization_rviz_plugin/CMakeLists.txt +++ b/common/tier4_localization_rviz_plugin/CMakeLists.txt @@ -13,7 +13,7 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) +find_package(Qt5 REQUIRED Core Widgets) set(QT_LIBRARIES Qt5::Widgets) set(CMAKE_AUTOMOC ON) set(CMAKE_INCLUDE_CURRENT_DIR ON) diff --git a/common/tier4_perception_rviz_plugin/CMakeLists.txt b/common/tier4_perception_rviz_plugin/CMakeLists.txt index a686fe78003b4..cfb573927bbcb 100644 --- a/common/tier4_perception_rviz_plugin/CMakeLists.txt +++ b/common/tier4_perception_rviz_plugin/CMakeLists.txt @@ -13,7 +13,7 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) +find_package(Qt5 REQUIRED Core Widgets) set(QT_LIBRARIES Qt5::Widgets) set(CMAKE_AUTOMOC ON) diff --git a/common/tier4_planning_rviz_plugin/CMakeLists.txt b/common/tier4_planning_rviz_plugin/CMakeLists.txt index a9cceee573c2d..4f7126749b0f9 100644 --- a/common/tier4_planning_rviz_plugin/CMakeLists.txt +++ b/common/tier4_planning_rviz_plugin/CMakeLists.txt @@ -13,7 +13,7 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) +find_package(Qt5 REQUIRED Core Widgets) set(QT_LIBRARIES Qt5::Widgets) set(CMAKE_AUTOMOC ON) set(CMAKE_INCLUDE_CURRENT_DIR ON) diff --git a/common/tier4_simulated_clock_rviz_plugin/CMakeLists.txt b/common/tier4_simulated_clock_rviz_plugin/CMakeLists.txt index 6418b77a661a4..11c5596b3c247 100644 --- a/common/tier4_simulated_clock_rviz_plugin/CMakeLists.txt +++ b/common/tier4_simulated_clock_rviz_plugin/CMakeLists.txt @@ -13,7 +13,7 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) +find_package(Qt5 REQUIRED Core Widgets) set(QT_LIBRARIES Qt5::Widgets) set(CMAKE_AUTOMOC ON) set(CMAKE_INCLUDE_CURRENT_DIR ON) diff --git a/common/tier4_state_rviz_plugin/CMakeLists.txt b/common/tier4_state_rviz_plugin/CMakeLists.txt index a2cf6b29e5cd4..d61ed83f1b98d 100644 --- a/common/tier4_state_rviz_plugin/CMakeLists.txt +++ b/common/tier4_state_rviz_plugin/CMakeLists.txt @@ -13,7 +13,7 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) +find_package(Qt5 REQUIRED Core Widgets) set(QT_LIBRARIES Qt5::Widgets) set(CMAKE_AUTOMOC ON) set(CMAKE_INCLUDE_CURRENT_DIR ON) diff --git a/common/tier4_traffic_light_rviz_plugin/CMakeLists.txt b/common/tier4_traffic_light_rviz_plugin/CMakeLists.txt index a9e6d1c3f8085..79bbeae4fbe15 100644 --- a/common/tier4_traffic_light_rviz_plugin/CMakeLists.txt +++ b/common/tier4_traffic_light_rviz_plugin/CMakeLists.txt @@ -13,7 +13,7 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) +find_package(Qt5 REQUIRED Core Widgets) set(QT_LIBRARIES Qt5::Widgets) set(CMAKE_AUTOMOC ON) set(CMAKE_INCLUDE_CURRENT_DIR ON) diff --git a/common/tier4_vehicle_rviz_plugin/CMakeLists.txt b/common/tier4_vehicle_rviz_plugin/CMakeLists.txt index 3906210a4625d..9d6eff5e5c77f 100644 --- a/common/tier4_vehicle_rviz_plugin/CMakeLists.txt +++ b/common/tier4_vehicle_rviz_plugin/CMakeLists.txt @@ -4,7 +4,7 @@ project(tier4_vehicle_rviz_plugin) find_package(autoware_cmake REQUIRED) autoware_package() -find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) +find_package(Qt5 REQUIRED Core Widgets) set(QT_LIBRARIES Qt5::Widgets) set(CMAKE_AUTOMOC ON) diff --git a/localization/initial_pose_button_panel/CMakeLists.txt b/localization/initial_pose_button_panel/CMakeLists.txt index b4194c5daf792..0b968c98ae682 100644 --- a/localization/initial_pose_button_panel/CMakeLists.txt +++ b/localization/initial_pose_button_panel/CMakeLists.txt @@ -11,7 +11,7 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) +find_package(Qt5 REQUIRED Core Widgets) set(QT_LIBRARIES Qt5::Widgets) add_definitions(-DQT_NO_KEYWORDS -g) From 12ee83b18c21e0d371ca1c97e6127d8cea5c443e Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 26 May 2023 14:47:35 +0900 Subject: [PATCH 85/92] feat(control): use autoware cmake to avoid Boost warning (#530) --- control/lane_departure_checker/CMakeLists.txt | 22 ++----------------- control/lane_departure_checker/package.xml | 1 + 2 files changed, 3 insertions(+), 20 deletions(-) diff --git a/control/lane_departure_checker/CMakeLists.txt b/control/lane_departure_checker/CMakeLists.txt index 6cf1dc525d718..66c2c208f9513 100644 --- a/control/lane_departure_checker/CMakeLists.txt +++ b/control/lane_departure_checker/CMakeLists.txt @@ -1,19 +1,8 @@ cmake_minimum_required(VERSION 3.5) project(lane_departure_checker) -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -### Compile options -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) -endif() - +find_package(autoware_cmake REQUIRED) +autoware_package() include_directories( include @@ -21,8 +10,6 @@ include_directories( ${EIGEN3_INCLUDE_DIRS} ) -# Target -## lane_departure_checker_node ament_auto_add_library(lane_departure_checker SHARED src/lane_departure_checker_node/lane_departure_checker.cpp src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -33,11 +20,6 @@ rclcpp_components_register_node(lane_departure_checker EXECUTABLE lane_departure_checker_node ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - ament_auto_package( INSTALL_TO_SHARE launch diff --git a/control/lane_departure_checker/package.xml b/control/lane_departure_checker/package.xml index baa78c263a5eb..c96deec5ee900 100644 --- a/control/lane_departure_checker/package.xml +++ b/control/lane_departure_checker/package.xml @@ -7,6 +7,7 @@ Apache License 2.0 ament_cmake_auto + autoware_cmake autoware_auto_mapping_msgs autoware_auto_planning_msgs From 12efad6208a4705e817e87ee5c89e56c1a24f921 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 26 May 2023 14:56:04 +0900 Subject: [PATCH 86/92] feat(map): use autoware_cmake to avoid Boost warning (#531) --- map/lanelet2_extension/CMakeLists.txt | 17 ++--------------- map/lanelet2_extension/package.xml | 2 +- 2 files changed, 3 insertions(+), 16 deletions(-) diff --git a/map/lanelet2_extension/CMakeLists.txt b/map/lanelet2_extension/CMakeLists.txt index c755d45eee3e0..9ec02cf66324e 100644 --- a/map/lanelet2_extension/CMakeLists.txt +++ b/map/lanelet2_extension/CMakeLists.txt @@ -1,18 +1,8 @@ cmake_minimum_required(VERSION 3.5) project(lanelet2_extension) -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) -endif() - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() +find_package(autoware_cmake REQUIRED) +autoware_package() find_package(PkgConfig) find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h @@ -72,7 +62,6 @@ target_link_libraries(autoware_lanelet2_validation ) if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(message_conversion-test test/src/test_message_conversion.cpp) target_link_libraries(message_conversion-test lanelet2_extension_lib) ament_add_gtest(projector-test test/src/test_projector.cpp) @@ -83,8 +72,6 @@ if(BUILD_TESTING) target_link_libraries(regulatory_elements-test lanelet2_extension_lib) ament_add_gtest(utilities-test test/src/test_utilities.cpp) target_link_libraries(utilities-test lanelet2_extension_lib) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() endif() ament_auto_package() diff --git a/map/lanelet2_extension/package.xml b/map/lanelet2_extension/package.xml index 3f1af3a245186..bc9cdfc5a387d 100644 --- a/map/lanelet2_extension/package.xml +++ b/map/lanelet2_extension/package.xml @@ -8,8 +8,8 @@ Apache License 2.0 - ament_cmake ament_cmake_auto + autoware_cmake autoware_auto_mapping_msgs geographiclib From bebc2f048ee5711509a77038e45346444aa977e2 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 26 May 2023 15:14:11 +0900 Subject: [PATCH 87/92] feat(perception): use autoware_cmake to avoid Boost warning (#533) --- .../elevation_map_loader/CMakeLists.txt | 19 ++--------- perception/elevation_map_loader/package.xml | 3 +- perception/ground_segmentation/CMakeLists.txt | 32 ++----------------- perception/ground_segmentation/package.xml | 1 + .../CMakeLists.txt | 32 ++----------------- .../package.xml | 1 + perception/shape_estimation/CMakeLists.txt | 18 ++--------- perception/shape_estimation/package.xml | 1 + 8 files changed, 12 insertions(+), 95 deletions(-) diff --git a/perception/elevation_map_loader/CMakeLists.txt b/perception/elevation_map_loader/CMakeLists.txt index 4c221e7af0e80..ab2c1dcc20e1d 100644 --- a/perception/elevation_map_loader/CMakeLists.txt +++ b/perception/elevation_map_loader/CMakeLists.txt @@ -1,21 +1,11 @@ cmake_minimum_required(VERSION 3.5) project(elevation_map_loader) -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() +find_package(autoware_cmake REQUIRED) +autoware_package() find_package(PCL REQUIRED COMPONENTS io) -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - ament_auto_add_library(elevation_map_loader_node SHARED src/elevation_map_loader_node.cpp ) @@ -26,11 +16,6 @@ rclcpp_components_register_node(elevation_map_loader_node EXECUTABLE elevation_map_loader ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - ament_auto_package(INSTALL_TO_SHARE launch config diff --git a/perception/elevation_map_loader/package.xml b/perception/elevation_map_loader/package.xml index 9643d9e2b3c92..859f4a0cb4ce1 100644 --- a/perception/elevation_map_loader/package.xml +++ b/perception/elevation_map_loader/package.xml @@ -7,9 +7,8 @@ Taichi Higashide Apache License 2.0 - ament_cmake ament_cmake_auto - + autoware_cmake autoware_auto_mapping_msgs grid_map_cv grid_map_pcl diff --git a/perception/ground_segmentation/CMakeLists.txt b/perception/ground_segmentation/CMakeLists.txt index 1db192cb4bc39..fb7352e4b8f32 100644 --- a/perception/ground_segmentation/CMakeLists.txt +++ b/perception/ground_segmentation/CMakeLists.txt @@ -1,34 +1,15 @@ cmake_minimum_required(VERSION 3.5) project(ground_segmentation) -### Compile options -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) -endif() +find_package(autoware_cmake REQUIRED) +autoware_package() -# Ignore PCL errors in Clang -if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wno-gnu-anonymous-struct -Wno-nested-anon-types) -endif() - -find_package(ament_cmake_auto REQUIRED) find_package(OpenCV REQUIRED) find_package(Eigen3 REQUIRED) find_package(Boost REQUIRED) find_package(PCL REQUIRED) find_package(pcl_conversions REQUIRED) find_package(OpenMP) -ament_auto_find_build_dependencies() - - -########### -## Build ## -########### include_directories( include @@ -75,15 +56,6 @@ rclcpp_components_register_node(ground_segmentation PLUGIN "ground_segmentation::ScanGroundFilterComponent" EXECUTABLE scan_ground_filter_node) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -############# -## Install ## -############# - ament_auto_package(INSTALL_TO_SHARE launch ) diff --git a/perception/ground_segmentation/package.xml b/perception/ground_segmentation/package.xml index 75f5349baba5c..e289c672b96eb 100644 --- a/perception/ground_segmentation/package.xml +++ b/perception/ground_segmentation/package.xml @@ -16,6 +16,7 @@ Apache License 2.0 ament_cmake_auto + autoware_cmake libopencv-dev pcl_conversions diff --git a/perception/occupancy_grid_map_outlier_filter/CMakeLists.txt b/perception/occupancy_grid_map_outlier_filter/CMakeLists.txt index 6a011366092b5..de9712f948a00 100644 --- a/perception/occupancy_grid_map_outlier_filter/CMakeLists.txt +++ b/perception/occupancy_grid_map_outlier_filter/CMakeLists.txt @@ -1,22 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(occupancy_grid_map_outlier_filter) -### Compile options -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) -endif() +find_package(autoware_cmake REQUIRED) +autoware_package() -# Ignore PCL errors in Clang -if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wno-gnu-anonymous-struct -Wno-nested-anon-types) -endif() - -find_package(ament_cmake_auto REQUIRED) find_package(OpenCV REQUIRED) find_package(Eigen3 REQUIRED) find_package(Boost REQUIRED) @@ -25,11 +12,6 @@ find_package(pcl_conversions REQUIRED) find_package(OpenMP) ament_auto_find_build_dependencies() - -########### -## Build ## -########### - include_directories( include SYSTEM @@ -63,14 +45,4 @@ rclcpp_components_register_node(occupancy_grid_map_outlier_filter PLUGIN "occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent" EXECUTABLE occupancy_grid_map_outlier_filter_node) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -############# -## Install ## -############# - ament_auto_package(INSTALL_TO_SHARE) diff --git a/perception/occupancy_grid_map_outlier_filter/package.xml b/perception/occupancy_grid_map_outlier_filter/package.xml index dec4aa9bf5a7e..38bc44fca3313 100644 --- a/perception/occupancy_grid_map_outlier_filter/package.xml +++ b/perception/occupancy_grid_map_outlier_filter/package.xml @@ -16,6 +16,7 @@ Apache License 2.0 ament_cmake_auto + autoware_cmake autoware_auto_vehicle_msgs diagnostic_updater diff --git a/perception/shape_estimation/CMakeLists.txt b/perception/shape_estimation/CMakeLists.txt index 080c5b6871c85..d502b10ea383c 100644 --- a/perception/shape_estimation/CMakeLists.txt +++ b/perception/shape_estimation/CMakeLists.txt @@ -1,17 +1,8 @@ cmake_minimum_required(VERSION 3.5) project(shape_estimation) -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) -endif() - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() +find_package(autoware_cmake REQUIRED) +autoware_package() find_package(PCL REQUIRED COMPONENTS common) find_package(pcl_conversions REQUIRED) @@ -73,11 +64,6 @@ rclcpp_components_register_node(shape_estimation_node EXECUTABLE shape_estimation ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - ament_auto_package(INSTALL_TO_SHARE launch ) diff --git a/perception/shape_estimation/package.xml b/perception/shape_estimation/package.xml index 0621114bc9bb8..6f1c1e639e854 100644 --- a/perception/shape_estimation/package.xml +++ b/perception/shape_estimation/package.xml @@ -9,6 +9,7 @@ Apache License 2.0 ament_cmake + autoware_cmake autoware_auto_perception_msgs builtin_interfaces From cbf5d46058909619746d6e5d16b2e802410e95fa Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 26 May 2023 15:45:15 +0900 Subject: [PATCH 88/92] feat(planning): use autoware_cmake to avoid Boost warning (#534) --- planning/behavior_path_planner/CMakeLists.txt | 14 ++----------- planning/behavior_path_planner/package.xml | 1 + .../behavior_velocity_planner/CMakeLists.txt | 17 ++-------------- .../behavior_velocity_planner/package.xml | 1 + planning/mission_planner/CMakeLists.txt | 19 ++---------------- planning/mission_planner/package.xml | 1 + .../obstacle_avoidance_planner/CMakeLists.txt | 18 ++--------------- .../obstacle_avoidance_planner/package.xml | 1 + planning/obstacle_stop_planner/CMakeLists.txt | 18 ++--------------- planning/obstacle_stop_planner/package.xml | 1 + .../planning_error_monitor/CMakeLists.txt | 15 ++------------ planning/planning_error_monitor/package.xml | 1 + planning/route_handler/CMakeLists.txt | 18 ++--------------- planning/route_handler/package.xml | 1 + planning/scenario_selector/CMakeLists.txt | 20 ++----------------- planning/scenario_selector/package.xml | 1 + .../surround_obstacle_checker/CMakeLists.txt | 19 ++---------------- .../surround_obstacle_checker/package.xml | 2 ++ 18 files changed, 28 insertions(+), 140 deletions(-) diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 0f908d09ad358..2b8f3352a81db 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -1,17 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(behavior_path_planner) -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) -endif() +find_package(autoware_cmake REQUIRED) +autoware_package() -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() find_package(OpenCV REQUIRED) ament_auto_add_library(behavior_path_planner_node SHARED @@ -46,8 +38,6 @@ rclcpp_components_register_node(behavior_path_planner_node ) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_utilities test/input.cpp test/test_utilities.cpp diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index f0e6f12a29719..7299e0d34163d 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -13,6 +13,7 @@ Apache License 2.0 ament_cmake_auto + autoware_cmake autoware_auto_perception_msgs autoware_auto_planning_msgs diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/CMakeLists.txt index 189b16a1e0517..9773cab163cef 100644 --- a/planning/behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/CMakeLists.txt @@ -1,27 +1,16 @@ cmake_minimum_required(VERSION 3.5) project(behavior_velocity_planner) -### Compile options -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) -endif() +find_package(autoware_cmake REQUIRED) +autoware_package() # Find the non-obvious packages manually -find_package(ament_cmake REQUIRED) find_package(Boost REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) find_package(PCL REQUIRED COMPONENTS common) find_package(OpenCV REQUIRED) -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - set(BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES tier4_api_msgs autoware_auto_perception_msgs @@ -312,8 +301,6 @@ rclcpp_components_register_node(behavior_velocity_planner if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() # utils for test ament_auto_add_library(utilization SHARED diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index f6773c51f2b0d..905e8d2aa4ab7 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -10,6 +10,7 @@ ament_cmake eigen3_cmake_module + autoware_cmake autoware_auto_mapping_msgs autoware_auto_perception_msgs diff --git a/planning/mission_planner/CMakeLists.txt b/planning/mission_planner/CMakeLists.txt index b7a9b3ce9c84f..2445c969d05d4 100644 --- a/planning/mission_planner/CMakeLists.txt +++ b/planning/mission_planner/CMakeLists.txt @@ -1,18 +1,8 @@ cmake_minimum_required(VERSION 3.5) project(mission_planner) -### Compile options -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() +find_package(autoware_cmake REQUIRED) +autoware_package() ament_auto_add_library(mission_planner_node SHARED lib/mission_planner_base.cpp @@ -34,11 +24,6 @@ rclcpp_components_register_node(goal_pose_visualizer_node EXECUTABLE goal_pose_visualizer ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - ament_auto_package(INSTALL_TO_SHARE launch ) diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml index 3aced14bbb08a..f42875b759788 100644 --- a/planning/mission_planner/package.xml +++ b/planning/mission_planner/package.xml @@ -9,6 +9,7 @@ Apache License 2.0 ament_cmake_auto + autoware_cmake autoware_auto_planning_msgs geometry_msgs diff --git a/planning/obstacle_avoidance_planner/CMakeLists.txt b/planning/obstacle_avoidance_planner/CMakeLists.txt index bc694f79f46a1..ea1e20881e5aa 100644 --- a/planning/obstacle_avoidance_planner/CMakeLists.txt +++ b/planning/obstacle_avoidance_planner/CMakeLists.txt @@ -1,17 +1,8 @@ cmake_minimum_required(VERSION 3.5) project(obstacle_avoidance_planner) -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) -endif() - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() +find_package(autoware_cmake REQUIRED) +autoware_package() find_package(Eigen3 REQUIRED) find_package(OpenCV REQUIRED) @@ -42,11 +33,6 @@ rclcpp_components_register_node(obstacle_avoidance_planner EXECUTABLE obstacle_avoidance_planner_node ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - ament_auto_package( INSTALL_TO_SHARE launch diff --git a/planning/obstacle_avoidance_planner/package.xml b/planning/obstacle_avoidance_planner/package.xml index c58292c667327..2d4c1f4c405b3 100644 --- a/planning/obstacle_avoidance_planner/package.xml +++ b/planning/obstacle_avoidance_planner/package.xml @@ -9,6 +9,7 @@ Apache License 2.0 ament_cmake_auto + autoware_cmake autoware_auto_perception_msgs autoware_auto_planning_msgs diff --git a/planning/obstacle_stop_planner/CMakeLists.txt b/planning/obstacle_stop_planner/CMakeLists.txt index d31efe651281f..2d5742347a83f 100644 --- a/planning/obstacle_stop_planner/CMakeLists.txt +++ b/planning/obstacle_stop_planner/CMakeLists.txt @@ -1,17 +1,8 @@ cmake_minimum_required(VERSION 3.5) project(obstacle_stop_planner) -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) -endif() - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() +find_package(autoware_cmake REQUIRED) +autoware_package() find_package(Eigen3 REQUIRED) find_package(OpenCV REQUIRED) @@ -39,11 +30,6 @@ rclcpp_components_register_node(obstacle_stop_planner EXECUTABLE obstacle_stop_planner_node ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - ament_auto_package( INSTALL_TO_SHARE config diff --git a/planning/obstacle_stop_planner/package.xml b/planning/obstacle_stop_planner/package.xml index a665fd8fcfe9c..8c66236fd5a4b 100644 --- a/planning/obstacle_stop_planner/package.xml +++ b/planning/obstacle_stop_planner/package.xml @@ -9,6 +9,7 @@ Apache License 2.0 ament_cmake_auto + autoware_cmake autoware_auto_perception_msgs autoware_auto_planning_msgs diff --git a/planning/planning_error_monitor/CMakeLists.txt b/planning/planning_error_monitor/CMakeLists.txt index 863056a52ad98..d6bd52fada7dd 100644 --- a/planning/planning_error_monitor/CMakeLists.txt +++ b/planning/planning_error_monitor/CMakeLists.txt @@ -1,17 +1,8 @@ cmake_minimum_required(VERSION 3.5) project(planning_error_monitor) -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() +find_package(autoware_cmake REQUIRED) +autoware_package() ament_auto_add_library(planning_error_monitor_node SHARED src/planning_error_monitor_node.cpp @@ -31,8 +22,6 @@ rclcpp_components_register_node(invalid_trajectory_publisher_node ) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() ament_add_gtest(test_planning_error_monitor test/src/test_main.cpp test/src/test_planning_error_monitor_functions.cpp diff --git a/planning/planning_error_monitor/package.xml b/planning/planning_error_monitor/package.xml index e35554a392d7e..72b1344a5d882 100644 --- a/planning/planning_error_monitor/package.xml +++ b/planning/planning_error_monitor/package.xml @@ -9,6 +9,7 @@ Apache License 2.0 ament_cmake_auto + autoware_cmake autoware_auto_planning_msgs diagnostic_updater diff --git a/planning/route_handler/CMakeLists.txt b/planning/route_handler/CMakeLists.txt index 9d05f8c9727c0..b7f2b1507129c 100644 --- a/planning/route_handler/CMakeLists.txt +++ b/planning/route_handler/CMakeLists.txt @@ -1,25 +1,11 @@ cmake_minimum_required(VERSION 3.5) project(route_handler) -### Compile options -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) -endif() - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() +find_package(autoware_cmake REQUIRED) +autoware_package() ament_auto_add_library(route_handler SHARED src/route_handler.cpp ) -# Test -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - ament_auto_package() diff --git a/planning/route_handler/package.xml b/planning/route_handler/package.xml index f56b62a3a23ea..5a0ecf6042659 100644 --- a/planning/route_handler/package.xml +++ b/planning/route_handler/package.xml @@ -7,6 +7,7 @@ Fumiya Watanabe Apache License 2.0 ament_cmake_auto + autoware_cmake ament_lint_auto autoware_lint_common diff --git a/planning/scenario_selector/CMakeLists.txt b/planning/scenario_selector/CMakeLists.txt index db178e34319c5..a39a3133b690b 100644 --- a/planning/scenario_selector/CMakeLists.txt +++ b/planning/scenario_selector/CMakeLists.txt @@ -1,19 +1,8 @@ cmake_minimum_required(VERSION 3.5) project(scenario_selector) -### Compile options -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -## Dependencies -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() +find_package(autoware_cmake REQUIRED) +autoware_package() # Target @@ -32,11 +21,6 @@ rclcpp_components_register_node(scenario_selector_node EXECUTABLE scenario_selector ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - ament_auto_package(INSTALL_TO_SHARE launch ) diff --git a/planning/scenario_selector/package.xml b/planning/scenario_selector/package.xml index c2c5a395458d0..47b05bd7b8364 100644 --- a/planning/scenario_selector/package.xml +++ b/planning/scenario_selector/package.xml @@ -8,6 +8,7 @@ Apache License 2.0 ament_cmake_auto + autoware_cmake autoware_auto_mapping_msgs autoware_auto_planning_msgs diff --git a/planning/surround_obstacle_checker/CMakeLists.txt b/planning/surround_obstacle_checker/CMakeLists.txt index e01a39d8e906e..febe29e0f9752 100644 --- a/planning/surround_obstacle_checker/CMakeLists.txt +++ b/planning/surround_obstacle_checker/CMakeLists.txt @@ -1,22 +1,12 @@ cmake_minimum_required(VERSION 3.5) project(surround_obstacle_checker) -### Compile options -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() +find_package(autoware_cmake REQUIRED) +autoware_package() -find_package(ament_cmake REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) find_package(PCL REQUIRED COMPONENTS common) -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() ament_auto_add_library(${PROJECT_NAME} SHARED src/debug_marker.cpp @@ -32,11 +22,6 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE ${PROJECT_NAME}_node ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - ament_auto_package( INSTALL_TO_SHARE config diff --git a/planning/surround_obstacle_checker/package.xml b/planning/surround_obstacle_checker/package.xml index ec743d3aa9d33..125e9adb44a48 100644 --- a/planning/surround_obstacle_checker/package.xml +++ b/planning/surround_obstacle_checker/package.xml @@ -11,6 +11,8 @@ ament_cmake eigen3_cmake_module + autoware_cmake + autoware_auto_perception_msgs autoware_auto_planning_msgs diagnostic_msgs From 1c7762de1e58195d80e1dc91dc2dac656d549510 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 26 May 2023 16:58:02 +0900 Subject: [PATCH 89/92] fix(probablistic_occupancy_grid_map): fix build warning (#535) * fix(probablistic_occupancy_grid_map): fix build warning * ci(pre-commit): autofix * delete unused --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../laserscan_based_occupancy_grid_map_node.cpp | 2 +- .../occupancy_grid_map.cpp | 6 +----- .../pointcloud_based_occupancy_grid_map_node.cpp | 2 +- 3 files changed, 3 insertions(+), 7 deletions(-) diff --git a/sensing/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp b/sensing/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp index b41ae16870136..1fb4976857849 100644 --- a/sensing/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp +++ b/sensing/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp @@ -21,10 +21,10 @@ #include #include +#include #include #include -#include #include #include diff --git a/sensing/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map.cpp b/sensing/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map.cpp index ed3732fa2b38e..9b9946349fe18 100644 --- a/sensing/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map.cpp +++ b/sensing/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map.cpp @@ -58,8 +58,7 @@ #include #include - -#include +#include #include namespace @@ -158,7 +157,6 @@ void OccupancyGridMap::updateWithPointCloud( const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud, const Pose & robot_pose) { - constexpr double ep = 0.001; constexpr double min_angle = tier4_autoware_utils::deg2rad(-180.0); constexpr double max_angle = tier4_autoware_utils::deg2rad(180.0); constexpr double angle_increment = tier4_autoware_utils::deg2rad(0.1); @@ -301,8 +299,6 @@ void OccupancyGridMap::updateWithPointCloud( // Third step: Overwrite occupied cell for (size_t bin_index = 0; bin_index < obstacle_pointcloud_angle_bins.size(); ++bin_index) { auto & obstacle_pointcloud_angle_bin = obstacle_pointcloud_angle_bins.at(bin_index); - auto & raw_pointcloud_angle_bin = raw_pointcloud_angle_bins.at(bin_index); - auto raw_distance_iter = raw_pointcloud_angle_bin.begin(); for (size_t dist_index = 0; dist_index < obstacle_pointcloud_angle_bin.size(); ++dist_index) { const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); setCellValue(source.wx, source.wy, occupancy_cost_value::LETHAL_OBSTACLE); diff --git a/sensing/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/sensing/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index e286a0ca52236..bb74ef96b7bbf 100644 --- a/sensing/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/sensing/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -21,10 +21,10 @@ #include #include +#include #include #include -#include #include #include From 9e47abbb29f1e559167608eed9e26fbcc532d7aa Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 26 May 2023 17:04:33 +0900 Subject: [PATCH 90/92] fix(ndt_scan_matcher): ndt_scan_matcher crash when input cloud is empty. (#3354) (#502) * fix(ndt_scan_matcher): ndt_scan_matcher crash when input cloud is empty. (#3354) * Fixed NDTScanMatcher crash when input cloud is empty. * style(pre-commit): autofix --------- Co-authored-by: Zygfryd Wieszok Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: M(-_-)Sakamoto <83898149+Motsu-san@users.noreply.github.com> * ci(pre-commit): autofix --------- Co-authored-by: Zygfryd Wieszok Co-authored-by: Zygfryd Wieszok Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: M(-_-)Sakamoto <83898149+Motsu-san@users.noreply.github.com> --- localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index f30270356688b..4a772ccd4793f 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -452,6 +452,12 @@ void NDTScanMatcher::callbackMapPoints( void NDTScanMatcher::callbackSensorPoints( sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_sensorTF_msg_ptr) { + if (sensor_points_sensorTF_msg_ptr->data.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); + return; + } + const auto exe_start_time = std::chrono::system_clock::now(); // mutex Map std::lock_guard lock(ndt_map_mtx_); From 7507f0e8d167dbedd3d60c998138ac5d30d25cf0 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Fri, 26 May 2023 17:07:39 +0900 Subject: [PATCH 91/92] feat(sensing): use autoware_cmake to avoid Boost warning (#536) --- .../pointcloud_preprocessor/CMakeLists.txt | 33 ++----------------- sensing/pointcloud_preprocessor/package.xml | 1 + .../CMakeLists.txt | 22 ++----------- .../package.xml | 1 + 4 files changed, 6 insertions(+), 51 deletions(-) diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index b3888ace507e7..20e5c38685db3 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -1,34 +1,14 @@ cmake_minimum_required(VERSION 3.5) project(pointcloud_preprocessor) -### Compile options -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) -endif() +find_package(autoware_cmake REQUIRED) +autoware_package() -# Ignore PCL errors in Clang -if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wno-gnu-anonymous-struct -Wno-nested-anon-types) -endif() - -find_package(ament_cmake_auto REQUIRED) find_package(OpenCV REQUIRED) find_package(Eigen3 REQUIRED) find_package(Boost REQUIRED) find_package(PCL REQUIRED) -find_package(pcl_conversions REQUIRED) find_package(OpenMP) -ament_auto_find_build_dependencies() - - -########### -## Build ## -########### include_directories( include @@ -155,15 +135,6 @@ rclcpp_components_register_node(pointcloud_preprocessor_filter PLUGIN "pointcloud_preprocessor::BlockageDiagComponent" EXECUTABLE blockage_diag_node) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -############# -## Install ## -############# - ament_auto_package(INSTALL_TO_SHARE launch ) diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml index 4b1e3830f5614..9278f0f2157c4 100644 --- a/sensing/pointcloud_preprocessor/package.xml +++ b/sensing/pointcloud_preprocessor/package.xml @@ -16,6 +16,7 @@ Apache License 2.0 ament_cmake_auto + autoware_cmake autoware_auto_vehicle_msgs autoware_point_types diff --git a/sensing/probabilistic_occupancy_grid_map/CMakeLists.txt b/sensing/probabilistic_occupancy_grid_map/CMakeLists.txt index d082019df49c2..0e32166432281 100644 --- a/sensing/probabilistic_occupancy_grid_map/CMakeLists.txt +++ b/sensing/probabilistic_occupancy_grid_map/CMakeLists.txt @@ -1,20 +1,12 @@ cmake_minimum_required(VERSION 3.5) project(probabilistic_occupancy_grid_map) -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() +find_package(autoware_cmake REQUIRED) +autoware_package() -find_package(ament_cmake_auto REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) find_package(PCL REQUIRED) -ament_auto_find_build_dependencies() # PointcloudBasedOccupancyGridMap ament_auto_add_library(pointcloud_based_occupancy_grid_map SHARED @@ -48,16 +40,6 @@ rclcpp_components_register_node(laserscan_based_occupancy_grid_map EXECUTABLE laserscan_based_occupancy_grid_map_node ) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - list(APPEND AMENT_LINT_AUTO_EXCLUDE - # To avoid conflicts between cpplint and uncrustify w.r.t. inclusion guards - ament_cmake_uncrustify - ) - ament_lint_auto_find_test_dependencies() -endif() - ament_auto_package( INSTALL_TO_SHARE launch diff --git a/sensing/probabilistic_occupancy_grid_map/package.xml b/sensing/probabilistic_occupancy_grid_map/package.xml index 873639a5e873c..4180843cb80d8 100644 --- a/sensing/probabilistic_occupancy_grid_map/package.xml +++ b/sensing/probabilistic_occupancy_grid_map/package.xml @@ -8,6 +8,7 @@ Yukihiro Saito Apache License 2.0 ament_cmake_auto + autoware_cmake eigen3_cmake_module laser_geometry From e0e6d0c9870a407dd661850c8b426e7b298affb6 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Mon, 12 Jun 2023 14:47:29 +0900 Subject: [PATCH 92/92] fix(lanelet2_extension): fix build error (#571) --- map/lanelet2_extension/CMakeLists.txt | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/map/lanelet2_extension/CMakeLists.txt b/map/lanelet2_extension/CMakeLists.txt index 9ec02cf66324e..60bdb758c913c 100644 --- a/map/lanelet2_extension/CMakeLists.txt +++ b/map/lanelet2_extension/CMakeLists.txt @@ -47,6 +47,12 @@ target_link_libraries(lanelet2_extension_lib ${GeographicLib_LIBRARIES} ) +get_target_property(lanelet2_core_INCLUDE_DIRECTORIES lanelet2_core::lanelet2_core INTERFACE_INCLUDE_DIRECTORIES) +target_include_directories(lanelet2_extension_lib + SYSTEM PRIVATE + ${lanelet2_core_INCLUDE_DIRECTORIES} +) + ament_auto_add_executable(lanelet2_extension_sample src/sample_code.cpp) add_dependencies(lanelet2_extension_sample lanelet2_extension_lib) target_link_libraries(lanelet2_extension_sample @@ -72,6 +78,10 @@ if(BUILD_TESTING) target_link_libraries(regulatory_elements-test lanelet2_extension_lib) ament_add_gtest(utilities-test test/src/test_utilities.cpp) target_link_libraries(utilities-test lanelet2_extension_lib) + target_include_directories(utilities-test + SYSTEM PRIVATE + ${lanelet2_core_INCLUDE_DIRECTORIES} + ) endif() ament_auto_package()