Feature: Creation of an Intensity Map

include/slam_toolbox/laser_utils.hpp

@@ -32,27 +32,39 @@ namespace laser_utils
 {
 
 // Convert a laser scan to a vector of readings
-inline std::vector<double> scanToReadings(
+inline void scanToReadings(
   const sensor_msgs::msg::LaserScan & scan,
+  std::vector<double> & ranges,
+  std::vector<double> & intensities,
   const bool & inverted)
 {
-  std::vector<double> readings;
+
+  size_t n = scan.ranges.size();
+  ranges.resize(n);
+  intensities.resize(n);
+
+  bool has_intensities = scan.intensities.size() == n;
 
   if (inverted) {
-    for (std::vector<float>::const_reverse_iterator it = scan.ranges.rbegin();
-      it != scan.ranges.rend(); ++it)
-    {
-      readings.push_back(*it);
+    for (size_t i = 0; i < n; ++i) {
+      ranges[i] = static_cast<double>(scan.ranges[n - 1 - i]);
+      if (has_intensities){
+        intensities[i] = static_cast<double>(scan.intensities[n - 1 - i]);
+      }else{
+        intensities[i] = 0.0;
+      }
     }
   } else {
-    for (std::vector<float>::const_iterator it = scan.ranges.begin(); it != scan.ranges.end();
-      ++it)
-    {
-      readings.push_back(*it);
+    for (size_t i = 0; i < n; ++i) {
+      ranges[i] = static_cast<double>(scan.ranges[i]);
+      if (has_intensities){
+        intensities[i] = static_cast<double>(scan.intensities[i]);
+      }else{
+        intensities[i] = 0.0;
+      }
     }
   }
-
-  return readings;
+
 }
 
 // Store laser scanner information

include/slam_toolbox/map_saver.hpp

@@ -24,6 +24,10 @@
 #include <chrono>
 #include "rclcpp/rclcpp.hpp"
 #include "slam_toolbox/toolbox_msgs.hpp"
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/synchronizer.h>
+
 
 namespace map_saver
 {
@@ -32,20 +36,27 @@ namespace map_saver
 class MapSaver
 {
 public:
-  MapSaver(rclcpp::Node::SharedPtr node, const std::string & service_name);
+  MapSaver(rclcpp::Node::SharedPtr node, const std::string & topic_map_name, const std::string & topic_intensity_map_name);
 
 protected:
   bool saveMapCallback(
     const std::shared_ptr<rmw_request_id_t> request_header,
     const std::shared_ptr<slam_toolbox::srv::SaveMap::Request> request,
     std::shared_ptr<slam_toolbox::srv::SaveMap::Response> response);
+  void bothMapsCallback(
+    const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & map,
+    const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & intensity_map);
 
 private:
   rclcpp::Node::SharedPtr node_;
   rclcpp::Service<slam_toolbox::srv::SaveMap>::SharedPtr server_;
-  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr sub_;
-  std::string service_name_, map_name_;
-  bool received_map_;
+  std::shared_ptr<message_filters::Subscriber<nav_msgs::msg::OccupancyGrid>> map_sub_, intensity_map_sub_;
+
+  using SyncPolicy = message_filters::sync_policies::ApproximateTime<nav_msgs::msg::OccupancyGrid, nav_msgs::msg::OccupancyGrid>;
+  std::shared_ptr<message_filters::Synchronizer<SyncPolicy>> sync_;
+  nav_msgs::msg::OccupancyGrid last_map_, last_intensity_map_;
+  std::string topic_map_name_, topic_intensity_map_name_;
+  bool received_map_, received_intensity_map_;
 };
 
 }  // namespace map_saver

include/slam_toolbox/merge_maps_kinematic.hpp

@@ -72,7 +72,7 @@ class MergeMapsKinematic : public rclcpp::Node
     visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr feedback);
   void kartoToROSOccupancyGrid(
     const karto::LocalizedRangeScanVector & scans,
-    nav_msgs::srv::GetMap::Response & map);
+    nav_msgs::srv::GetMap::Response & occ_map,nav_msgs::srv::GetMap::Response & int_map);
   void transformScan(LocalizedRangeScansIt iter, tf2::Transform & submap_correction);
 
   // apply transformation to correct pose
@@ -84,6 +84,8 @@ class MergeMapsKinematic : public rclcpp::Node
   // ROS-y-ness
   std::vector<std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::MapMetaData>>> sstmS_;
   std::vector<std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>>> sstS_;
+  std::vector<std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::MapMetaData>>> sstmIS_;
+  std::vector<std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>>> sstIS_;
   std::shared_ptr<rclcpp::Service<slam_toolbox::srv::MergeMaps>> ssMap_;
   std::shared_ptr<rclcpp::Service<slam_toolbox::srv::AddSubmap>> ssSubmap_;
 
@@ -104,6 +106,8 @@ class MergeMapsKinematic : public rclcpp::Node
   double resolution_;
   int min_pass_through_;
   double occupancy_threshold_;
+  int min_intensity_counter_;
+  std::string intensity_strategy_;
   int num_submaps_;
 };
 

include/slam_toolbox/slam_mapper.hpp

@@ -62,6 +62,7 @@ class SMapper
 
 protected:
   std::unique_ptr<karto::Mapper> mapper_;
+
 };
 
 }  // namespace mapper_utils

include/slam_toolbox/slam_toolbox_common.hpp

@@ -134,14 +134,16 @@ class SlamToolbox : public rclcpp::Node
   std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>> scan_filter_;
   std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>> sst_;
   std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::MapMetaData>> sstm_;
+  std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>> intensity_map_pub_;
+  std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::MapMetaData>> intensity_metamap_pub_;
   std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>> pose_pub_;
   std::shared_ptr<rclcpp::Service<nav_msgs::srv::GetMap>> ssMap_;
   std::shared_ptr<rclcpp::Service<slam_toolbox::srv::Pause>> ssPauseMeasurements_;
   std::shared_ptr<rclcpp::Service<slam_toolbox::srv::SerializePoseGraph>> ssSerialize_;
   std::shared_ptr<rclcpp::Service<slam_toolbox::srv::DeserializePoseGraph>> ssDesserialize_;
 
   // Storage for ROS parameters
-  std::string odom_frame_, map_frame_, base_frame_, map_name_, scan_topic_;
+  std::string odom_frame_, map_frame_, base_frame_, map_topic_name_, scan_topic_, intensity_map_topic_name_;
   bool use_map_saver_;
   rclcpp::Duration transform_timeout_, minimum_time_interval_;
   std_msgs::msg::Header scan_header;

include/slam_toolbox/visualization_utils.hpp

@@ -102,38 +102,60 @@ inline visualization_msgs::msg::InteractiveMarker toInteractiveMarker(
 
 inline void toNavMap(
   const karto::OccupancyGrid * occ_grid,
-  nav_msgs::msg::OccupancyGrid & map)
+  nav_msgs::msg::OccupancyGrid & occupancy_map,
+  nav_msgs::msg::OccupancyGrid & intensity_map)
 {
   // Translate to ROS format
   kt_int32s width = occ_grid->GetWidth();
   kt_int32s height = occ_grid->GetHeight();
   karto::Vector2<kt_double> offset =
     occ_grid->GetCoordinateConverter()->GetOffset();
 
-  if (map.info.width != (unsigned int) width ||
-    map.info.height != (unsigned int) height ||
-    map.info.origin.position.x != offset.GetX() ||
-    map.info.origin.position.y != offset.GetY())
+  intensity_map.header = occupancy_map.header;
+  intensity_map.info = occupancy_map.info;
+
+  if (occupancy_map.info.width != (unsigned int) width ||
+    occupancy_map.info.height != (unsigned int) height ||
+    occupancy_map.info.origin.position.x != offset.GetX() ||
+    occupancy_map.info.origin.position.y != offset.GetY())
+  {
+    occupancy_map.info.origin.position.x = offset.GetX();
+    occupancy_map.info.origin.position.y = offset.GetY();
+    occupancy_map.info.width = width;
+    occupancy_map.info.height = height;
+    occupancy_map.data.resize(occupancy_map.info.width * occupancy_map.info.height);
+  }
+  if (intensity_map.info.width != (unsigned int) width ||
+    intensity_map.info.height != (unsigned int) height ||
+    intensity_map.info.origin.position.x != offset.GetX() ||
+    intensity_map.info.origin.position.y != offset.GetY())
   {
-    map.info.origin.position.x = offset.GetX();
-    map.info.origin.position.y = offset.GetY();
-    map.info.width = width;
-    map.info.height = height;
-    map.data.resize(map.info.width * map.info.height);
+    intensity_map.info.origin.position.x = offset.GetX();
+    intensity_map.info.origin.position.y = offset.GetY();
+    intensity_map.info.width = width;
+    intensity_map.info.height = height;
+    intensity_map.data.resize(intensity_map.info.width * intensity_map.info.height);
   }
+  intensity_map.data.assign(intensity_map.info.width * intensity_map.info.height, 0);
 
   for (kt_int32s y = 0; y < height; y++) {
     for (kt_int32s x = 0; x < width; x++) {
-      kt_int8u value = occ_grid->GetValue(karto::Vector2<kt_int32s>(x, y));
-      switch (value) {
+      kt_int8u occupancy = occ_grid->GetValue(karto::Vector2<kt_int32s>(x, y));      
+      kt_double intensity = occ_grid->GetCellIntensity(karto::Vector2<kt_int32s>(x, y));
+
+      switch (occupancy) {
         case karto::GridStates_Unknown:
-          map.data[MAP_IDX(map.info.width, x, y)] = -1;
+          occupancy_map.data[MAP_IDX(occupancy_map.info.width, x, y)] = -1;
           break;
         case karto::GridStates_Occupied:
-          map.data[MAP_IDX(map.info.width, x, y)] = 100;
+          occupancy_map.data[MAP_IDX(occupancy_map.info.width, x, y)] = 100;
+          if(intensity > 0){
+            int norm_intensity = std::clamp(static_cast<int>(std::round(100.0 * (intensity / 255))), 0, 100);
+            intensity_map.data[MAP_IDX(intensity_map.info.width, x, y)] = norm_intensity;
+          }
           break;
         case karto::GridStates_Free:
-          map.data[MAP_IDX(map.info.width, x, y)] = 0;
+          occupancy_map.data[MAP_IDX(occupancy_map.info.width, x, y)] = 0;
           break;
       }
     }

lib/karto_sdk/Authors

@@ -4,4 +4,5 @@ Contributors:
 -------------------------------
 Michael A. Eriksen ([email protected])
 Benson Limketkai ([email protected])
-Steven Macenski ([email protected])
+Steven Macenski ([email protected])
+Ángel Soriano ([email protected])