diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp index 3962159375974..a720de68acbcd 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp @@ -31,13 +31,12 @@ #include #include -#include - #include #include #include #include +#include #include #include #include @@ -93,13 +92,13 @@ class SurroundObstacleCheckerNode : public rclcpp::Node void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); - boost::optional getNearestObstacle() const; + std::optional getNearestObstacle() const; - boost::optional getNearestObstacleByPointCloud() const; + std::optional getNearestObstacleByPointCloud() const; - boost::optional getNearestObstacleByDynamicObject() const; + std::optional getNearestObstacleByDynamicObject() const; - boost::optional getTransform( + std::optional getTransform( const std::string & source, const std::string & target, const rclcpp::Time & stamp, double duration_sec) const; diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/surround_obstacle_checker/src/node.cpp index 0dd021cd8ddad..4c7d0589f843b 100644 --- a/planning/surround_obstacle_checker/src/node.cpp +++ b/planning/surround_obstacle_checker/src/node.cpp @@ -29,6 +29,8 @@ #include #include #include + +#include #ifdef ROS_DISTRO_GALACTIC #include #else @@ -314,7 +316,7 @@ void SurroundObstacleCheckerNode::onTimer() switch (state_) { case State::PASS: { const auto is_obstacle_found = - !nearest_obstacle ? false : nearest_obstacle.get().first < epsilon; + !nearest_obstacle ? false : nearest_obstacle.value().first < epsilon; if (!isStopRequired(is_obstacle_found, is_vehicle_stopped)) { break; @@ -340,7 +342,7 @@ void SurroundObstacleCheckerNode::onTimer() const auto is_obstacle_found = !nearest_obstacle ? false - : nearest_obstacle.get().first < node_param_.surround_check_hysteresis_distance; + : nearest_obstacle.value().first < node_param_.surround_check_hysteresis_distance; if (isStopRequired(is_obstacle_found, is_vehicle_stopped)) { break; @@ -363,7 +365,7 @@ void SurroundObstacleCheckerNode::onTimer() } if (nearest_obstacle) { - debug_ptr_->pushObstaclePoint(nearest_obstacle.get().second, PointType::NoStart); + debug_ptr_->pushObstaclePoint(nearest_obstacle.value().second, PointType::NoStart); } diagnostic_msgs::msg::DiagnosticStatus no_start_reason_diag; @@ -392,7 +394,7 @@ void SurroundObstacleCheckerNode::onOdometry(const nav_msgs::msg::Odometry::Cons odometry_ptr_ = msg; } -boost::optional SurroundObstacleCheckerNode::getNearestObstacle() const +std::optional SurroundObstacleCheckerNode::getNearestObstacle() const { const auto nearest_pointcloud = getNearestObstacleByPointCloud(); const auto nearest_object = getNearestObstacleByDynamicObject(); @@ -408,28 +410,29 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacle() cons return nearest_pointcloud; } - return nearest_pointcloud.get().first < nearest_object.get().first ? nearest_pointcloud - : nearest_object; + return nearest_pointcloud.value().first < nearest_object.value().first ? nearest_pointcloud + : nearest_object; } -boost::optional SurroundObstacleCheckerNode::getNearestObstacleByPointCloud() const +std::optional SurroundObstacleCheckerNode::getNearestObstacleByPointCloud() const { if (!node_param_.pointcloud_enable_check || !pointcloud_ptr_) { - return boost::none; + return std::nullopt; } if (pointcloud_ptr_->data.empty()) { - return boost::none; + return std::nullopt; } const auto transform_stamped = getTransform("base_link", pointcloud_ptr_->header.frame_id, pointcloud_ptr_->header.stamp, 0.5); if (!transform_stamped) { - return boost::none; + return std::nullopt; } - Eigen::Affine3f isometry = tf2::transformToEigen(transform_stamped.get().transform).cast(); + Eigen::Affine3f isometry = + tf2::transformToEigen(transform_stamped.value().transform).cast(); pcl::PointCloud transformed_pointcloud; pcl::fromROSMsg(*pointcloud_ptr_, transformed_pointcloud); tier4_autoware_utils::transformPointCloud( @@ -458,22 +461,22 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacleByPoint if (was_minimum_distance_updated) { return std::make_pair(minimum_distance, nearest_point); } - return boost::none; + return std::nullopt; } -boost::optional SurroundObstacleCheckerNode::getNearestObstacleByDynamicObject() const +std::optional SurroundObstacleCheckerNode::getNearestObstacleByDynamicObject() const { - if (!object_ptr_ || !use_dynamic_object_) return boost::none; + if (!object_ptr_ || !use_dynamic_object_) return std::nullopt; const auto transform_stamped = getTransform(object_ptr_->header.frame_id, "base_link", object_ptr_->header.stamp, 0.5); if (!transform_stamped) { - return boost::none; + return std::nullopt; } tf2::Transform tf_src2target; - tf2::fromMsg(transform_stamped.get().transform, tf_src2target); + tf2::fromMsg(transform_stamped.value().transform, tf_src2target); // TODO(murooka) check computation cost geometry_msgs::msg::Point nearest_point; @@ -515,10 +518,10 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacleByDynam if (was_minimum_distance_updated) { return std::make_pair(minimum_distance, nearest_point); } - return boost::none; + return std::nullopt; } -boost::optional SurroundObstacleCheckerNode::getTransform( +std::optional SurroundObstacleCheckerNode::getTransform( const std::string & source, const std::string & target, const rclcpp::Time & stamp, double duration_sec) const {