Skip to content

Commit

Permalink
fix(autoware_auto_tf2): remove tf2 geometry function duplicated in tf…
Browse files Browse the repository at this point in the history
…2 geometry msgs (#5089)
  • Loading branch information
wep21 authored and TakaHoribe committed Sep 28, 2023
1 parent 93df1a1 commit 2943318
Show file tree
Hide file tree
Showing 5 changed files with 0 additions and 74 deletions.
6 changes: 0 additions & 6 deletions common/autoware_auto_geometry/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -12,12 +12,6 @@ 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
Expand Down
5 changes: 0 additions & 5 deletions common/autoware_auto_tf2/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -17,11 +17,6 @@ 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()
Original file line number Diff line number Diff line change
Expand Up @@ -45,57 +45,6 @@ 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 **/
/******************/
Expand Down
6 changes: 0 additions & 6 deletions planning/behavior_path_planner/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -58,12 +58,6 @@ 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}
)
Expand Down
6 changes: 0 additions & 6 deletions planning/surround_obstacle_checker/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -18,12 +18,6 @@ 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}
)
Expand Down

0 comments on commit 2943318

Please sign in to comment.