reduce the dimensions of the storage map

autonomy_core/client/client_launch/rviz/client.rviz

@@ -8,8 +8,6 @@ Panels:
         - /Status1
         - /TF1/Frames1
         - /Env1/Environment1
-        - /Planner1
-        - /Maps1
         - /Maps1/Local1
         - /Estimation1/ground_truth_odom1/Shape1
         - /Estimation1/vio_corrected1/Shape1
@@ -20,7 +18,7 @@ Panels:
         - /Estimation1/graphslam1/Shape1
         - /Estimation1/ukf_odom1/Shape1
       Splitter Ratio: 0.5211267471313477
-    Tree Height: 323
+    Tree Height: 574
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -36,10 +34,9 @@ Panels:
     Name: Views
     Splitter Ratio: 0.5
   - Class: rviz/Time
-    Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: Local
+    SyncSource: StorageMap
 Preferences:
   PromptSaveOnExit: true
 Toolbars:
@@ -216,8 +213,8 @@ Visualization Manager:
         - Alpha: 1
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
-            Max Value: 10.125
-            Min Value: -0.125
+            Max Value: 3.625
+            Min Value: 0.125
             Value: true
           Axis: Z
           BoundScale: 0.5
@@ -279,7 +276,7 @@ Visualization Manager:
           Use Fixed Frame: true
           Use rainbow: true
           Value: true
-      Enabled: true
+      Enabled: false
       Name: Maps
     - Class: rviz/Group
       Displays:
@@ -485,6 +482,39 @@ Visualization Manager:
       Show Visual Aids: false
       Update Topic: /quadrotor/waypoint_nav/update
       Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 4.125
+        Min Value: 0.125
+        Value: true
+      Axis: Z
+      BoundScale: 0.5
+      Channel Name: intensity
+      Class: planning_rviz_plugins/VoxelMap
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      MeshAlpha: 0.019999999552965164
+      MeshColor: 0; 170; 255
+      MeshHeight: 0.5
+      Min Color: 0; 0; 0
+      Name: StorageMap
+      Position Transformer: XYZ
+      Queue Size: 1
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.5
+      State: Occupied
+      Style: Boxes
+      Topic: /quadrotor/mapper/storage_voxel_map
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 189; 234; 201
@@ -518,7 +548,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 60.19675827026367
+      Distance: 175.9504852294922
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -534,17 +564,17 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 1.1453958749771118
+      Pitch: 0.7797964811325073
       Target Frame: ground_truth/quadrotor/quadrotor
-      Yaw: 3.170363426208496
+      Yaw: 3.120365619659424
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 752
+  Height: 1136
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd0000000400000000000001d300000252fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000180000000c900fffffffb000000260057006100790070006f0069006e00740020004e0061007600690067006100740069006f006e01000001c3000000cc000000be00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000252000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000056a0000003efc0100000002fb0000000800540069006d006501000000000000056a000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003910000025200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001fc000003d2fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000027b000000c900fffffffb000000260057006100790070006f0069006e00740020004e0061007600690067006100740069006f006e01000002be00000151000000be00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000252000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000536000003d200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -555,6 +585,6 @@ Window Geometry:
     collapsed: true
   Waypoint Navigation:
     collapsed: false
-  Width: 1386
-  X: 274
-  Y: 277
+  Width: 1848
+  X: 72
+  Y: 27

autonomy_core/map_plan/mapper/include/mapper/local_global_mapper.h

@@ -13,6 +13,17 @@
 #include "mapper/tf_listener.h"
 #include "mapper/voxel_mapper.h"
 
+// Define storage map reinitialize direction
+#define CENTER 0
+#define RIGHT_BOTTOM 1
+#define RIGHT_TOP 2
+#define LEFT_BOTTOM 3
+#define LEFT_TOP 4
+#define LEFT 5
+#define RIGHT 6
+#define TOP 7
+#define BOTTOM 8
+
 // Timer stuff
 using boost::timer::cpu_timer;
 using boost::timer::cpu_times;
@@ -31,6 +42,12 @@ class LocalGlobalMapperNode {
    */
   void initParams();
 
+  /**
+   * @brief Check if storage map needs reinitialize
+   * 
+   */
+  int reinitializeStorageMapFlag(const Eigen::Vector3d& center_position_map);
+
   /**
    * @brief Crops the local map from the storage map, transforms it to odometry
    * frame and publishes
@@ -78,6 +95,12 @@ class LocalGlobalMapperNode {
    */
   void storageMapInit();
 
+  /**unique_ptr
+   * @brief Re allocates storage map
+   * 
+   */
+  void storageMapReInit(int direction);
+
   /**
    * @brief Initializes the array for inflation for the local map
    */
@@ -129,12 +152,14 @@ class LocalGlobalMapperNode {
   bool global_use_robot_dim_z_;
   double global_map_dim_d_x_, global_map_dim_d_y_, global_map_dim_d_z_;
   double local_map_dim_d_x_, local_map_dim_d_y_, local_map_dim_d_z_;
+  double storage_ori_offset_x_, storage_ori_offset_y_;
+  double prev_storage_center_x_, prev_storage_center_y_;
 
   double local_max_raycast_, global_max_raycast_;  // maximum raycasting range
   double occ_map_height_;
   Eigen::Vector3d local_ori_offset_;
   bool pub_storage_map_ =
-      false;  // don't set this as true unless you're debugging, it's very slow
+      true; //false;  // don't set this as true unless you're debugging, it's very slow
 
   int update_interval_;
   int counter_ = 0;

autonomy_core/map_plan/mapper/include/mapper/voxel_mapper.h

@@ -134,7 +134,7 @@ class VoxelMapper {
   vec_Vec3d getCloud();
 
   /**
-   * @brief Get a vector of points that are located at the center of occupied
+   * @brief Get a vector of points that are located at  ctheenter of occupied
    * voxels in the inflated map
    */
   vec_Vec3d getInflatedCloud();
@@ -185,6 +185,19 @@ class VoxelMapper {
    */
   void freeCloud(const vec_Vec3d& pts, const Eigen::Affine3d& tf);
 
+  mapper::VoxelMap get_voxel_map() {
+    return map_;
+  };
+
+  mapper::VoxelMap get_inflated_voxel_map() {
+    return inflated_map_;
+  };
+
+  bool reAllocate(const std::vector<signed char> old_map,
+                  const Eigen::Vector3d& new_ori_d,
+                  const Eigen::Vector3d& new_dim_d,
+                  int direction);
+
  private:
   /**
    * @brief Initialize a space for the map. If this method is called after

autonomy_core/map_plan/mapper/src/local_global_mapper.cpp

@@ -23,13 +23,27 @@ LocalGlobalMapperNode::LocalGlobalMapperNode(const ros::NodeHandle& nh)
   storage_map_info_.dim.z = local_map_info_.dim.z;
 
   // storage map should have same x y z center and x_dim y_dim as global map
-  storage_map_info_.origin.x = global_map_info_.origin.x;
-  storage_map_info_.origin.y = global_map_info_.origin.y;
-  storage_map_info_.origin.z = global_map_info_.origin.z;
+  /*
   storage_map_info_.dim.x = static_cast<int>(
       ceil((global_map_dim_d_x_) / storage_map_info_.resolution));
   storage_map_info_.dim.y = static_cast<int>(
       ceil((global_map_dim_d_y_) / storage_map_info_.resolution));
+  */
+
+  storage_map_info_.dim.x = static_cast<int>(
+      ceil((local_map_dim_d_x_) * 3 / local_map_info_.resolution));
+  storage_map_info_.dim.y = static_cast<int>(
+      ceil((local_map_dim_d_y_) * 3 / local_map_info_.resolution));
+
+  storage_ori_offset_x_ = - local_map_dim_d_x_ * 3 / 2;
+  storage_ori_offset_y_ = - local_map_dim_d_y_ * 3 / 2;
+
+  storage_map_info_.origin.x = local_map_info_.origin.x + storage_ori_offset_x_; // global_map_info_.origin.x;
+  storage_map_info_.origin.y = local_map_info_.origin.y + storage_ori_offset_y_; // global_map_info_.origin.y;
+  storage_map_info_.origin.z = global_map_info_.origin.z;
+
+  prev_storage_center_x_ = local_map_info_.origin.x;
+  prev_storage_center_y_ = local_map_info_.origin.y;
 
   local_map_info_.dim.x =
       static_cast<int>(ceil((local_map_dim_d_x_) / local_map_info_.resolution));
@@ -155,6 +169,7 @@ void LocalGlobalMapperNode::storageMapInit() {
       storage_map_info_.dim.z * storage_map_info_.resolution);
   const double res = storage_map_info_.resolution;
   int8_t storage_val_default = 0;
+
   // Initialize the mapper
   storage_voxel_mapper_.reset(
       new mapper::VoxelMapper(storage_origin,
@@ -164,6 +179,30 @@ void LocalGlobalMapperNode::storageMapInit() {
                               local_decay_times_to_empty_));
 }
 
+void LocalGlobalMapperNode::storageMapReInit(int direction) {
+  const Eigen::Vector3d storage_origin(storage_map_info_.origin.x,
+                                       storage_map_info_.origin.y,
+                                       storage_map_info_.origin.z);
+  const Eigen::Vector3d storage_dim_d(
+      storage_map_info_.dim.x * storage_map_info_.resolution,
+      storage_map_info_.dim.y * storage_map_info_.resolution,
+      storage_map_info_.dim.z * storage_map_info_.resolution);
+  const double res = storage_map_info_.resolution;
+  int8_t storage_val_default = 0;
+
+  std::vector<signed char> old_map = storage_voxel_mapper_->get_voxel_map().getMap();
+
+  storage_voxel_mapper_.reset(
+      new mapper::VoxelMapper(storage_origin,
+                              storage_dim_d,
+                              res,
+                              storage_val_default,
+                              local_decay_times_to_empty_));
+
+  storage_voxel_mapper_->reAllocate(old_map, storage_origin, storage_dim_d, direction);
+
+}
+
 void LocalGlobalMapperNode::localInflaInit() {
   const double res = storage_map_info_.resolution;
   local_infla_array_.clear();
@@ -262,6 +301,34 @@ void LocalGlobalMapperNode::getLidarPoses(
   }
 }
 
+int LocalGlobalMapperNode::reinitializeStorageMapFlag(const Eigen::Vector3d& center_position_map) {
+  // TODO: Check if we need to reinitialize the storage map
+  int x = center_position_map[0] - prev_storage_center_x_;
+  int y = center_position_map[1] - prev_storage_center_y_;
+  double thresh_x = local_map_dim_d_x_ * 0.8;
+  double thresh_y = local_map_dim_d_y_ * 0.8;
+
+  if (x < -thresh_x && y < -thresh_y) {
+    return RIGHT_BOTTOM;
+  } else if (x < -thresh_x && y > thresh_y) {
+    return RIGHT_TOP;
+  } else if (x > thresh_x && y < -thresh_y) {
+    return LEFT_BOTTOM;
+  } else if (x > thresh_x && y > thresh_y) {
+    return LEFT_TOP;
+  } else if (y < -thresh_y) {
+    return BOTTOM;
+  } else if (y > thresh_y) {
+    return TOP;
+  } else if (x > thresh_x) {
+    return LEFT;
+  } else if (x < -thresh_x) {
+    return RIGHT;
+  } else {
+    return CENTER;
+  }
+}
+
 void LocalGlobalMapperNode::processCloud(
     const sensor_msgs::PointCloud& cloud) {
   if ((storage_voxel_mapper_ == nullptr) || (global_voxel_mapper_ == nullptr)) {
@@ -303,6 +370,63 @@ void LocalGlobalMapperNode::processCloud(
   ROS_DEBUG("[storage map addCloud]: %f",
             static_cast<double>(timer.elapsed().wall) / 1e6);
 
+  // Reinitiliaze storage map to follow local map
+  int pos_to_storage_ori = reinitializeStorageMapFlag(lidar_position_map);
+
+  if (pos_to_storage_ori == LEFT_BOTTOM) {
+    // TODO: Reset storage map origin
+    prev_storage_center_x_ = prev_storage_center_x_ + local_map_dim_d_x_;
+    prev_storage_center_y_ = prev_storage_center_y_ - local_map_dim_d_y_;
+    storage_map_info_.origin.x = prev_storage_center_x_ + storage_ori_offset_x_; 
+    storage_map_info_.origin.y = prev_storage_center_y_ + storage_ori_offset_y_; 
+    // storageMapInit();    
+  } else if (pos_to_storage_ori == LEFT_TOP) {
+    // TODO: Reset storage map origin
+    prev_storage_center_x_ = prev_storage_center_x_ + local_map_dim_d_x_;
+    prev_storage_center_y_ = prev_storage_center_y_ + local_map_dim_d_y_;
+    storage_map_info_.origin.x = prev_storage_center_x_ + storage_ori_offset_x_; 
+    storage_map_info_.origin.y = prev_storage_center_y_ + storage_ori_offset_y_; 
+    // storageMapInit();    
+  } else if (pos_to_storage_ori == RIGHT_BOTTOM) {
+    // TODO: Reset storage map origin
+    prev_storage_center_x_ = prev_storage_center_x_ - local_map_dim_d_x_;
+    prev_storage_center_y_ = prev_storage_center_y_ - local_map_dim_d_y_;
+    storage_map_info_.origin.x = prev_storage_center_x_ + storage_ori_offset_x_; 
+    storage_map_info_.origin.y = prev_storage_center_y_ + storage_ori_offset_y_; 
+    // storageMapInit();    
+  } else if (pos_to_storage_ori == RIGHT_TOP) {
+    // TODO: Reset storage map origin
+    prev_storage_center_x_ = prev_storage_center_x_ - local_map_dim_d_x_;
+    prev_storage_center_y_ = prev_storage_center_y_ + local_map_dim_d_y_;
+    storage_map_info_.origin.x = prev_storage_center_x_+ storage_ori_offset_x_; 
+    storage_map_info_.origin.y = prev_storage_center_y_ + storage_ori_offset_y_; 
+    // storageMapInit();    
+  } else if (pos_to_storage_ori == LEFT) {
+    // TODO: Reset storage map origin
+    prev_storage_center_x_ = prev_storage_center_x_ + local_map_dim_d_x_;
+    storage_map_info_.origin.x = prev_storage_center_x_ + storage_ori_offset_x_; 
+    // storageMapInit();    
+    storageMapReInit(LEFT); 
+  } else if (pos_to_storage_ori == RIGHT) {
+    // TODO: Reset storage map origin
+    prev_storage_center_x_ = prev_storage_center_x_ - local_map_dim_d_x_;
+    storage_map_info_.origin.x = prev_storage_center_x_ + storage_ori_offset_x_; 
+    // storageMapInit();
+    storageMapReInit(RIGHT);    
+  } else if (pos_to_storage_ori == TOP) {
+    // TODO: Reset storage map origin
+    prev_storage_center_y_ = prev_storage_center_y_ + local_map_dim_d_y_;
+    storage_map_info_.origin.y = prev_storage_center_y_ + storage_ori_offset_y_; 
+    // storageMapInit();   
+    storageMapReInit(TOP);  
+  } else if (pos_to_storage_ori == BOTTOM) {
+    // TODO: Reset storage map origin
+    prev_storage_center_y_ = prev_storage_center_y_ - local_map_dim_d_y_;
+    storage_map_info_.origin.y = prev_storage_center_y_ + storage_ori_offset_y_; 
+    // storageMapReInit();   
+    storageMapReInit(BOTTOM);  
+  }
+
   // get and publish storage map (this is very slow)
   if (pub_storage_map_) {
     planning_ros_msgs::VoxelMap storage_map =