diff --git a/CMakeLists.txt b/CMakeLists.txt index 15a230f..15de12a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(ament_cmake REQUIRED) find_package(grid_map_msgs REQUIRED) find_package(grid_map_ros REQUIRED) find_package(grid_map_core REQUIRED) +find_package(tf2_ros REQUIRED) find_package(rclcpp REQUIRED) find_package(Eigen3 REQUIRED) @@ -49,6 +50,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC Eigen3::Eigen GDAL::GDAL) ament_target_dependencies(${PROJECT_NAME} PUBLIC grid_map_core grid_map_ros + tf2_ros ) add_executable(test_tif_loader diff --git a/include/grid_map_geo/grid_map_geo.hpp b/include/grid_map_geo/grid_map_geo.hpp index a1e145d..64d76ac 100644 --- a/include/grid_map_geo/grid_map_geo.hpp +++ b/include/grid_map_geo/grid_map_geo.hpp @@ -38,6 +38,7 @@ #include #include +#include "tf2_ros/transform_broadcaster.h" #if __APPLE__ #include @@ -176,6 +177,8 @@ class GridMapGeo { */ void AddLayerNormals(std::string reference_layer); + geometry_msgs::msg::TransformStamped static_transformStamped; + protected: grid_map::GridMap grid_map_; Location maporigin_; diff --git a/package.xml b/package.xml index b996158..988807d 100644 --- a/package.xml +++ b/package.xml @@ -16,6 +16,7 @@ eigen grid_map_core grid_map_ros + tf2_ros libgdal-dev rclcpp yaml_cpp_vendor diff --git a/src/grid_map_geo.cpp b/src/grid_map_geo.cpp index e072db3..9a33ecb 100644 --- a/src/grid_map_geo.cpp +++ b/src/grid_map_geo.cpp @@ -43,8 +43,7 @@ #include #include -#include -#include +#include "geometry_msgs/msg/transform_stamped.hpp" GridMapGeo::GridMapGeo(const std::string frame_id) { frame_id_ = frame_id; } @@ -102,6 +101,16 @@ bool GridMapGeo::initializeFromGeotiff(const std::string &path) { maporigin_.espg = ESPG::CH1903_LV03; maporigin_.position = Eigen::Vector3d(mapcenter_e, mapcenter_n, 0.0); + static_transformStamped.header.frame_id = name_coordinate; + static_transformStamped.child_frame_id = frame_id_; + static_transformStamped.transform.translation.x = mapcenter_e; + static_transformStamped.transform.translation.y = mapcenter_n; + static_transformStamped.transform.translation.z = 0.0; + static_transformStamped.transform.rotation.x = 0.0; + static_transformStamped.transform.rotation.y = 0.0; + static_transformStamped.transform.rotation.z = 0.0; + static_transformStamped.transform.rotation.w = 1.0; + Eigen::Vector2d position{Eigen::Vector2d::Zero()}; grid_map_.setGeometry(length, resolution, position); @@ -123,21 +132,6 @@ bool GridMapGeo::initializeFromGeotiff(const std::string &path) { layer_elevation(x, y) = data[gridMapIndex(0) + width * gridMapIndex(1)]; } - static tf2_ros::StaticTransformBroadcaster static_broadcaster; - geometry_msgs::TransformStamped static_transformStamped; - - static_transformStamped.header.stamp = ros::Time::now(); - static_transformStamped.header.frame_id = name_coordinate; - static_transformStamped.child_frame_id = frame_id_; - static_transformStamped.transform.translation.x = mapcenter_e; - static_transformStamped.transform.translation.y = mapcenter_n; - static_transformStamped.transform.translation.z = 0.0; - static_transformStamped.transform.rotation.x = 0.0; - static_transformStamped.transform.rotation.y = 0.0; - static_transformStamped.transform.rotation.z = 0.0; - static_transformStamped.transform.rotation.w = 1.0; - static_broadcaster.sendTransform(static_transformStamped); - return true; } diff --git a/src/test_tif_loader.cpp b/src/test_tif_loader.cpp index c920891..d28e845 100644 --- a/src/test_tif_loader.cpp +++ b/src/test_tif_loader.cpp @@ -59,7 +59,7 @@ class MapPublisher : public rclcpp::Node { RCLCPP_INFO_STREAM(get_logger(), "color_path " << color_path); map_ = std::make_shared(); - map_->Load(file_path, false, color_path); + map_->Load(file_path, color_path); timer_ = this->create_wall_timer(5s, std::bind(&MapPublisher::timer_callback, this)); } @@ -67,10 +67,14 @@ class MapPublisher : public rclcpp::Node { void timer_callback() { auto msg = grid_map::GridMapRosConverter::toMessage(map_->getGridMap()); original_map_pub_->publish(std::move(msg)); + tf_broadcaster_ = std::make_unique(*this); + map_->static_transformStamped.header.stamp = this->get_clock()->now(); + tf_broadcaster_->sendTransform(map_->static_transformStamped); } rclcpp::TimerBase::SharedPtr timer_; rclcpp::Publisher::SharedPtr original_map_pub_; std::shared_ptr map_; + std::unique_ptr tf_broadcaster_; }; int main(int argc, char **argv) {