Added Tf publisher

map_merge/CMakeLists.txt

@@ -46,6 +46,54 @@ else()
   message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
 endif()
 
+# Build Types
+set(CMAKE_BUILD_TYPE Debug)
+#     CACHE STRING "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel tsan asan lsan msan ubsan"
+#     FORCE)
+# set(CMAKE_BUILD_TYPE ${CMAKE_BUILD_TYPE}
+#     CACHE STRING "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel tsan asan lsan msan ubsan"
+#     FORCE)
+
+# # ThreadSanitizer
+# set(CMAKE_C_FLAGS_TSAN
+#     "-fsanitize=thread -g -O1"
+#     CACHE STRING "Flags used by the C compiler during ThreadSanitizer builds."
+#     FORCE)
+# set(CMAKE_CXX_FLAGS_TSAN
+#     "-fsanitize=thread -g -O1"
+#     CACHE STRING "Flags used by the C++ compiler during ThreadSanitizer builds."
+#     FORCE)
+
+# # AddressSanitize
+# set(CMAKE_C_FLAGS_ASAN
+#     "-fsanitize=address -fno-optimize-sibling-calls -fno-omit-frame-pointer -g -O1"
+#     CACHE STRING "Flags used by the C compiler during AddressSanitizer builds."
+#     FORCE)
+# set(CMAKE_CXX_FLAGS_ASAN
+#     "-fsanitize=address -fno-optimize-sibling-calls -fno-omit-frame-pointer -g -O1"
+#     CACHE STRING "Flags used by the C++ compiler during AddressSanitizer builds."
+#     FORCE)
+
+# # LeakSanitizer
+# set(CMAKE_C_FLAGS_LSAN
+#     "-fsanitize=leak -fno-omit-frame-pointer -g -O1"
+#     CACHE STRING "Flags used by the C compiler during LeakSanitizer builds."
+#     FORCE)
+# set(CMAKE_CXX_FLAGS_LSAN
+#     "-fsanitize=leak -fno-omit-frame-pointer -g -O1"
+#     CACHE STRING "Flags used by the C++ compiler during LeakSanitizer builds."
+#     FORCE)
+
+# # MemorySanitizer
+# set(CMAKE_C_FLAGS_MSAN
+#     "-fsanitize=memory -fno-optimize-sibling-calls -fsanitize-memory-track-origins=2 -fno-omit-frame-pointer -g -O2"
+#     CACHE STRING "Flags used by the C compiler during MemorySanitizer builds."
+#     FORCE)
+# set(CMAKE_CXX_FLAGS_MSAN
+#     "-fsanitize=memory -fno-optimize-sibling-calls -fsanitize-memory-track-origins=2 -fno-omit-frame-pointer -g -O2"
+#     CACHE STRING "Flags used by the C++ compiler during MemorySanitizer builds."
+#     FORCE)
+
 ## Specify additional locations of header files
 include_directories(
   ${catkin_INCLUDE_DIRS}

map_merge/include/combine_grids/merging_pipeline.h

@@ -43,6 +43,7 @@
 #include <nav_msgs/OccupancyGrid.h>
 
 #include <opencv2/core/utility.hpp>
+#include <opencv2/photo.hpp>
 
 namespace combine_grids
 {
@@ -69,6 +70,10 @@ class MergingPipeline
   std::vector<nav_msgs::OccupancyGrid::ConstPtr> grids_;
   std::vector<cv::Mat> images_;
   std::vector<cv::Mat> transforms_;
+  float result_map_width = 0.0;
+  float result_map_height = 0.0;
+  std::vector<cv::Rect> roi_info_;
+  cv::Rect complete_roi_;
 };
 
 template <typename InputIt>
@@ -88,8 +93,11 @@ void MergingPipeline::feed(InputIt grids_begin, InputIt grids_end)
       grids_.push_back(*it);
       /* convert to opencv images. it creates only a view for opencv and does
        * not copy or own actual data. */
-      images_.emplace_back((*it)->info.height, (*it)->info.width, CV_8UC1,
-                           const_cast<signed char*>((*it)->data.data()));
+      cv::Mat image_denoised;
+      cv::Mat image((*it)->info.height, (*it)->info.width, CV_8UC1,
+              const_cast<signed char*>((*it)->data.data()));
+          cv::fastNlMeansDenoising(image, image_denoised, 10, 9, 15);
+      images_.emplace_back(image_denoised);
     } else {
       grids_.emplace_back();
       images_.emplace_back();

map_merge/include/map_merge/map_merge.h

@@ -45,8 +45,10 @@
 
 #include <combine_grids/merging_pipeline.h>
 #include <geometry_msgs/Pose.h>
+#include <geometry_msgs/TransformStamped.h>
 #include <map_msgs/OccupancyGridUpdate.h>
 #include <nav_msgs/OccupancyGrid.h>
+#include <tf2_ros/transform_broadcaster.h>
 #include <ros/ros.h>
 #include <boost/thread.hpp>
 
@@ -56,6 +58,7 @@ struct MapSubscription {
   // protects consistency of writable_map and readonly_map
   // also protects reads and writes of shared_ptrs
   std::mutex mutex;
+  std::string map_frame;
 
   geometry_msgs::Transform initial_pose;
   nav_msgs::OccupancyGrid::Ptr writable_map;
@@ -74,12 +77,14 @@ class MapMerge
   double merging_rate_;
   double discovery_rate_;
   double estimation_rate_;
+  double publish_rate_;
   double confidence_threshold_;
   std::string robot_map_topic_;
   std::string robot_map_updates_topic_;
   std::string robot_namespace_;
   std::string world_frame_;
   bool have_initial_poses_;
+  bool publish_tf = true;
 
   // publishing
   ros::Publisher merged_map_publisher_;
@@ -92,6 +97,18 @@ class MapMerge
   combine_grids::MergingPipeline pipeline_;
   std::mutex pipeline_mutex_;
 
+  // publishing tfs
+  std::vector<std::string> map_frames_;
+  std::vector<geometry_msgs::TransformStamped> tf_transforms_;
+  tf2_ros::TransformBroadcaster tf_publisher_;
+  std::thread tf_thread_;             //  tf needs it own thread
+  std::atomic_flag tf_current_flag_;  // whether tf_transforms_ are up to date
+                                        // with transforms_
+
+
+  void publishTF();
+  std::vector<geometry_msgs::TransformStamped> stampTransforms(const std::vector<geometry_msgs::Transform>);
+
   std::string robotNameFromTopic(const std::string& topic);
   bool isRobotMapTopic(const ros::master::TopicInfo& topic);
   bool getInitPose(const std::string& name, geometry_msgs::Transform& pose);
@@ -108,6 +125,7 @@ class MapMerge
   void executetopicSubscribing();
   void executemapMerging();
   void executeposeEstimation();
+  void executeposePublishTf();
 
   void topicSubscribing();
   void mapMerging();

map_merge/launch/map_merge.launch

@@ -1,15 +1,14 @@
 <launch>
-<group ns="map_merge">
   <node pkg="multirobot_map_merge" type="map_merge" respawn="false" name="map_merge" output="screen">
     <param name="robot_map_topic" value="map"/>
     <param name="robot_namespace" value=""/>
     <param name="merged_map_topic" value="map"/>
     <param name="world_frame" value="world"/>
     <param name="known_init_poses" value="false"/>
-    <param name="merging_rate" value="0.5"/>
+    <param name="merging_rate" value="0.2"/>
     <param name="discovery_rate" value="0.05"/>
     <param name="estimation_rate" value="0.1"/>
-    <param name="estimation_confidence" value="1.0"/>
+    <param name="publish_rate" value="1.0"/>
+    <param name="estimation_confidence" value="0.6"/>
   </node>
-</group>
 </launch>

map_merge/src/combine_grids/grid_compositor.cpp

@@ -63,6 +63,7 @@ nav_msgs::OccupancyGrid::Ptr GridCompositor::compose(
 
   result_grid->info.width = static_cast<uint>(dst_roi.width);
   result_grid->info.height = static_cast<uint>(dst_roi.height);
+
   result_grid->data.resize(static_cast<size_t>(dst_roi.area()), -1);
   // create view for opencv pointing to newly allocated grid
   cv::Mat result(dst_roi.size(), CV_8S, result_grid->data.data());