diff --git a/auv_navigation/auv_mapping/CMakeLists.txt b/auv_navigation/auv_mapping/CMakeLists.txt index 28c84b95..fc769572 100644 --- a/auv_navigation/auv_mapping/CMakeLists.txt +++ b/auv_navigation/auv_mapping/CMakeLists.txt @@ -9,6 +9,7 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) find_package(catkin REQUIRED roscpp + tf tf2 tf2_ros geometry_msgs diff --git a/auv_navigation/auv_mapping/include/auv_mapping/object_map_tf_server_ros.hpp b/auv_navigation/auv_mapping/include/auv_mapping/object_map_tf_server_ros.hpp index ded6e90c..57258e93 100644 --- a/auv_navigation/auv_mapping/include/auv_mapping/object_map_tf_server_ros.hpp +++ b/auv_navigation/auv_mapping/include/auv_mapping/object_map_tf_server_ros.hpp @@ -18,7 +18,7 @@ class ObjectMapTFServerROS { public: ObjectMapTFServerROS(const ros::NodeHandle &nh) : nh_{nh}, - tf_listener_{std::make_shared()}, + tf_listener_{nh_}, rate_{10.0}, static_frame_{""}, transforms_{}, @@ -125,16 +125,12 @@ class ObjectMapTFServerROS { std::optional get_transform( const std::string &target_frame, const std::string &source_frame, const ros::Duration timeout) { - if (!tf_listener) { - return std::nullopt; - } - try { - tf_listener_->waitForTransform(target_frame, source_frame, ros::Time(0), - timeout); + tf_listener_.waitForTransform(target_frame, source_frame, ros::Time(0), + timeout); tf::StampedTransform tf_transform; - tf_listener_->lookupTransform(target_frame, source_frame, ros::Time(0), - tf_transform); + tf_listener_.lookupTransform(target_frame, source_frame, ros::Time(0), + tf_transform); return tf_transform; } catch (tf::TransformException &ex) { return std::nullopt; @@ -142,7 +138,7 @@ class ObjectMapTFServerROS { } ros::NodeHandle nh_; - std::shared_ptr tf_listener_; + tf::TransformListener tf_listener_; double rate_; std::string static_frame_; TransformMap transforms_; @@ -152,4 +148,4 @@ class ObjectMapTFServerROS { ros::ServiceServer service_; }; -} // namespace auv_mapping \ No newline at end of file +} // namespace auv_mapping