Commit 0da07008 authored by Federico Ciuffardi's avatar Federico Ciuffardi
Browse files

Fix variable size grid bugs

parent 558e2f0a
......@@ -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 -->
......
......@@ -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
......
......@@ -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]);
}
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment