diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp index 853d4a5d..e2fc6aba 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp @@ -34,6 +34,7 @@ #include #include +#include #include #include @@ -164,13 +165,13 @@ lanelet::ConstLineStrings3d getLinkedParkingSpaces( const lanelet::ConstLanelet & lanelet, const lanelet::ConstLineStrings3d & all_parking_spaces, const lanelet::ConstPolygons3d & all_parking_lots); // query linked lanelets from parking space -bool getLinkedLanelet( +std::optional getLinkedLanelet( const lanelet::ConstLineString3d & parking_space, const lanelet::ConstLanelets & all_road_lanelets, - const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstLanelet * linked_lanelet); -bool getLinkedLanelet( + const lanelet::ConstPolygons3d & all_parking_lots); +std::optional getLinkedLanelet( const lanelet::ConstLineString3d & parking_space, - const lanelet::LaneletMapConstPtr & lanelet_map_ptr, lanelet::ConstLanelet * linked_lanelet); + const lanelet::LaneletMapConstPtr & lanelet_map_ptr); lanelet::ConstLanelets getLinkedLanelets( const lanelet::ConstLineString3d & parking_space, const lanelet::ConstLanelets & all_road_lanelets, @@ -180,17 +181,16 @@ lanelet::ConstLanelets getLinkedLanelets( const lanelet::LaneletMapConstPtr & lanelet_map_ptr); // get linked parking lot from lanelet -bool getLinkedParkingLot( - const lanelet::ConstLanelet & lanelet, const lanelet::ConstPolygons3d & all_parking_lots, - lanelet::ConstPolygon3d * linked_parking_lot); +std::optional getLinkedParkingLot( + const lanelet::ConstLanelet & lanelet, const lanelet::ConstPolygons3d & all_parking_lots); // get linked parking lot from current pose of ego car -bool getLinkedParkingLot( - const lanelet::BasicPoint2d & current_position, const lanelet::ConstPolygons3d & all_parking_lots, - lanelet::ConstPolygon3d * linked_parking_lot); +std::optional getLinkedParkingLot( + const lanelet::BasicPoint2d & current_position, + const lanelet::ConstPolygons3d & all_parking_lots); // get linked parking lot from parking space -bool getLinkedParkingLot( +std::optional getLinkedParkingLot( const lanelet::ConstLineString3d & parking_space, - const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstPolygon3d * linked_parking_lot); + const lanelet::ConstPolygons3d & all_parking_lots); // query linked parking space from parking lot lanelet::ConstLineStrings3d getLinkedParkingSpaces( @@ -246,23 +246,19 @@ ConstLanelets getAllNeighbors( const routing::RoutingGraphPtr & graph, const ConstLanelets & road_lanelets, const geometry_msgs::msg::Point & search_point); -bool getClosestLanelet( - const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose, - ConstLanelet * closest_lanelet_ptr); +std::optional getClosestLanelet( + const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose); -bool getClosestLaneletWithConstrains( +std::optional getClosestLaneletWithConstraints( const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose, - ConstLanelet * closest_lanelet_ptr, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); -bool getCurrentLanelets( - const ConstLanelets & lanelets, const geometry_msgs::msg::Point & search_point, - ConstLanelets * current_lanelets_ptr); +ConstLanelets getCurrentLanelets( + const ConstLanelets & lanelets, const geometry_msgs::msg::Point & search_point); -bool getCurrentLanelets( - const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose, - ConstLanelets * current_lanelets_ptr); +ConstLanelets getCurrentLanelets( + const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose); /** * [getSucceedingLaneletSequences retrieves a sequence of lanelets after the given lanelet. diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp index 7df3f6a2..e183d4cf 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp @@ -27,6 +27,7 @@ #include #include +#include namespace lanelet::utils { @@ -62,11 +63,11 @@ void overwriteLaneletsCenterline( lanelet::ConstLanelets getConflictingLanelets( const lanelet::routing::RoutingGraphConstPtr & graph, const lanelet::ConstLanelet & lanelet); -bool lineStringWithWidthToPolygon( - const lanelet::ConstLineString3d & linestring, lanelet::ConstPolygon3d * polygon); +std::optional lineStringWithWidthToPolygon( + const lanelet::ConstLineString3d & linestring); -bool lineStringToPolygon( - const lanelet::ConstLineString3d & linestring, lanelet::ConstPolygon3d * polygon); +std::optional lineStringToPolygon( + const lanelet::ConstLineString3d & linestring); double getLaneletLength2d(const lanelet::ConstLanelet & lanelet); double getLaneletLength3d(const lanelet::ConstLanelet & lanelet); diff --git a/tmp/lanelet2_extension/lib/query.cpp b/tmp/lanelet2_extension/lib/query.cpp index f444bdbd..5bd2c62f 100644 --- a/tmp/lanelet2_extension/lib/query.cpp +++ b/tmp/lanelet2_extension/lib/query.cpp @@ -407,38 +407,38 @@ lanelet::ConstLineStrings3d query::getAllParkingSpaces( return parking_spaces; } -bool query::getLinkedLanelet( +std::optional query::getLinkedLanelet( const lanelet::ConstLineString3d & parking_space, - const lanelet::LaneletMapConstPtr & lanelet_map_ptr, lanelet::ConstLanelet * linked_lanelet) + const lanelet::LaneletMapConstPtr & lanelet_map_ptr) { const auto & all_lanelets = query::laneletLayer(lanelet_map_ptr); const auto & all_road_lanelets = query::roadLanelets(all_lanelets); const auto & all_parking_lots = query::getAllParkingLots(lanelet_map_ptr); - return query::getLinkedLanelet( - parking_space, all_road_lanelets, all_parking_lots, linked_lanelet); + return query::getLinkedLanelet(parking_space, all_road_lanelets, all_parking_lots); } -bool query::getLinkedLanelet( +std::optional query::getLinkedLanelet( const lanelet::ConstLineString3d & parking_space, const lanelet::ConstLanelets & all_road_lanelets, - const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstLanelet * linked_lanelet) + const lanelet::ConstPolygons3d & all_parking_lots) { const auto & linked_lanelets = getLinkedLanelets(parking_space, all_road_lanelets, all_parking_lots); if (linked_lanelets.empty()) { - return false; + return std::nullopt; } + std::optional min{std::nullopt}; double min_distance = std::numeric_limits::max(); for (const auto & lanelet : linked_lanelets) { const double distance = boost::geometry::distance( to2D(parking_space).basicLineString(), lanelet.polygon2d().basicPolygon()); if (distance < min_distance) { - *linked_lanelet = lanelet; + min = lanelet; min_distance = distance; } } - return true; + return min; } lanelet::ConstLanelets query::getLinkedLanelets( @@ -457,15 +457,15 @@ lanelet::ConstLanelets query::getLinkedLanelets( const lanelet::ConstLanelets & all_road_lanelets, const lanelet::ConstPolygons3d & all_parking_lots) { - lanelet::ConstLanelets linked_lanelets; - // get lanelets within same parking lot - lanelet::ConstPolygon3d linked_parking_lot; - if (!getLinkedParkingLot(parking_space, all_parking_lots, &linked_parking_lot)) { - return linked_lanelets; + const auto linked_parking_lot_opt = getLinkedParkingLot(parking_space, all_parking_lots); + if (!linked_parking_lot_opt) { + return {}; } + const auto linked_parking_lot = linked_parking_lot_opt.value(); const auto & candidate_lanelets = getLinkedLanelets(linked_parking_lot, all_road_lanelets); + lanelet::ConstLanelets linked_lanelets; // get lanelets that are close to parking space and facing to parking space for (const auto & lanelet : candidate_lanelets) { // check if parking space is close to lanelet @@ -522,16 +522,16 @@ lanelet::ConstLineStrings3d query::getLinkedParkingSpaces( const lanelet::ConstLanelet & lanelet, const lanelet::ConstLineStrings3d & all_parking_spaces, const lanelet::ConstPolygons3d & all_parking_lots) { - lanelet::ConstLineStrings3d linked_parking_spaces; - // get parking spaces that are in same parking lot. - lanelet::ConstPolygon3d linked_parking_lot; - if (!getLinkedParkingLot(lanelet, all_parking_lots, &linked_parking_lot)) { - return linked_parking_spaces; + const auto linked_parking_lot_opt = getLinkedParkingLot(lanelet, all_parking_lots); + if (!linked_parking_lot_opt) { + return {}; } + const auto linked_parking_lot = linked_parking_lot_opt.value(); const auto & possible_parking_spaces = getLinkedParkingSpaces(linked_parking_lot, all_parking_spaces); + lanelet::ConstLineStrings3d linked_parking_spaces; // check for parking spaces that are within 5m and facing towards lanelet for (const auto & parking_space : possible_parking_spaces) { // check if parking space is close to lanelet @@ -561,51 +561,46 @@ lanelet::ConstLineStrings3d query::getLinkedParkingSpaces( } // get overlapping parking lot -bool query::getLinkedParkingLot( - const lanelet::ConstLanelet & lanelet, const lanelet::ConstPolygons3d & all_parking_lots, - lanelet::ConstPolygon3d * linked_parking_lot) +std::optional query::getLinkedParkingLot( + const lanelet::ConstLanelet & lanelet, const lanelet::ConstPolygons3d & all_parking_lots) { for (const auto & parking_lot : all_parking_lots) { const double distance = boost::geometry::distance( lanelet.polygon2d().basicPolygon(), to2D(parking_lot).basicPolygon()); if (distance < std::numeric_limits::epsilon()) { - *linked_parking_lot = parking_lot; - return true; + return parking_lot; } } - return false; + return std::nullopt; } // get overlapping parking lot -bool query::getLinkedParkingLot( - const lanelet::BasicPoint2d & current_position, const lanelet::ConstPolygons3d & all_parking_lots, - lanelet::ConstPolygon3d * linked_parking_lot) +std::optional query::getLinkedParkingLot( + const lanelet::BasicPoint2d & current_position, const lanelet::ConstPolygons3d & all_parking_lots) { for (const auto & parking_lot : all_parking_lots) { const double distance = boost::geometry::distance(current_position, to2D(parking_lot).basicPolygon()); if (distance < std::numeric_limits::epsilon()) { - *linked_parking_lot = parking_lot; - return true; + return parking_lot; } } - return false; + return std::nullopt; } // get overlapping parking lot -bool query::getLinkedParkingLot( +std::optional query::getLinkedParkingLot( const lanelet::ConstLineString3d & parking_space, - const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstPolygon3d * linked_parking_lot) + const lanelet::ConstPolygons3d & all_parking_lots) { for (const auto & parking_lot : all_parking_lots) { const double distance = boost::geometry::distance( to2D(parking_space).basicLineString(), to2D(parking_lot).basicPolygon()); if (distance < std::numeric_limits::epsilon()) { - *linked_parking_lot = parking_lot; - return true; + return parking_lot; } } - return false; + return std::nullopt; } lanelet::ConstLineStrings3d query::getLinkedParkingSpaces( @@ -827,84 +822,63 @@ ConstLanelets query::getAllNeighbors( return road_slices; } -bool query::getClosestLanelet( - const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose, - ConstLanelet * closest_lanelet_ptr) +std::optional query::getClosestLanelet( + const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose) { - if (closest_lanelet_ptr == nullptr) { - std::cerr << "argument closest_lanelet_ptr is null! Failed to find closest lanelet" - << std::endl; - return false; - } - if (lanelets.empty()) { - return false; + return std::nullopt; } - bool found = false; - lanelet::BasicPoint2d search_point(search_pose.position.x, search_pose.position.y); // find by distance lanelet::ConstLanelets candidate_lanelets; - { - double min_distance = std::numeric_limits::max(); - for (const auto & llt : lanelets) { - double distance = - boost::geometry::comparable_distance(llt.polygon2d().basicPolygon(), search_point); - - if (std::abs(distance - min_distance) <= std::numeric_limits::epsilon()) { - candidate_lanelets.push_back(llt); - } else if (distance < min_distance) { - found = true; - candidate_lanelets.clear(); - candidate_lanelets.push_back(llt); - min_distance = distance; - } + + std::optional closest_distance{std::nullopt}; + double min_distance = std::numeric_limits::max(); + for (const auto & llt : lanelets) { + double distance = + boost::geometry::comparable_distance(llt.polygon2d().basicPolygon(), search_point); + + if (std::abs(distance - min_distance) <= std::numeric_limits::epsilon()) { + candidate_lanelets.push_back(llt); + } else if (distance < min_distance) { + closest_distance = llt; + min_distance = distance; } } - if (candidate_lanelets.size() == 1) { - *closest_lanelet_ptr = candidate_lanelets.at(0); - return found; + if (closest_distance) { + return closest_distance.value(); } // find by angle - { - double min_angle = std::numeric_limits::max(); - double pose_yaw = tf2::getYaw(search_pose.orientation); - for (const auto & llt : candidate_lanelets) { - lanelet::ConstLineString3d segment = getClosestSegment(search_point, llt.centerline()); - double angle_diff = M_PI; - if (!segment.empty()) { - double segment_angle = std::atan2( - segment.back().y() - segment.front().y(), segment.back().x() - segment.front().x()); - angle_diff = std::abs(autoware_utils::normalize_radian(segment_angle - pose_yaw)); - } - if (angle_diff < min_angle) { - min_angle = angle_diff; - *closest_lanelet_ptr = llt; - } + std::optional closest_angle{std::nullopt}; + double min_angle = std::numeric_limits::max(); + double pose_yaw = tf2::getYaw(search_pose.orientation); + for (const auto & llt : candidate_lanelets) { + lanelet::ConstLineString3d segment = getClosestSegment(search_point, llt.centerline()); + double angle_diff = M_PI; + if (!segment.empty()) { + double segment_angle = std::atan2( + segment.back().y() - segment.front().y(), segment.back().x() - segment.front().x()); + angle_diff = std::abs(autoware_utils::normalize_radian(segment_angle - pose_yaw)); + } + if (angle_diff < min_angle) { + min_angle = angle_diff; + closest_angle = llt; } } - return found; + return closest_angle; } -bool query::getClosestLaneletWithConstrains( +std::optional query::getClosestLaneletWithConstraints( const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose, - ConstLanelet * closest_lanelet_ptr, const double dist_threshold, const double yaw_threshold) + const double dist_threshold, const double yaw_threshold) { - bool found = false; - - if (closest_lanelet_ptr == nullptr) { - std::cerr << "argument closest_lanelet_ptr is null! Failed to find closest lanelet" - << std::endl; - return found; - } - if (lanelets.empty()) { - return found; + return std::nullopt; } lanelet::BasicPoint2d search_point(search_pose.position.x, search_pose.position.y); @@ -928,11 +902,12 @@ bool query::getClosestLaneletWithConstrains( const std::pair & x, std::pair & y) { return x.second < y.second; }); } else { - return found; + return std::nullopt; } } // find closest lanelet within yaw_threshold + std::optional closest; { double min_angle = std::numeric_limits::max(); double min_distance = std::numeric_limits::max(); @@ -949,61 +924,48 @@ bool query::getClosestLaneletWithConstrains( if (angle_diff < min_angle) { min_angle = angle_diff; min_distance = distance; - *closest_lanelet_ptr = llt_pair.first; - found = true; + closest = llt_pair.first; } } } - return found; + return closest; } -bool query::getCurrentLanelets( - const ConstLanelets & lanelets, const geometry_msgs::msg::Point & search_point, - ConstLanelets * current_lanelets_ptr) +ConstLanelets query::getCurrentLanelets( + const ConstLanelets & lanelets, const geometry_msgs::msg::Point & search_point) { - if (current_lanelets_ptr == nullptr) { - std::cerr << "argument closest_lanelet_ptr is null! Failed to find closest lanelet" - << std::endl; - return false; - } - if (lanelets.empty()) { - return false; + return {}; } + ConstLanelets current_lanelets; lanelet::BasicPoint2d search_point_2d(search_point.x, search_point.y); for (const auto & llt : lanelets) { if (lanelet::geometry::inside(llt, search_point_2d)) { - current_lanelets_ptr->push_back(llt); + current_lanelets.push_back(llt); } } - return !current_lanelets_ptr->empty(); // return found + return current_lanelets; } -bool query::getCurrentLanelets( - const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose, - ConstLanelets * current_lanelets_ptr) +ConstLanelets query::getCurrentLanelets( + const ConstLanelets & lanelets, const geometry_msgs::msg::Pose & search_pose) { - if (current_lanelets_ptr == nullptr) { - std::cerr << "argument closest_lanelet_ptr is null! Failed to find closest lanelet" - << std::endl; - return false; - } - if (lanelets.empty()) { - return false; + return {}; } + ConstLanelets current_lanelets; lanelet::BasicPoint2d search_point(search_pose.position.x, search_pose.position.y); for (const auto & llt : lanelets) { if (lanelet::geometry::inside(llt, search_point)) { - current_lanelets_ptr->push_back(llt); + current_lanelets.push_back(llt); } } - return !current_lanelets_ptr->empty(); // return found + return current_lanelets; } std::vector> getSucceedingLaneletSequencesRecursive( diff --git a/tmp/lanelet2_extension/lib/utilities.cpp b/tmp/lanelet2_extension/lib/utilities.cpp index f84e9a43..c8c15b4c 100644 --- a/tmp/lanelet2_extension/lib/utilities.cpp +++ b/tmp/lanelet2_extension/lib/utilities.cpp @@ -523,24 +523,19 @@ lanelet::ConstLanelets getConflictingLanelets( return lanelets; } -bool lineStringWithWidthToPolygon( - const lanelet::ConstLineString3d & linestring, lanelet::ConstPolygon3d * polygon) +std::optional lineStringWithWidthToPolygon( + const lanelet::ConstLineString3d & linestring) { - if (polygon == nullptr) { - std::cerr << __func__ << ": polygon is null pointer! Failed to convert to polygon." - << std::endl; - return false; - } if (linestring.size() != 2) { std::cerr << __func__ << ": linestring" << linestring.id() << " must have 2 points! (" << linestring.size() << " != 2)" << std::endl << "Failed to convert to polygon."; - return false; + return std::nullopt; } if (!linestring.hasAttribute("width")) { std::cerr << __func__ << ": linestring" << linestring.id() << " does not have width tag. Failed to convert to polygon."; - return false; + return std::nullopt; } const Eigen::Vector3d direction = @@ -559,20 +554,12 @@ bool lineStringWithWidthToPolygon( const lanelet::Point3d p3(lanelet::InvalId, eigen_p3.x(), eigen_p3.y(), eigen_p3.z()); const lanelet::Point3d p4(lanelet::InvalId, eigen_p4.x(), eigen_p4.y(), eigen_p4.z()); - *polygon = lanelet::Polygon3d(lanelet::InvalId, {p1, p2, p3, p4}); - - return true; + return lanelet::Polygon3d(lanelet::InvalId, {p1, p2, p3, p4}); } -bool lineStringToPolygon( - const lanelet::ConstLineString3d & linestring, lanelet::ConstPolygon3d * polygon) +std::optional lineStringToPolygon( + const lanelet::ConstLineString3d & linestring) { - if (polygon == nullptr) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("lanelet2_extension.visualization"), - __func__ << ": polygon is null pointer! Failed to convert to polygon."); - return false; - } if (linestring.size() < 4) { if (linestring.size() < 3 || linestring.front().id() == linestring.back().id()) { RCLCPP_WARN_STREAM( @@ -580,7 +567,7 @@ bool lineStringToPolygon( __func__ << ": linestring" << linestring.id() << " must have more than different 3 points! (size is " << linestring.size() << "). Failed to convert to polygon."); - return false; + return std::nullopt; } } @@ -595,9 +582,7 @@ bool lineStringToPolygon( llt_poly.pop_back(); } - *polygon = llt_poly; - - return true; + return llt_poly; } double getLaneletLength2d(const lanelet::ConstLanelet & lanelet) @@ -632,11 +617,14 @@ double getLaneletLength3d(const lanelet::ConstLanelets & lanelet_sequence) lanelet::ArcCoordinates getArcCoordinates( const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose & pose) { - lanelet::ConstLanelet closest_lanelet; - lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lanelet); + const auto closest_lanelet_opt = lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose); + lanelet::ArcCoordinates arc_coordinates; + if (!closest_lanelet_opt) { + return arc_coordinates; + } + const auto closest_lanelet = closest_lanelet_opt.value(); double length = 0; - lanelet::ArcCoordinates arc_coordinates; for (const auto & llt : lanelet_sequence) { const auto & centerline_2d = lanelet::utils::to2D(llt.centerline()); if (llt == closest_lanelet) { @@ -783,9 +771,11 @@ double getLateralDistanceToCenterline( double getLateralDistanceToClosestLanelet( const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose & pose) { - lanelet::ConstLanelet closest_lanelet; - lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lanelet); - return getLateralDistanceToCenterline(closest_lanelet, pose); + const auto closest_lanelet_opt = lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose); + if (!closest_lanelet_opt) { + return 0.0; + } + return getLateralDistanceToCenterline(closest_lanelet_opt.value(), pose); } } // namespace lanelet::utils diff --git a/tmp/lanelet2_extension/lib/visualization.cpp b/tmp/lanelet2_extension/lib/visualization.cpp index 95301cef..1513d796 100644 --- a/tmp/lanelet2_extension/lib/visualization.cpp +++ b/tmp/lanelet2_extension/lib/visualization.cpp @@ -932,9 +932,9 @@ visualization_msgs::msg::MarkerArray visualization::pedestrianMarkingsAsMarkerAr visualization_msgs::msg::Marker marker = createPolygonMarker("pedestrian_marking", c); for (const auto & linestring : pedestrian_markings) { - lanelet::ConstPolygon3d polygon; - if (utils::lineStringToPolygon(linestring, &polygon)) { - pushPolygonMarker(&marker, polygon, c); + const auto polygon_opt = utils::lineStringToPolygon(linestring); + if (polygon_opt) { + pushPolygonMarker(&marker, polygon_opt.value(), c); } else { RCLCPP_WARN_STREAM( rclcpp::get_logger("lanelet2_extension.visualization"), @@ -976,9 +976,9 @@ visualization_msgs::msg::MarkerArray visualization::parkingSpacesAsMarkerArray( visualization_msgs::msg::Marker marker = createPolygonMarker("parking_space", c); for (const auto & linestring : parking_spaces) { - lanelet::ConstPolygon3d polygon; - if (utils::lineStringWithWidthToPolygon(linestring, &polygon)) { - pushPolygonMarker(&marker, polygon, c); + const auto polygon_opt = utils::lineStringToPolygon(linestring); + if (polygon_opt) { + pushPolygonMarker(&marker, polygon_opt.value(), c); } else { std::cerr << "parking space " << linestring.id() << " failed conversion." << std::endl; }