feat(lanelet2_extension): deprecate toGeomMsgPt/toLaneltPoint

autoware_lanelet2_extension/CMakeLists.txt

@@ -38,8 +38,6 @@ ament_auto_add_library(${PROJECT_NAME}_lib SHARED
 target_compile_options(${PROJECT_NAME}_lib PRIVATE -Wno-error=maybe-uninitialized)
 
 if(BUILD_TESTING)
-  ament_add_ros_isolated_gtest(message_conversion-test test/src/test_message_conversion.cpp)
-  target_link_libraries(message_conversion-test ${PROJECT_NAME}_lib ${tf2_LIBRARIES})
   ament_add_ros_isolated_gtest(projector-test test/src/test_projector.cpp)
   target_link_libraries(projector-test ${PROJECT_NAME}_lib ${tf2_LIBRARIES})
   ament_add_ros_isolated_gtest(query-test test/src/test_query.cpp)

autoware_lanelet2_extension/include/autoware_lanelet2_extension/utility/message_conversion.hpp

@@ -36,21 +36,21 @@ namespace lanelet::utils::conversion
  * @param map [lanelet map data]
  * @param msg [converted ROS message. Only "data" field is filled]
  */
-[[deprecated("please use autoware::lanelet2_utils::to_autoware_map_msgs instead")]]
-void toBinMsg(const lanelet::LaneletMapPtr & map, autoware_map_msgs::msg::LaneletMapBin * msg);
+[[deprecated("please use autoware::lanelet2_utils::to_autoware_map_msgs instead")]] void toBinMsg(
+  const lanelet::LaneletMapPtr & map, autoware_map_msgs::msg::LaneletMapBin * msg);
 
 /**
  * [fromBinMsg converts ROS message into lanelet2 data. Similar implementation
  * to lanelet::io_handlers::BinHandler::parse()]
  * @param msg [ROS message for lanelet map]
  * @param map [Converted lanelet2 data]
  */
-[[deprecated("please use autoware::lanelet2_utils::from_autoware_map_msgs instead")]]
-void fromBinMsg(const autoware_map_msgs::msg::LaneletMapBin & msg, lanelet::LaneletMapPtr map);
+[[deprecated("please use autoware::lanelet2_utils::from_autoware_map_msgs instead")]] void
+fromBinMsg(const autoware_map_msgs::msg::LaneletMapBin & msg, lanelet::LaneletMapPtr map);
 [[deprecated(
   "please use autoware::lanelet2_utils::from_autoware_map_msgs and "
-  "autoware::lanelet2_utils::instantiate_routing_graph_and_traffic_rules instead")]]
-void fromBinMsg(
+  "autoware::lanelet2_utils::instantiate_routing_graph_and_traffic_rules instead")]] void
+fromBinMsg(
   const autoware_map_msgs::msg::LaneletMapBin & msg, lanelet::LaneletMapPtr map,
   lanelet::traffic_rules::TrafficRulesPtr * traffic_rules,
   lanelet::routing::RoutingGraphPtr * routing_graph);
@@ -61,40 +61,53 @@ void fromBinMsg(
  * Eigen::VEctor3d=lanelet::BasicPoint3d, lanelet::Point3d, lanelet::Point2d) ]
  * @param dst [converted geometry_msgs point]
  */
-void toGeomMsgPt(const geometry_msgs::msg::Point32 & src, geometry_msgs::msg::Point * dst);
-void toGeomMsgPt(const Eigen::Vector3d & src, geometry_msgs::msg::Point * dst);
-void toGeomMsgPt(const lanelet::ConstPoint3d & src, geometry_msgs::msg::Point * dst);
-void toGeomMsgPt(const lanelet::ConstPoint2d & src, geometry_msgs::msg::Point * dst);
+[[deprecated("please use geometry_msgs::build instead")]] void toGeomMsgPt(
+  const geometry_msgs::msg::Point32 & src, geometry_msgs::msg::Point * dst);
+[[deprecated("please use geometry_msgs::build instead")]] void toGeomMsgPt(
+  const Eigen::Vector3d & src, geometry_msgs::msg::Point * dst);
+[[deprecated("please use autoware::lanelet2_utils::to_ros")]] void toGeomMsgPt(
+  const lanelet::ConstPoint3d & src, geometry_msgs::msg::Point * dst);
+[[deprecated("please use autoware::lanelet2_utils::to_ros")]] void toGeomMsgPt(
+  const lanelet::ConstPoint2d & src, geometry_msgs::msg::Point * dst);
 
 /**
  * [toGeomMsgPt converts various point types to geometry_msgs point]
  * @param src [input point(geometry_msgs::msg::Point3,
  * Eigen::VEctor3d=lanelet::BasicPoint3d, lanelet::Point3d, lanelet::Point2d) ]
  * @return    [converted geometry_msgs point]
  */
-geometry_msgs::msg::Point toGeomMsgPt(const geometry_msgs::msg::Point32 & src);
-geometry_msgs::msg::Point toGeomMsgPt(const Eigen::Vector3d & src);
-geometry_msgs::msg::Point toGeomMsgPt(const lanelet::ConstPoint3d & src);
-geometry_msgs::msg::Point toGeomMsgPt(const lanelet::ConstPoint2d & src);
+[[deprecated("please use geometry_msgs::build instead")]] geometry_msgs::msg::Point toGeomMsgPt(
+  const geometry_msgs::msg::Point32 & src);
+[[deprecated("please use geometry_msgs::build instead")]] geometry_msgs::msg::Point toGeomMsgPt(
+  const Eigen::Vector3d & src);
+[[deprecated("please use autoware::lanelet2_utils::to_ros")]] geometry_msgs::msg::Point toGeomMsgPt(
+  const lanelet::ConstPoint3d & src);
+[[deprecated("please use autoware::lanelet2_utils::to_ros")]] geometry_msgs::msg::Point toGeomMsgPt(
+  const lanelet::ConstPoint2d & src);
 
-lanelet::ConstPoint3d toLaneletPoint(const geometry_msgs::msg::Point & src);
-void toLaneletPoint(const geometry_msgs::msg::Point & src, lanelet::ConstPoint3d * dst);
+[[deprecated("please use autoware::lanelet2_utils::from_ros")]] lanelet::ConstPoint3d
+toLaneletPoint(const geometry_msgs::msg::Point & src);
+[[deprecated("please use autoware::lanelet2_utils::from_ros")]] void toLaneletPoint(
+  const geometry_msgs::msg::Point & src, lanelet::ConstPoint3d * dst);
 
 /**
  * [toGeomMsgPoly converts lanelet polygon to geometry_msgs polygon]
  * @param ll_poly   [input polygon]
  * @param geom_poly [converted geometry_msgs point]
  */
-void toGeomMsgPoly(
-  const lanelet::ConstPolygon3d & ll_poly, geometry_msgs::msg::Polygon * geom_poly);
+[[deprecated(
+  "toGeomMsgPoly(const lanelet::ConstPolygon3d, geometry_msgs::msg::Polygon *) will be "
+  "deprecated")]] void
+toGeomMsgPoly(const lanelet::ConstPolygon3d & ll_poly, geometry_msgs::msg::Polygon * geom_poly);
 
 /**
  * [toGeomMsgPt32 converts Eigen::Vector3d(lanelet:BasicPoint3d to
  * geometry_msgs::msg::Point32)]
  * @param src [input point]
  * @param dst [converted point]
  */
-void toGeomMsgPt32(const Eigen::Vector3d & src, geometry_msgs::msg::Point32 * dst);
+[[deprecated("please use geometry_msgs::build instead")]] void toGeomMsgPt32(
+  const Eigen::Vector3d & src, geometry_msgs::msg::Point32 * dst);
 
 }  // namespace lanelet::utils::conversion
 

autoware_lanelet2_extension/lib/deprecated.cpp

@@ -253,7 +253,7 @@ double getLateralDistanceToCenterline(
   const lanelet::ConstLanelet & lanelet, const geometry_msgs::msg::Pose & pose)
 {
   const auto & centerline_2d = lanelet::utils::to2D(lanelet.centerline());
-  const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(pose.position);
+  const auto lanelet_point = toLaneletPoint(pose.position);
   return lanelet::geometry::signedDistance(
     centerline_2d, lanelet::utils::to2D(lanelet_point).basicPoint());
 }
@@ -369,4 +369,67 @@ lanelet::ConstLanelet getExpandedLanelet(
   return lanelet;
 }
 
+void toGeomMsgPt(const geometry_msgs::msg::Point32 & src, geometry_msgs::msg::Point * dst)
+{
+  if (dst == nullptr) {
+    std::cerr << __FUNCTION__ << "pointer is null!";
+    return;
+  }
+  dst->x = src.x;
+  dst->y = src.y;
+  dst->z = src.z;
+}
+void toGeomMsgPt(const Eigen::Vector3d & src, geometry_msgs::msg::Point * dst)
+{
+  if (dst == nullptr) {
+    std::cerr << __FUNCTION__ << "pointer is null!";
+    return;
+  }
+  dst->x = src.x();
+  dst->y = src.y();
+  dst->z = src.z();
+}
+void toGeomMsgPt(const lanelet::ConstPoint3d & src, geometry_msgs::msg::Point * dst)
+{
+  if (dst == nullptr) {
+    std::cerr << __FUNCTION__ << "pointer is null!";
+    return;
+  }
+  dst->x = src.x();
+  dst->y = src.y();
+  dst->z = src.z();
+}
+void toGeomMsgPt(const lanelet::ConstPoint2d & src, geometry_msgs::msg::Point * dst)
+{
+  if (dst == nullptr) {
+    std::cerr << __FUNCTION__ << "pointer is null!" << std::endl;
+    return;
+  }
+  dst->x = src.x();
+  dst->y = src.y();
+  dst->z = 0;
+}
+
+void toGeomMsgPt32(const Eigen::Vector3d & src, geometry_msgs::msg::Point32 * dst)
+{
+  if (dst == nullptr) {
+    std::cerr << __FUNCTION__ << "pointer is null!" << std::endl;
+    return;
+  }
+  dst->x = static_cast<float>(src.x());
+  dst->y = static_cast<float>(src.y());
+  dst->z = static_cast<float>(src.z());
+}
+
+void toLaneletPoint(const geometry_msgs::msg::Point & src, lanelet::ConstPoint3d * dst)
+{
+  *dst = lanelet::Point3d(lanelet::InvalId, src.x, src.y, src.z);
+}
+
+lanelet::ConstPoint3d toLaneletPoint(const geometry_msgs::msg::Point & src)
+{
+  lanelet::ConstPoint3d dst;
+  toLaneletPoint(src, &dst);
+  return dst;
+}
 }  // namespace deprecated

autoware_lanelet2_extension/lib/deprecated.hpp

@@ -20,6 +20,7 @@
 #define AUTOWARE_LANELET2_EXTENSION__LIB__DEPRECATED_HPP_
 
 #include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/point32.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 
 #include <lanelet2_core/primitives/Lanelet.h>
@@ -84,6 +85,19 @@ lanelet::ConstLanelet combineLaneletsShape(const lanelet::ConstLanelets & lanele
 lanelet::ConstLanelet getExpandedLanelet(
   const lanelet::ConstLanelet & lanelet_obj, const double left_offset, const double right_offset);
 
+void toGeomMsgPt(const geometry_msgs::msg::Point32 & src, geometry_msgs::msg::Point * dst);
+
+void toGeomMsgPt(const Eigen::Vector3d & src, geometry_msgs::msg::Point * dst);
+
+void toGeomMsgPt(const lanelet::ConstPoint3d & src, geometry_msgs::msg::Point * dst);
+
+void toGeomMsgPt(const lanelet::ConstPoint2d & src, geometry_msgs::msg::Point * dst);
+
+void toGeomMsgPt32(const Eigen::Vector3d & src, geometry_msgs::msg::Point32 * dst);
+
+void toLaneletPoint(const geometry_msgs::msg::Point & src, lanelet::ConstPoint3d * dst);
+
+lanelet::ConstPoint3d toLaneletPoint(const geometry_msgs::msg::Point & src);
 }  // namespace deprecated
 // NOLINTEND(readability-identifier-naming)
 

autoware_lanelet2_extension/lib/message_conversion.cpp

@@ -19,6 +19,7 @@
 #include "autoware_lanelet2_extension/utility/message_conversion.hpp"
 
 #include "autoware_lanelet2_extension/projection/mgrs_projector.hpp"
+#include "deprecated.hpp"
 
 #include <boost/archive/binary_iarchive.hpp>
 #include <boost/archive/binary_oarchive.hpp>
@@ -39,10 +40,9 @@
 #include <string>
 #include <utility>
 
-namespace impl
+namespace deprecated
 {
-static void fromBinMsg(
-  const autoware_map_msgs::msg::LaneletMapBin & msg, lanelet::LaneletMapPtr map)
+void fromBinMsg(const autoware_map_msgs::msg::LaneletMapBin & msg, lanelet::LaneletMapPtr map)
 {
   if (!map) {
     std::cerr << __FUNCTION__ << ": map is null pointer!";
@@ -71,7 +71,7 @@ void fromBinMsg(
     lanelet::Locations::Germany, lanelet::Participants::Vehicle);
   *routing_graph = lanelet::routing::RoutingGraph::build(*map, **traffic_rules);
 }
-}  // namespace impl
+}  // namespace deprecated
 
 namespace lanelet::utils::conversion
 {
@@ -119,99 +119,66 @@ void fromBinMsg(
   lanelet::traffic_rules::TrafficRulesPtr * traffic_rules,
   lanelet::routing::RoutingGraphPtr * routing_graph)
 {
-  ::impl::fromBinMsg(msg, map);
+  deprecated::fromBinMsg(msg, map);
   *traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create(
     lanelet::Locations::Germany, lanelet::Participants::Vehicle);
   *routing_graph = lanelet::routing::RoutingGraph::build(*map, **traffic_rules);
 }
 
 void toGeomMsgPt(const geometry_msgs::msg::Point32 & src, geometry_msgs::msg::Point * dst)
 {
-  if (dst == nullptr) {
-    std::cerr << __FUNCTION__ << "pointer is null!";
-    return;
-  }
-  dst->x = src.x;
-  dst->y = src.y;
-  dst->z = src.z;
+  deprecated::toGeomMsgPt(src, dst);
 }
 void toGeomMsgPt(const Eigen::Vector3d & src, geometry_msgs::msg::Point * dst)
 {
-  if (dst == nullptr) {
-    std::cerr << __FUNCTION__ << "pointer is null!";
-    return;
-  }
-  dst->x = src.x();
-  dst->y = src.y();
-  dst->z = src.z();
+  deprecated::toGeomMsgPt(src, dst);
 }
 void toGeomMsgPt(const lanelet::ConstPoint3d & src, geometry_msgs::msg::Point * dst)
 {
-  if (dst == nullptr) {
-    std::cerr << __FUNCTION__ << "pointer is null!";
-    return;
-  }
-  dst->x = src.x();
-  dst->y = src.y();
-  dst->z = src.z();
+  deprecated::toGeomMsgPt(src, dst);
 }
 void toGeomMsgPt(const lanelet::ConstPoint2d & src, geometry_msgs::msg::Point * dst)
 {
-  if (dst == nullptr) {
-    std::cerr << __FUNCTION__ << "pointer is null!" << std::endl;
-    return;
-  }
-  dst->x = src.x();
-  dst->y = src.y();
-  dst->z = 0;
+  deprecated::toGeomMsgPt(src, dst);
 }
-
 void toGeomMsgPt32(const Eigen::Vector3d & src, geometry_msgs::msg::Point32 * dst)
 {
-  if (dst == nullptr) {
-    std::cerr << __FUNCTION__ << "pointer is null!" << std::endl;
-    return;
-  }
-  dst->x = static_cast<float>(src.x());
-  dst->y = static_cast<float>(src.y());
-  dst->z = static_cast<float>(src.z());
+  deprecated::toGeomMsgPt32(src, dst);
 }
 
 geometry_msgs::msg::Point toGeomMsgPt(const geometry_msgs::msg::Point32 & src)
 {
   geometry_msgs::msg::Point dst;
-  toGeomMsgPt(src, &dst);
+  deprecated::toGeomMsgPt(src, &dst);
   return dst;
 }
 geometry_msgs::msg::Point toGeomMsgPt(const Eigen::Vector3d & src)
 {
   geometry_msgs::msg::Point dst;
-  toGeomMsgPt(src, &dst);
+  deprecated::toGeomMsgPt(src, &dst);
   return dst;
 }
 geometry_msgs::msg::Point toGeomMsgPt(const lanelet::ConstPoint3d & src)
 {
   geometry_msgs::msg::Point dst;
-  toGeomMsgPt(src, &dst);
+  deprecated::toGeomMsgPt(src, &dst);
   return dst;
 }
 geometry_msgs::msg::Point toGeomMsgPt(const lanelet::ConstPoint2d & src)
 {
   geometry_msgs::msg::Point dst;
-  toGeomMsgPt(src, &dst);
+  deprecated::toGeomMsgPt(src, &dst);
   return dst;
 }
 
 lanelet::ConstPoint3d toLaneletPoint(const geometry_msgs::msg::Point & src)
 {
-  lanelet::ConstPoint3d dst;
-  toLaneletPoint(src, &dst);
-  return dst;
+  return deprecated::toLaneletPoint(src);
 }
 
 void toLaneletPoint(const geometry_msgs::msg::Point & src, lanelet::ConstPoint3d * dst)
 {
-  *dst = lanelet::Point3d(lanelet::InvalId, src.x, src.y, src.z);
+  deprecated::toLaneletPoint(src, dst);
 }
 
 void toGeomMsgPoly(const lanelet::ConstPolygon3d & ll_poly, geometry_msgs::msg::Polygon * geom_poly)
@@ -220,7 +187,7 @@ void toGeomMsgPoly(const lanelet::ConstPolygon3d & ll_poly, geometry_msgs::msg::
   geom_poly->points.reserve(ll_poly.size());
   for (const auto & ll_pt : ll_poly) {
     geometry_msgs::msg::Point32 geom_pt32;
-    utils::conversion::toGeomMsgPt32(ll_pt.basicPoint(), &geom_pt32);
+    deprecated::toGeomMsgPt32(ll_pt.basicPoint(), &geom_pt32);
     geom_poly->points.push_back(geom_pt32);
   }
 }

autoware_lanelet2_extension/lib/utilities.cpp

@@ -674,7 +674,7 @@ lanelet::ArcCoordinates getArcCoordinates(
   for (const auto & llt : lanelet_sequence) {
     const auto & centerline_2d = lanelet::utils::to2D(llt.centerline());
     if (llt == closest_lanelet) {
-      const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(pose.position);
+      const auto lanelet_point = deprecated::toLaneletPoint(pose.position);
       arc_coordinates = lanelet::geometry::toArcCoordinates(
         centerline_2d, lanelet::utils::to2D(lanelet_point).basicPoint());
       arc_coordinates.length += length;
@@ -704,7 +704,7 @@ lanelet::ArcCoordinates getArcCoordinatesOnEgoCenterline(
     }
 
     if (llt == closest_lanelet) {
-      const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(pose.position);
+      const auto lanelet_point = deprecated::toLaneletPoint(pose.position);
       arc_coordinates = lanelet::geometry::toArcCoordinates(
         centerline_2d, lanelet::utils::to2D(lanelet_point).basicPoint());
       arc_coordinates.length += length;
@@ -841,7 +841,7 @@ double getLateralDistanceToCenterline(
   const lanelet::ConstLanelet & lanelet, const geometry_msgs::msg::Pose & pose)
 {
   const auto & centerline_2d = lanelet::utils::to2D(lanelet.centerline());
-  const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(pose.position);
+  const auto lanelet_point = deprecated::toLaneletPoint(pose.position);
   return lanelet::geometry::signedDistance(
     centerline_2d, lanelet::utils::to2D(lanelet_point).basicPoint());
 }