Fix variable size grid bugs

launch/multirobot.launch

@@ -24,7 +24,7 @@
   <param name="map_size" type="int" value="$(arg map_size)" />  
 
   <!-- ## Set the cell size TO USE in the occupancy grid  -->
-  <arg name="cell_size" default="0.75" /> 
+  <arg name="cell_size" default="0.25" /> 
   <param name="cell_size" type="double" value="$(arg cell_size)" />  
 
   <!-- ## Set TO MATCH the robot speed  -->

src/CentralModule/CentralModuleNode.cpp

@@ -16,8 +16,10 @@ using namespace std;
 ///          after the first robot request
 ///   *  2 : enabled, timeout , delay the auction start to wait for the robots expected to arrive soon (estimated with gvd construction time)
 ///          after the first robot request. Reset and decrease the delay on new requests.
-int auctionStartTimeoutMode = -1;
-
+///
+///   if the time to contruct the bids is grater than 4 seconds all the modes act like the mode 2
+int auctionStartTimeoutModeParam = -1;
+int auctionStartTimeoutMode = auctionStartTimeoutModeParam;
 // mapUpdateDelay: The expected delay of a map update to arrive from the robot to the central module
 float mapUpdateDelay = 2;
 
@@ -258,6 +260,12 @@ void startAuction() {
   gvdTime = (ros::Time::now() - lastGvdStart);
   gvdTimeIncrement = max(gvdTimeIncrement, gvdTime - lastGvdTime);
 
+  if(gvdTime.toSec() > 4){
+    auctionStartTimeoutMode = 2;
+  }else if(auctionStartTimeoutModeParam < 2){
+    auctionStartTimeoutMode = auctionStartTimeoutModeParam;
+  }
+
   // set markers for rviz
   setRvizMarks(auction, centralModule.occupancyGrid.info);
 
@@ -554,7 +562,7 @@ int main(int argc, char* argv[]) {
   n.param<int>("/critical_condition_min", GvdConfig::get()->criticalConditionMin, GvdConfig::get()->criticalConditionMin);
 
   /// Auction
-  n.param<int>("/auction_start_timeout_mode", auctionStartTimeoutMode, auctionStartTimeoutMode);
+  n.param<int>("/auction_start_timeout_mode", auctionStartTimeoutModeParam, auctionStartTimeoutModeParam);
   n.param<float>("/map_update_delay", mapUpdateDelay, mapUpdateDelay);
 
   // Frontier

src/MapMerger/MapMerger.cpp

@@ -16,6 +16,10 @@ bool isUnknown(int8_t data){
   return data == -1;
 }
 
+bool isOccupied(int8_t data){
+  return data >= 50;
+}
+
 bool MapMerger::isInitialized(){
   return !mapsArrived.empty();
 }
@@ -56,7 +60,7 @@ void MapMerger::mergeMap(const OccupancyGridConstPtr& msg, string name) {
       } else {
         // Skip if there is no line of vision from the update index to the robot (on the update grid)
         Pos globalPos = toPos(globalInd, msg->info.width); 
-        if(unobstructedLine(robotGlobalPos, globalPos, msg->data, msg->info.width)){ 
+        if(isOccupied(msg->data[globalInd]) || unobstructedLine(robotGlobalPos, globalPos, msg->data, msg->info.width)){ 
           mapMerged.data[globalInd] = round(decay*msg->data[globalInd] + (1-decay)*mapMerged.data[globalInd]);
         }
       }
@@ -86,7 +90,7 @@ OccupancyGridUpdate MapMerger::mergeMapUpdate(const OccupancyGridUpdateConstPtr&
         mapMerged.data[globalInd] = round(decay*update->data[updateInd] + (1-decay)*50);
       } else {
         // Skip if there is no line of vision from the update index to the robot (on the update grid)
-        if(unobstructedLine(robotUpdatePos, updatePos, update->data, update->width)){
+        if(isOccupied(update->data[updateInd]) || unobstructedLine(robotUpdatePos, updatePos, update->data, update->width)){
           mapMerged.data[globalInd] = round(decay*update->data[updateInd] + (1-decay)*mapMerged.data[globalInd]);
         }
       }