fix master map update

ETHZ-RobotX · May 23, 2024 · 1a39349 · 1a39349
1 parent 77cd5d6
commit 1a39349
Show file tree

Hide file tree

Showing 3 changed files with 50 additions and 19 deletions.
diff --git a/smb_exploration/launch/frontier/smb_rss_frontier_without_gmapping.launch b/smb_exploration/launch/frontier/smb_rss_frontier_without_gmapping.launch
@@ -16,7 +16,7 @@
 
 <!-- Frontier Exploration (explore_lite) -->
 <!-- Please refer to the following http://wiki.ros.org/explore_lite for details on the parameters-->
-<node pkg="explore_lite" type="explore" respawn="false" name="explore" output="screen">
+<!-- <node pkg="explore_lite" type="explore" respawn="false" name="explore" output="screen">
   <param name="robot_base_frame" value="$(arg base_frame)"/>
   <param name="costmap_topic" value="/exploration_map/global_costmap/costmap"/>
   <param name="costmap_updates_topic" value="map_updates"/>
@@ -28,6 +28,6 @@
   <param name="gain_scale" value="1.0"/>
   <param name="transform_tolerance" value="0.3"/>
   <param name="min_frontier_size" value="0.75"/>
-</node>
+</node> -->
 
 </launch>
diff --git a/smb_exploration/src/ExplorationMap.cpp b/smb_exploration/src/ExplorationMap.cpp
@@ -94,9 +94,6 @@ void ExplorationMap::scanCallback(const sensor_msgs::LaserScanConstPtr& scan)
 
 bool ExplorationMap::shouldUpdateBounds(const float min_x, const float min_y, const float max_x, const float max_y)
 {
-    ROS_INFO_STREAM("====== min_x " << min_x << " min_y " << min_y << " / max_x " << max_x << " max_y " << max_y);
-    ROS_INFO_STREAM("====== min_x_ " << min_x_ << " min_y_ " << min_y_ << " / max_x_ " << max_x_ << " max_y_ " << max_y_);
-
     return (min_x < min_x_) || (min_y < min_y_) || (max_x > max_x_) || (max_y > max_y_);
 }
 
@@ -131,7 +128,7 @@ void ExplorationMap::updateBounds(const float min_x, const float min_y, const fl
     int old_size_x = costmap_copy->getSizeInCellsX();
     int old_size_y = costmap_copy->getSizeInCellsY();
 
-    // TODO: resize the map (layeredCostmap &-> plugins)
+    // resize the map (layeredCostmap &-> plugins)
     int new_size_x = (new_max_x - new_min_x) / resolution_;
     int new_size_y = (new_max_y - new_min_y) / resolution_;
 
@@ -148,12 +145,27 @@ void ExplorationMap::updateBounds(const float min_x, const float min_y, const fl
         for (unsigned int j = 0; j < old_size_y; j++) {
             int new_i = i + x_offset;
             int new_j = j + y_offset;
-            // ROS_INFO_STREAM("====== old i, j " << i << " , " << j << " / new i, j " << new_i << " x " << new_j);            
+            // ROS_INFO_STREAM("====== old i, j " << i << " , " << j << " / new i, j " << new_i << " , " << new_j << " value : " << int(costmap_copy->getCost(i,j)));            
             // set to the obstacle layer
             obstacle_layer->setCost(new_i, new_j, costmap_copy->getCost(i,j));
         }
     }
 
+    // need to manually propagate the update to the master map, 
+    // since the update bound might not fully enclose the transcription and 
+    // the full transcription thus will not reflected in the next update in the main loop
+    costmap_2d::Costmap2D* new_costmap = costmap2d_ros_->getCostmap();
+    ROS_INFO_STREAM("====== old size " << old_size_x << " x " << old_size_y << " / new size " << new_size_x << " x " << new_size_y);
+    for (unsigned int i = 0; i < old_size_x; i++) {
+        for (unsigned int j = 0; j < old_size_y; j++) {
+            int new_i = i + x_offset;
+            int new_j = j + y_offset;
+            // ROS_INFO_STREAM("====== old i, j " << i << " , " << j << " / new i, j " << new_i << " , " << new_j << " value : " << int(costmap_copy->getCost(i,j)));            
+            // set to the master map
+            new_costmap->setCost(new_i, new_j, costmap_copy->getCost(i,j));
+        }
+    }
+
     // update the new map bounds
     min_x_ = new_min_x;
     min_y_ = new_min_y;

diff --git a/smb_opc/rviz/smb_vis.rviz b/smb_opc/rviz/smb_vis.rviz
@@ -9,7 +9,7 @@ Panels:
         - /SLAM1/Map1
         - /Object Detection1
       Splitter Ratio: 0.5
-    Tree Height: 372
+    Tree Height: 487
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -74,8 +74,8 @@ Visualization Manager:
             - Alpha: 1
               Autocompute Intensity Bounds: true
               Autocompute Value Bounds:
-                Max Value: 4.302511692047119
-                Min Value: -0.29804784059524536
+                Max Value: 2.26214599609375
+                Min Value: -0.21057188510894775
                 Value: true
               Axis: Z
               Channel Name: intensity
@@ -105,6 +105,8 @@ Visualization Manager:
           Displays:
             - Class: rviz/TF
               Enabled: true
+              Filter (blacklist): ""
+              Filter (whitelist): ""
               Frame Timeout: 15
               Frames:
                 All Enabled: true
@@ -132,6 +134,8 @@ Visualization Manager:
                   Value: true
                 rslidar:
                   Value: true
+                rslidar_base_link:
+                  Value: true
                 top:
                   Value: true
                 tracking_camera_link:
@@ -162,8 +166,9 @@ Visualization Manager:
                     imu:
                       {}
                   lidar_mount_link:
-                    rslidar:
-                      {}
+                    rslidar_base_link:
+                      rslidar:
+                        {}
                   rgb_camera_link:
                     rgb_camera_optical_link:
                       {}
@@ -242,6 +247,10 @@ Visualization Manager:
                   Alpha: 1
                   Show Axes: false
                   Show Trail: false
+                rslidar_base_link:
+                  Alpha: 1
+                  Show Axes: false
+                  Show Trail: false
                   Value: true
                 top:
                   Alpha: 1
@@ -547,6 +556,16 @@ Visualization Manager:
             {}
           Queue Size: 100
           Value: true
+        - Alpha: 0.699999988079071
+          Class: rviz/Map
+          Color Scheme: map
+          Draw Behind: false
+          Enabled: true
+          Name: Map
+          Topic: /exploration_map/global_costmap/costmap
+          Unreliable: false
+          Use Timestamp: false
+          Value: true
       Enabled: true
       Name: Exploration
     - Class: rviz/Group
@@ -645,12 +664,12 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1492
+  Height: 1536
   Hide Left Dock: false
-  Hide Right Dock: false
+  Hide Right Dock: true
   Image bounding Boxes:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000396000004cefc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e0000026c0000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001a0053004d00420050006f007700650072004d006f0074006f007201000002e600000134000000e000fffffffb0000001e0053004d00420050006f007700650072005000610079006c006f00610064010000042600000116000000e000ffffff00000001000001c3000004cefc0200000005fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006e000004ce0000013200fffffffb0000001200520061007700200049006d00610067006500000003a20000019a0000002600fffffffb000000280049006d00610067006500200062006f0075006e00640069006e006700200042006f007800650073000000032b000002110000002600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000ab00000005efc0100000002fb0000000800540069006d0065010000000000000ab0000006dc00fffffffb0000000800540069006d006501000000000000045000000000000000000000053f000004ce00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000002bf00000542fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ae000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001a0053004d00420050006f007700650072004d006f0074006f007201000002f1000001550000007300fffffffb0000001e0053004d00420050006f007700650072005000610079006c006f00610064010000044c000001330000007300ffffff00000001000001c300000542fc0200000005fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000542000000a400fffffffb0000001200520061007700200049006d00610067006500000003a20000019a0000001600fffffffb000000280049006d00610067006500200062006f0075006e00640069006e006700200042006f007800650073000000032b000002110000001600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000061c0000005efc0100000002fb0000000800540069006d006501000000000000061c000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000003570000054200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Raw Image:
     collapsed: false
   SMBPowerMotor:
@@ -664,7 +683,7 @@ Window Geometry:
   Tool Properties:
     collapsed: false
   Views:
-    collapsed: false
-  Width: 2736
-  X: 144
-  Y: 54
+    collapsed: true
+  Width: 1564
+  X: 0
+  Y: 27