Dynamic map published by Maps manager

Author: juanscelyg

easynav_pointcloud_common/include/easynav_pointcloud_common/PointCloudData.hpp

@@ -127,7 +127,7 @@ class PointCloudData
    */
   bool load_from_file(const std::string & path);
 
-  void show(void) const;
+  void show(const std::string & comment) const;
 
 private:
   std::size_t width_;

easynav_pointcloud_common/src/easynav_pointcloud_common/PointCloudData.cpp

@@ -116,7 +116,6 @@ PointCloudData::refresh(
 void
 PointCloudData::to_point_cloud(sensor_msgs::msg::PointCloud2 & cloud_msg) const
 {
-  std::cerr << "Getting Data from msg\n";
   cloud_msg.width = static_cast<uint32_t>(width_);
   cloud_msg.height = static_cast<uint32_t>(height_);
   cloud_msg.point_step = static_cast<uint32_t>(point_step_);
@@ -185,9 +184,10 @@ PointCloudData::load_from_file(const std::string & path)
 }
 
 void
-PointCloudData::show(void) const
+PointCloudData::show(const std::string & comment) const
 {
   std::cerr << ":: PointCloudData values :: \n";
+  std::cerr << "Comment: " << comment << "\n";
   std::cerr << "width: " << width_ << "\n";
   std::cerr << "height: " << height_ << "\n";
   std::cerr << "point step: " << point_step_ << "\n";

easynav_pointcloud_maps_manager/CMakeLists.txt

@@ -12,6 +12,7 @@ set(CMAKE_CXX_EXTENSIONS OFF)
 find_package(ament_cmake REQUIRED)
 find_package(easynav_common REQUIRED)
 find_package(easynav_core REQUIRED)
+find_package(tf2_ros REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(ament_index_cpp REQUIRED)
 find_package(easynav_pointcloud_common REQUIRED)
@@ -23,11 +24,12 @@ add_library(${PROJECT_NAME} SHARED
 )
 target_include_directories(${PROJECT_NAME} PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
 )
 target_link_libraries(${PROJECT_NAME} PUBLIC
   easynav_common::easynav_common
   easynav_core::easynav_core
+  tf2_ros::tf2_ros
   pluginlib::pluginlib
   ament_index_cpp::ament_index_cpp
   easynav_pointcloud_common::easynav_pointcloud_common

easynav_pointcloud_maps_manager/src/easynav_pointcloud_maps_manager/PointCloudMapsManager.cpp

@@ -55,36 +55,31 @@ PointCloudMapsManager::on_initialize()
   auto node = get_node();
   const auto & plugin_name = get_plugin_name();
 
-  std::string package_name, map_path_file, map_topic_in;
-  node->declare_parameter(plugin_name + ".package", package_name);
+  std::string map_path_file, map_topic_in;
   node->declare_parameter(plugin_name + ".map_path_file", map_path_file);
   node->declare_parameter(plugin_name + ".map_topic_in", map_topic_in);
 
-  node->get_parameter(plugin_name + ".package", package_name);
   node->get_parameter(plugin_name + ".map_path_file", map_path_file);
   node->get_parameter(plugin_name + ".map_topic_in", map_topic_in);
 
   std::cerr << ":: PARAMETERS :: \n";
-  std::cerr << "Package name: " << package_name << "\n";
   std::cerr << "Map path file: " << map_path_file << "\n";
   std::cerr << "map topic in name: " << map_topic_in << "\n";
 
-  if (package_name != "" && map_path_file != "") {
-    std::string pkgpath;
-    try {
-      pkgpath = ament_index_cpp::get_package_share_directory(package_name);
-      map_path_ = pkgpath + "/" + map_path_file;
-    } catch(ament_index_cpp::PackageNotFoundError & ex) {
-      return std::unexpected("Package " + package_name + " not found. Error: " + ex.what());
-    }
+  if (map_path_file != "") {
+    map_path_ = map_path_file;
 
     if (!static_map_.load_from_file(map_path_)) {
       return std::unexpected("File [" + map_path_ + "] not found");
     } else {
       RCLCPP_INFO(get_node()->get_logger(),
-        "PointCloudMapsManager::File read : Map loaded");
+        "PointCloudMapsManager : Map loaded from file");
       dynamic_map_.deep_copy(static_map_);
     }
+  } else {
+    map_path_ = "map.pcd";
+    RCLCPP_INFO(get_node()->get_logger(),
+      "PointCloudMapsManager: Map path file to save: %s", map_path_.c_str());
   }
 
   std::string topic_name;
@@ -94,13 +89,6 @@ PointCloudMapsManager::on_initialize()
     topic_name = node->get_name() + std::string("/") + plugin_name + "/topic_in";
   }
 
-  if (map_path_file == "") {
-    map_path_ = "map.PointCloudData";
-  }
-
-  RCLCPP_INFO(get_node()->get_logger(),
-      "PointCloudMapsManager::Map path file: %s", map_path_.c_str());
-
   static_map_pub_ = node->create_publisher<sensor_msgs::msg::PointCloud2>(
     node->get_name() + std::string("/") + plugin_name + "/static_map",
     rclcpp::QoS(1).transient_local().reliable());
@@ -113,9 +101,8 @@ PointCloudMapsManager::on_initialize()
     [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) {
 
       RCLCPP_INFO(get_node()->get_logger(),
-      "PointCloudMapsManager::topic_callback: reading map");
+      "PointCloudMapsManager: topic_callback: reading map");
       static_map_.from_point_cloud(*msg);
-      dynamic_map_.from_point_cloud(*msg);
 
       static_map_.to_point_cloud(static_map_msg_);
       static_map_msg_.header.frame_id = "map";
@@ -147,9 +134,13 @@ PointCloudMapsManager::on_initialize()
   static_map_.to_point_cloud(static_map_msg_);
   static_map_msg_.header.frame_id = "map";
   static_map_msg_.header.stamp = this->get_node()->now();
-
   static_map_pub_->publish(static_map_msg_);
 
+  dynamic_map_.to_point_cloud(dynamic_map_msg_);
+  dynamic_map_msg_.header.frame_id = "map";
+  dynamic_map_msg_.header.stamp = this->get_node()->now();
+  dynamic_map_pub_->publish(dynamic_map_msg_);
+
   return {};
 }
 
@@ -168,22 +159,22 @@ PointCloudMapsManager::set_dynamic_map(const PointCloudData & new_map)
 void
 PointCloudMapsManager::update(NavState & nav_state)
 {
-  RCLCPP_INFO(get_node()->get_logger(),"Updating");
-  dynamic_map_.deep_copy(static_map_);
 
-  if (!nav_state.has("points")) {
-    nav_state.set("map.static", static_map_);
-    nav_state.set("map.dynamic", dynamic_map_);
-    return;
-  }
+  dynamic_map_.deep_copy(static_map_);
 
-  const auto & perceptions = nav_state.get<PointPerceptions>("points");
+  if (nav_state.has("points")) {
+    const auto & perceptions = nav_state.get<PointPerceptions>("points");
 
-  auto fused = PointPerceptionsOpsView(perceptions)
-    .fuse("map")->as_points();
+    auto fused = PointPerceptionsOpsView(perceptions)
+      .fuse("map")->as_points();
 
-  dynamic_map_.refresh(dynamic_map_msg_, fused);
+    dynamic_map_.refresh(dynamic_map_msg_, fused);
+    dynamic_map_.show("dynamic fused");
+  }
+  nav_state.set("map.static", static_map_);
+  nav_state.set("map.dynamic", dynamic_map_);
 
+  dynamic_map_.to_point_cloud(dynamic_map_msg_);
   dynamic_map_msg_.header.frame_id = "map";
   dynamic_map_msg_.header.stamp = get_node()->now();
   dynamic_map_pub_->publish(dynamic_map_msg_);

robot_params/maps_manager.params.yaml

@@ -1,10 +1,9 @@
 maps_manager_node:
   ros__parameters:
     use_sim_time: true
-    map_types:
-      - PointCloudData
+    map_types: [PointCloudData]
     PointCloudData:
       plugin: easynav_pointcloud_maps_manager/PointCloudMapsManager
-      package: easynav_pointcloud_maps_manager
       map_topic_in: /map_builder/cloud_filtered 
+      map_path_file: /home/juanscelyg/ws/easynav_ws/src/easynav_outdoor_testcase/maps/map.pcd