WIP transform stamped

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

include/grid_map_geo/grid_map_geo.hpp

@@ -38,6 +38,7 @@
 
 #include <grid_map_core/GridMap.hpp>
 #include <grid_map_core/iterators/GridMapIterator.hpp>
+#include "tf2_ros/transform_broadcaster.h"
 
 #if __APPLE__
 #include <cpl_string.h>
@@ -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_;

package.xml

@@ -16,6 +16,7 @@
   <depend>eigen</depend>
   <depend>grid_map_core</depend>
   <depend>grid_map_ros</depend>
+  <depend>tf2_ros</depend>
   <depend>libgdal-dev</depend>
   <depend>rclcpp</depend>
   <depend>yaml_cpp_vendor</depend>

src/grid_map_geo.cpp

@@ -43,8 +43,7 @@
 #include <grid_map_core/iterators/CircleIterator.hpp>
 #include <grid_map_core/iterators/GridMapIterator.hpp>
 
-#include <geometry_msgs/TransformStamped.h>
-#include <tf2_ros/static_transform_broadcaster.h>
+#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;
 }
 

src/test_tif_loader.cpp

@@ -59,18 +59,22 @@ class MapPublisher : public rclcpp::Node {
     RCLCPP_INFO_STREAM(get_logger(), "color_path " << color_path);
 
     map_ = std::make_shared<GridMapGeo>();
-    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));
   }
 
  private:
   void timer_callback() {
     auto msg = grid_map::GridMapRosConverter::toMessage(map_->getGridMap());
     original_map_pub_->publish(std::move(msg));
+    tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
+    map_->static_transformStamped.header.stamp = this->get_clock()->now();
+    tf_broadcaster_->sendTransform(map_->static_transformStamped);
   }
   rclcpp::TimerBase::SharedPtr timer_;
   rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr original_map_pub_;
   std::shared_ptr<GridMapGeo> map_;
+  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
 };
 
 int main(int argc, char **argv) {