Remove tf2_geometry_msg changes

Signed-off-by: Maxime CLEMENT <[email protected]>

common/autoware_auto_geometry/CMakeLists.txt

@@ -12,6 +12,12 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/bounding_box.cpp
 )
 
+if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4)
+  target_compile_definitions(${PROJECT_NAME} PRIVATE
+    DEFINE_LEGACY_FUNCTION
+  )
+endif()
+
 if(BUILD_TESTING)
   set(GEOMETRY_GTEST geometry_gtest)
   set(GEOMETRY_SRC test/src/test_geometry.cpp

common/autoware_auto_tf2/CMakeLists.txt

@@ -17,6 +17,11 @@ if(BUILD_TESTING)
     "tf2_geometry_msgs"
     "tf2_ros"
   )
+  if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4)
+    target_compile_definitions(test_tf2_autoware_auto_msgs PRIVATE
+      DEFINE_LEGACY_FUNCTION
+    )
+  endif()
 endif()
 
 ament_auto_package()

common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp

@@ -45,6 +45,57 @@ using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox;
 
 namespace tf2
 {
+#ifdef DEFINE_LEGACY_FUNCTION
+/*************/
+/** Point32 **/
+/*************/
+
+/** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs Point32 type.
+ * This function is a specialization of the doTransform template defined in tf2/convert.h.
+ * \param t_in The point to transform, as a Point32 message.
+ * \param t_out The transformed point, as a Point32 message.
+ * \param transform The timestamped transform to apply, as a TransformStamped message.
+ */
+template <>
+inline void doTransform(
+  const geometry_msgs::msg::Point32 & t_in, geometry_msgs::msg::Point32 & t_out,
+  const geometry_msgs::msg::TransformStamped & transform)
+{
+  const KDL::Vector v_out = gmTransformToKDL(transform) * KDL::Vector(t_in.x, t_in.y, t_in.z);
+  t_out.x = static_cast<float>(v_out[0]);
+  t_out.y = static_cast<float>(v_out[1]);
+  t_out.z = static_cast<float>(v_out[2]);
+}
+
+/*************/
+/** Polygon **/
+/*************/
+
+/** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs Polygon type.
+ * This function is a specialization of the doTransform template defined in tf2/convert.h.
+ * \param t_in The polygon to transform.
+ * \param t_out The transformed polygon.
+ * \param transform The timestamped transform to apply, as a TransformStamped message.
+ */
+template <>
+inline void doTransform(
+  const geometry_msgs::msg::Polygon & t_in, geometry_msgs::msg::Polygon & t_out,
+  const geometry_msgs::msg::TransformStamped & transform)
+{
+  // Don't call the Point32 doTransform to avoid doing this conversion every time
+  const auto kdl_frame = gmTransformToKDL(transform);
+  // We don't use std::back_inserter to allow aliasing between t_in and t_out
+  t_out.points.resize(t_in.points.size());
+  for (size_t i = 0; i < t_in.points.size(); ++i) {
+    const KDL::Vector v_out =
+      kdl_frame * KDL::Vector(t_in.points[i].x, t_in.points[i].y, t_in.points[i].z);
+    t_out.points[i].x = static_cast<float>(v_out[0]);
+    t_out.points[i].y = static_cast<float>(v_out[1]);
+    t_out.points[i].z = static_cast<float>(v_out[2]);
+  }
+}
+#endif
+
 /******************/
 /** Quaternion32 **/
 /******************/

planning/behavior_path_planner/CMakeLists.txt

@@ -57,6 +57,12 @@ ament_auto_add_library(behavior_path_planner_node SHARED
   src/marker_utils/lane_change/debug.cpp
 )
 
+if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4)
+  target_compile_definitions(behavior_path_planner_node PRIVATE
+    DEFINE_LEGACY_FUNCTION
+  )
+endif()
+
 target_include_directories(behavior_path_planner_node SYSTEM PUBLIC
   ${EIGEN3_INCLUDE_DIR}
 )

planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp

@@ -26,15 +26,6 @@
 
 #include <boost/geometry.hpp>
 
-// for writing the svg file
-#include <fstream>
-#include <iostream>
-// for the geometry types
-#include <tier4_autoware_utils/geometry/geometry.hpp>
-// for the svg mapper
-#include <boost/geometry/io/svg/svg_mapper.hpp>
-#include <boost/geometry/io/svg/write.hpp>
-
 namespace drivable_area_expansion
 {
 
@@ -66,22 +57,6 @@ void expandDrivableArea(
 {
   if (path.points.empty() || path.left_bound.empty() || path.right_bound.empty()) return;
 
-  // Declare a stream and an SVG mapper
-  std::ofstream svg1("/home/mclement/Pictures/DA_before.svg");  // /!\ CHANGE PATH
-  boost::geometry::svg_mapper<tier4_autoware_utils::Point2d> before(svg1, 400, 400);
-  // Declare a stream and an SVG mapper
-  std::ofstream svg2("/home/mclement/Pictures/DA_after.svg");  // /!\ CHANGE PATH
-  boost::geometry::svg_mapper<tier4_autoware_utils::Point2d> after(svg2, 400, 400);
-
-  linestring_t left_ls;
-  linestring_t right_ls;
-  for (const auto & p : path.left_bound) left_ls.emplace_back(p.x, p.y);
-  for (const auto & p : path.right_bound) right_ls.emplace_back(p.x, p.y);
-  before.add(left_ls);
-  before.add(right_ls);
-  before.map(left_ls, "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2");
-  before.map(right_ls, "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2");
-
   tier4_autoware_utils::StopWatch<std::chrono::milliseconds> stopwatch;
   const auto & params = planner_data->drivable_area_expansion_parameters;
   const auto & dynamic_objects = *planner_data->dynamic_object;
@@ -151,15 +126,6 @@ void expandDrivableArea(
     std::printf("\tupdate: %2.2fms\n", update_duration);
     std::cout << std::endl;
   }
-
-  left_ls.clear();
-  right_ls.clear();
-  for (const auto & p : path.left_bound) left_ls.emplace_back(p.x, p.y);
-  for (const auto & p : path.right_bound) right_ls.emplace_back(p.x, p.y);
-  after.add(left_ls);
-  after.add(right_ls);
-  after.map(left_ls, "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2");
-  after.map(right_ls, "fill-opacity:0.3;fill:red;stroke:red;stroke-width:2");
 }
 
 point_t convert_point(const Point & p)

planning/surround_obstacle_checker/CMakeLists.txt

@@ -18,6 +18,12 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/node.cpp
 )
 
+if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4)
+  target_compile_definitions(${PROJECT_NAME} PRIVATE
+    DEFINE_LEGACY_FUNCTION
+  )
+endif()
+
 target_link_libraries(${PROJECT_NAME}
   ${PCL_LIBRARIES}
 )