diff --git a/tf2/CMakeLists.txt b/tf2/CMakeLists.txt index 7e0cddb86..b46415813 100644 --- a/tf2/CMakeLists.txt +++ b/tf2/CMakeLists.txt @@ -12,8 +12,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake_ros REQUIRED) -find_package(console_bridge_vendor REQUIRED) # Provides console_bridge 0.4.0 on platforms without it. -find_package(console_bridge REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(geometry_msgs REQUIRED) find_package(rcutils REQUIRED) @@ -30,7 +28,6 @@ target_link_libraries(tf2 PUBLIC ${geometry_msgs_TARGETS}) target_link_libraries(tf2 PRIVATE ${builtin_interfaces_TARGETS} - console_bridge::console_bridge rcutils::rcutils) # Causes the visibility macros to use dllexport rather than dllimport, @@ -112,7 +109,7 @@ if(BUILD_TESTING) endif() endif() -ament_export_dependencies(console_bridge geometry_msgs rcutils rosidl_runtime_cpp) +ament_export_dependencies(geometry_msgs rcutils rosidl_runtime_cpp) # Export old-style CMake variables ament_export_include_directories("include/${PROJECT_NAME}") diff --git a/tf2/include/tf2/buffer_core.h b/tf2/include/tf2/buffer_core.h index bc62c2dd1..6d75127da 100644 --- a/tf2/include/tf2/buffer_core.h +++ b/tf2/include/tf2/buffer_core.h @@ -50,6 +50,7 @@ #include "tf2/exceptions.h" #include "tf2/transform_storage.h" #include "tf2/visibility_control.h" +#include "rcutils/logging_macros.h" namespace tf2 { diff --git a/tf2/package.xml b/tf2/package.xml index 6590cdfd6..b424bc949 100644 --- a/tf2/package.xml +++ b/tf2/package.xml @@ -28,9 +28,7 @@ rosidl_runtime_cpp builtin_interfaces - console_bridge_vendor geometry_msgs - libconsole-bridge-dev rcutils ament_cmake_gtest diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index f0c61b3ce..4c270800e 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -42,7 +42,6 @@ #include "tf2/time_cache.h" #include "tf2/exceptions.h" -#include "console_bridge/console.h" #include "tf2/LinearMath/Quaternion.h" #include "tf2/LinearMath/Transform.h" #include "tf2/LinearMath/Vector3.h" @@ -90,7 +89,7 @@ void fillOrWarnMessageForInvalidFrame( if (error_msg != nullptr) { *error_msg = s; } else { - CONSOLE_BRIDGE_logWarn("%s", s.c_str()); + RCUTILS_LOG_WARN("%s", s.c_str()); } } @@ -206,7 +205,7 @@ bool BufferCore::setTransformImpl( bool error_exists = false; if (stripped_child_frame_id == stripped_frame_id) { - CONSOLE_BRIDGE_logError( + RCUTILS_LOG_ERROR( "TF_SELF_TRANSFORM: Ignoring transform from authority \"%s\" with frame_id and " "child_frame_id \"%s\" because they are the same", authority.c_str(), stripped_child_frame_id.c_str()); @@ -214,14 +213,14 @@ bool BufferCore::setTransformImpl( } if (stripped_child_frame_id.empty()) { - CONSOLE_BRIDGE_logError( + RCUTILS_LOG_ERROR( "TF_NO_CHILD_FRAME_ID: Ignoring transform from authority \"%s\" because child_frame_id not" " set ", authority.c_str()); error_exists = true; } if (stripped_frame_id.empty()) { - CONSOLE_BRIDGE_logError( + RCUTILS_LOG_ERROR( "TF_NO_FRAME_ID: Ignoring transform with child_frame_id \"%s\" from authority \"%s\" " "because frame_id not set", stripped_child_frame_id.c_str(), authority.c_str()); error_exists = true; @@ -232,7 +231,7 @@ bool BufferCore::setTransformImpl( std::isnan(transform_in.getRotation().x()) || std::isnan(transform_in.getRotation().y()) || std::isnan(transform_in.getRotation().z()) || std::isnan(transform_in.getRotation().w())) { - CONSOLE_BRIDGE_logError( + RCUTILS_LOG_ERROR( "TF_NAN_INPUT: Ignoring transform for child_frame_id \"%s\" from authority \"%s\" because" " of a nan value in the transform (%f %f %f) (%f %f %f %f)", stripped_child_frame_id.c_str(), authority.c_str(), @@ -251,7 +250,7 @@ bool BufferCore::setTransformImpl( QUATERNION_NORMALIZATION_TOLERANCE; if (!valid) { - CONSOLE_BRIDGE_logError( + RCUTILS_LOG_ERROR( "TF_DENORMALIZED_QUATERNION: Ignoring transform for child_frame_id \"%s\" from authority" " \"%s\" because of an invalid quaternion in the transform (%f %f %f %f)", stripped_child_frame_id.c_str(), authority.c_str(), @@ -289,7 +288,7 @@ bool BufferCore::setTransformImpl( frame_authority_[frame_number] = authority; } else { std::string stamp_str = displayTimePoint(stamp); - CONSOLE_BRIDGE_logWarn( + RCUTILS_LOG_WARN( "TF_OLD_DATA ignoring data from the past for frame %s at time %s according to authority" " %s\nPossible reasons are listed at http://wiki.ros.org/tf/Errors%%20explained", stripped_child_frame_id.c_str(), stamp_str.c_str(), authority.c_str()); @@ -678,7 +677,7 @@ void BufferCore::lookupTransformImpl( case tf2::TF2Error::TF2_LOOKUP_ERROR: throw LookupException(error_string); default: - CONSOLE_BRIDGE_logError("Unknown error code: %d", retval); + RCUTILS_LOG_ERROR("Unknown error code: %u", static_cast(retval)); assert(0); } } @@ -1480,7 +1479,7 @@ void BufferCore::_chainAsVector( case tf2::TF2Error::TF2_LOOKUP_ERROR: throw LookupException(error_string); default: - CONSOLE_BRIDGE_logError("Unknown error code: %d", retval); + RCUTILS_LOG_ERROR("Unknown error code: %u", static_cast(retval)); assert(0); } } @@ -1506,7 +1505,7 @@ void BufferCore::_chainAsVector( case tf2::TF2Error::TF2_LOOKUP_ERROR: throw LookupException(error_string); default: - CONSOLE_BRIDGE_logError("Unknown error code: %d", retval); + RCUTILS_LOG_ERROR("Unknown error code: %u", static_cast(retval)); assert(0); } }