diff --git a/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt b/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt index 011d215c6ac3a..a2aaba44b6626 100644 --- a/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt +++ b/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt @@ -50,7 +50,6 @@ target_link_libraries(${TARGET} ${PCL_LIBRARIES} Sophus::Sophus) # Semantic segmentation install(PROGRAMS src/semantic_segmentation/semantic_segmentation_core.py - src/semantic_segmentation/semantic_segmentation_node.py src/semantic_segmentation/semantic_segmentation_server.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/localization/yabloc/yabloc_pose_initializer/README.md b/localization/yabloc/yabloc_pose_initializer/README.md index 5a018397b96a7..4e3c0a0363402 100644 --- a/localization/yabloc/yabloc_pose_initializer/README.md +++ b/localization/yabloc/yabloc_pose_initializer/README.md @@ -5,6 +5,20 @@ This package contains some nodes related to initial pose estimation. - [camera_pose_initializer](#camera_pose_initializer) - [semantic_segmentation_server](#semantic_segmentation_server) +Ideally, this package downloads a pre-trained semantic segmentation model during the build and loads it at runtime for initialization. +However, to handle cases where network connectivity is not available at build time, **the default behavior is not to download the model during build.** +Even if the model is not downloaded, initialization will still complete, but the accuracy may be compromised. + + + +To download the model, please specify `--cmake-args -DDOWNLOAD_ARTIFACTS=ON` to the build command. + +```bash +colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release -DDOWNLOAD_ARTIFACTS=ON --packages-select yabloc_pose_initializer +``` + +For detailed information about the downloaded contents, please consult the `download.cmake` file in this package. + ## Note This package makes use of external code. The trained files are provided by apollo. The trained files are automatically downloaded when you build. diff --git a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp index a5db23f262ff2..ccb55e79b8ea9 100644 --- a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp +++ b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp @@ -70,7 +70,8 @@ class CameraPoseInitializer : public rclcpp::Node PoseCovStamped create_rectified_initial_pose( const Eigen::Vector3f & pos, double yaw_angle_rad, const PoseCovStamped & src_msg); - bool estimate_pose(const Eigen::Vector3f & position, double & yaw_angle_rad, double yaw_std_rad); + std::optional estimate_pose( + const Eigen::Vector3f & position, const double yaw_angle_rad, const double yaw_std_rad); }; } // namespace yabloc diff --git a/localization/yabloc/yabloc_pose_initializer/launch/semantic_segmentation.launch.xml b/localization/yabloc/yabloc_pose_initializer/launch/semantic_segmentation.launch.xml deleted file mode 100644 index 66c80f7983484..0000000000000 --- a/localization/yabloc/yabloc_pose_initializer/launch/semantic_segmentation.launch.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp index 5af81028efa22..5fab9763328e3 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp @@ -20,10 +20,6 @@ #include #include -#include -#include -#include - namespace yabloc { CameraPoseInitializer::CameraPoseInitializer() @@ -74,7 +70,7 @@ cv::Mat bitwise_and_3ch(const cv::Mat src1, const cv::Mat src2) return merged; } -int count_nonzero(cv::Mat image_3ch) +int count_non_zero(cv::Mat image_3ch) { std::vector images; cv::split(image_3ch, images); @@ -85,20 +81,20 @@ int count_nonzero(cv::Mat image_3ch) return count; } -bool CameraPoseInitializer::estimate_pose( - const Eigen::Vector3f & position, double & yaw_angle_rad, double yaw_std_rad) +std::optional CameraPoseInitializer::estimate_pose( + const Eigen::Vector3f & position, const double yaw_angle_rad, const double yaw_std_rad) { if (!projector_module_->define_project_func()) { - return false; + return std::nullopt; } if (!lane_image_) { RCLCPP_WARN_STREAM(get_logger(), "vector map is not ready "); - return false; + return std::nullopt; } // TODO(KYabuuchi) check latest_image_msg's time stamp, too if (!latest_image_msg_.has_value()) { RCLCPP_WARN_STREAM(get_logger(), "source image is not ready"); - return false; + return std::nullopt; } Image segmented_image; @@ -110,10 +106,22 @@ bool CameraPoseInitializer::estimate_pose( using namespace std::literals::chrono_literals; std::future_status status = result_future.wait_for(1000ms); if (status == std::future_status::ready) { - segmented_image = result_future.get()->dst_image; + const auto response = result_future.get(); + if (response->success) { + segmented_image = response->dst_image; + } else { + RCLCPP_WARN_STREAM(get_logger(), "segmentation service failed unexpectedly"); + // NOTE: Even if the segmentation service fails, the function will still return the + // yaw_angle_rad as it is and complete the initialization. The service fails + // when the DNN model is not downloaded. Ideally, initialization should rely on + // segmentation, but this implementation allows initialization even in cases where network + // connectivity is not available. + return yaw_angle_rad; + } } else { - RCLCPP_ERROR_STREAM(get_logger(), "segmentation service exited unexpectedly"); - return false; + RCLCPP_ERROR_STREAM( + get_logger(), "segmentation service did not return within the expected time"); + return std::nullopt; } } @@ -149,30 +157,18 @@ bool CameraPoseInitializer::estimate_pose( if (lane_angle_rad) { gain = 2 + std::cos((lane_angle_rad.value() - angle_rad) / 2.0); } - const float score = gain * count_nonzero(dst); - - // DEBUG: - constexpr bool imshow = false; - if (imshow) { - cv::Mat show_image; - cv::hconcat(std::vector{rotated_image, vector_map_image, dst}, show_image); - cv::imshow("and operator", show_image); - cv::waitKey(50); - } + // If count_non_zero() returns 0 everywhere, the orientation is chosen by the only gain + const float score = gain * (1 + count_non_zero(dst)); scores.push_back(score); angles_rad.push_back(angle_rad); } - { - size_t max_index = - std::distance(scores.begin(), std::max_element(scores.begin(), scores.end())); - yaw_angle_rad = angles_rad.at(max_index); - } - marker_module_->publish_marker(scores, angles_rad, position); - return true; + const size_t max_index = + std::distance(scores.begin(), std::max_element(scores.begin(), scores.end())); + return angles_rad.at(max_index); } void CameraPoseInitializer::on_map(const HADMapBin & msg) @@ -193,8 +189,6 @@ void CameraPoseInitializer::on_service( { RCLCPP_INFO_STREAM(get_logger(), "CameraPoseInitializer on_service"); - response->success = false; - const auto query_pos_with_cov = request->pose_with_covariance; const auto query_pos = request->pose_with_covariance.pose.pose.position; const auto orientation = request->pose_with_covariance.pose.pose.orientation; @@ -203,12 +197,14 @@ void CameraPoseInitializer::on_service( RCLCPP_INFO_STREAM(get_logger(), "Given initial position " << pos_vec3f.transpose()); // Estimate orientation - const auto header = request->pose_with_covariance.header; - double yaw_angle_rad = 2 * std::atan2(orientation.z, orientation.w); - if (estimate_pose(pos_vec3f, yaw_angle_rad, yaw_std_rad)) { + const double initial_yaw_angle_rad = 2 * std::atan2(orientation.z, orientation.w); + const auto yaw_angle_rad_opt = estimate_pose(pos_vec3f, initial_yaw_angle_rad, yaw_std_rad); + if (yaw_angle_rad_opt.has_value()) { response->success = true; response->pose_with_covariance = - create_rectified_initial_pose(pos_vec3f, yaw_angle_rad, query_pos_with_cov); + create_rectified_initial_pose(pos_vec3f, yaw_angle_rad_opt.value(), query_pos_with_cov); + } else { + response->success = false; } } diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp index 95d948e86a7bc..c6c34f0e28a42 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp @@ -32,7 +32,7 @@ void MarkerModule::publish_marker( auto minmax = std::minmax_element(scores.begin(), scores.end()); auto normalize = [minmax](int score) -> float { return static_cast(score - *minmax.first) / - static_cast(*minmax.second - *minmax.first); + std::max(1e-4f, static_cast(*minmax.second - *minmax.first)); }; MarkerArray array; diff --git a/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_node.py b/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_node.py deleted file mode 100755 index 9b486f16c01e0..0000000000000 --- a/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_node.py +++ /dev/null @@ -1,93 +0,0 @@ -#!/usr/bin/env python3 - -# 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. - -import sys -import time - -import cv2 -from cv_bridge import CvBridge -import rclpy -from rclpy.node import Node -import semantic_segmentation_core as core -from sensor_msgs.msg import Image - - -class SemanticSegmentationNode(Node): - def __init__(self): - super().__init__("semantic_segmentation_node") - - model_path = self.declare_parameter("model_path", "").value - self.imshow_ = self.declare_parameter("imshow", False).value - - self.get_logger().info("model path: " + model_path) - - self.sub_image_ = self.create_subscription( - Image, "~/input/image_raw", self.imageCallback, 10 - ) - - self.pub_overlay_image_ = self.create_publisher(Image, "~/output/overlay_image", 10) - self.pub_image_ = self.create_publisher(Image, "~/output/semantic_image", 10) - - self.dnn_ = core.SemanticSegmentationCore(model_path) - self.bridge_ = CvBridge() - - def imageCallback(self, msg: Image): - stamp = msg.header.stamp - self.get_logger().info("Subscribed image: " + str(stamp)) - - src_image = self.bridge_.imgmsg_to_cv2(msg) - start_time = time.time() - mask = self.dnn_.inference(src_image) - elapsed_time = time.time() - start_time - - show_image = self.dnn_.drawOverlay(src_image, mask) - cv2.putText( - show_image, - "Inference: " + "{:.1f}".format(elapsed_time * 1000) + "ms", - (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, - 0.7, - (0, 255, 255), - 2, - cv2.LINE_AA, - ) - - # visualize - if self.imshow_: - cv2.imshow("segmentation", show_image) - cv2.waitKey(1) - - # publish dst image - self.__publish_image(mask, self.pub_image_) - self.__publish_image(show_image, self.pub_overlay_image_) - - def __publish_image(self, image, publisher): - out_msg = self.bridge_.cv2_to_imgmsg(image) - out_msg.encoding = "bgr8" - publisher.publish(out_msg) - - -def main(): - rclpy.init(args=sys.argv) - - segmentation_node = SemanticSegmentationNode() - rclpy.spin(segmentation_node) - segmentation_node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_server.py b/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_server.py index e7d212a9758aa..03de9de572910 100755 --- a/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_server.py +++ b/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_server.py @@ -14,6 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import os import sys from cv_bridge import CvBridge @@ -23,6 +24,12 @@ from sensor_msgs.msg import Image from yabloc_pose_initializer.srv import SemanticSegmentation +# cspell: ignore DDOWNLOAD +ERROR_MESSAGE = """\ +The yabloc_pose_initializer is not working correctly because the DNN model has not been downloaded correctly. +To download models, "-DDOWNLOAD_ARTIFACTS=ON" is required at build time. +Please see the README of yabloc_pose_initializer for more information.""" + class SemanticSegmentationServer(Node): def __init__(self): @@ -31,26 +38,43 @@ def __init__(self): model_path = self.declare_parameter("model_path", "").value self.get_logger().info("model path: " + model_path) - self.dnn_ = core.SemanticSegmentationCore(model_path) self.bridge_ = CvBridge() + if os.path.exists(model_path): + self.dnn_ = core.SemanticSegmentationCore(model_path) + else: + self.dnn_ = None + self.__print_error_message() + self.srv = self.create_service( SemanticSegmentation, "semantic_segmentation_srv", self.on_service ) + def __print_error_message(self): + messages = ERROR_MESSAGE.split("\n") + for message in messages: + self.get_logger().error(message) + def on_service(self, request, response): - response.dst_image = self.__inference(request.src_image) + if self.dnn_: + response.dst_image = self.__inference(request.src_image) + response.success = True + else: + self.__print_error_message() + response.success = False + response.dst_image = request.src_image + return response def __inference(self, msg: Image): stamp = msg.header.stamp self.get_logger().info("Subscribed image: " + str(stamp)) - src_image = self.bridge_.imgmsg_to_cv2(msg) - mask = self.dnn_.inference(src_image) + mask = self.dnn_.inference(src_image) dst_msg = self.bridge_.cv2_to_imgmsg(mask) dst_msg.encoding = "bgr8" + return dst_msg diff --git a/localization/yabloc/yabloc_pose_initializer/srv/SemanticSegmentation.srv b/localization/yabloc/yabloc_pose_initializer/srv/SemanticSegmentation.srv index b0fef18afdcb0..3daa611fd18ce 100644 --- a/localization/yabloc/yabloc_pose_initializer/srv/SemanticSegmentation.srv +++ b/localization/yabloc/yabloc_pose_initializer/srv/SemanticSegmentation.srv @@ -1,3 +1,4 @@ sensor_msgs/Image src_image --- +bool success sensor_msgs/Image dst_image