From 6f7fa76fbe8f67c513d91353975587788189f569 Mon Sep 17 00:00:00 2001
From: Lisa <robbie@uni-koblenz.de>
Date: Thu, 19 Jan 2017 21:11:52 +0100
Subject: [PATCH] dynamic masking map
---
homer_map_manager/CMakeLists.txt | 2 ++
.../Managers/MaskingManager.h | 2 +-
homer_map_manager/src/Managers/MapManager.cpp | 6 ------
.../src/Managers/MaskingManager.cpp | 11 +++++++----
homer_map_manager/src/map_manager_node.cpp | 3 ++-
.../src/OccupancyMap/OccupancyMap.cpp | 18 +++++++++---------
6 files changed, 21 insertions(+), 21 deletions(-)
diff --git a/homer_map_manager/CMakeLists.txt b/homer_map_manager/CMakeLists.txt
index c659d0ab..ce519e8f 100644
--- a/homer_map_manager/CMakeLists.txt
+++ b/homer_map_manager/CMakeLists.txt
@@ -5,6 +5,8 @@ find_package(catkin REQUIRED COMPONENTS roscpp roslib tf homer_mapnav_msgs homer
find_package( Eigen3 REQUIRED )
+set(CMAKE_BUILD_TYPE Release)
+
find_package(OpenMP)
if (OPENMP_FOUND)
set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
diff --git a/homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h b/homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h
index 1947da82..a7b3c7c9 100644
--- a/homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h
+++ b/homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h
@@ -23,7 +23,7 @@ class MaskingManager {
/** @brief The destructor. */
virtual ~MaskingManager();
- void updateMapInfo(const nav_msgs::MapMetaData &mapInfo);
+ nav_msgs::OccupancyGrid::ConstPtr updateMapInfo(const nav_msgs::MapMetaData &mapInfo);
/** modifies either the masking layer or the slam layer (accordingly to the
* given map layer in the msg */
diff --git a/homer_map_manager/src/Managers/MapManager.cpp b/homer_map_manager/src/Managers/MapManager.cpp
index 8e3e5fe2..6368ff30 100644
--- a/homer_map_manager/src/Managers/MapManager.cpp
+++ b/homer_map_manager/src/Managers/MapManager.cpp
@@ -52,12 +52,6 @@ MapManager::~MapManager()
void MapManager::updateMapLayer(int type, nav_msgs::OccupancyGrid::ConstPtr layer)
{
- //TODO possible crash
- if(m_MapLayers[type]->info.width != layer->info.width)
- {
-
- ROS_INFO_STREAM("Map_manager: map size changed!");
- }
m_MapLayers[type] = layer;
if(type == homer_mapnav_msgs::MapLayers::SLAM_LAYER)
{
diff --git a/homer_map_manager/src/Managers/MaskingManager.cpp b/homer_map_manager/src/Managers/MaskingManager.cpp
index 4ee2ff60..2ee1dd73 100644
--- a/homer_map_manager/src/Managers/MaskingManager.cpp
+++ b/homer_map_manager/src/Managers/MaskingManager.cpp
@@ -17,16 +17,17 @@ MaskingManager::MaskingManager(nav_msgs::MapMetaData mapInfo) {
MaskingManager::~MaskingManager() {}
-void MaskingManager::updateMapInfo(const nav_msgs::MapMetaData &mapInfo) {
- if (m_SlamMap.info.width != mapInfo.width) {
+nav_msgs::OccupancyGrid::ConstPtr MaskingManager::updateMapInfo(const nav_msgs::MapMetaData &mapInfo) {
+ if (m_SlamMap.info.width < mapInfo.width || m_SlamMap.info.height < mapInfo.height) {
m_SlamMap.info = mapInfo;
int x_add_left =
- (m_MaskingMap.info.origin.position.x - mapInfo.origin.position.x) /
+ (m_MaskingMap.info.origin.position.x - mapInfo.origin.position.x + 0.025) /
mapInfo.resolution;
int y_add_up =
- (m_MaskingMap.info.origin.position.y - mapInfo.origin.position.y) /
+ (m_MaskingMap.info.origin.position.y - mapInfo.origin.position.y + 0.025) /
mapInfo.resolution;
+ ROS_INFO_STREAM("x add "<<x_add_left<<" y add "<<y_add_up);
std::vector<signed char> tmpData = m_MaskingMap.data;
@@ -43,11 +44,13 @@ void MaskingManager::updateMapInfo(const nav_msgs::MapMetaData &mapInfo) {
}
m_MaskingMap.info = mapInfo;
}
+ return boost::make_shared<const ::nav_msgs::OccupancyGrid>(m_MaskingMap);
}
nav_msgs::OccupancyGrid::ConstPtr MaskingManager::modifyMap(
homer_mapnav_msgs::ModifyMap::ConstPtr msg) {
// reset SLAM mask map before each masking
+ m_SlamMap.data.resize(m_SlamMap.info.height * m_SlamMap.info.width);
std::fill(m_SlamMap.data.begin(), m_SlamMap.data.end(),
homer_mapnav_msgs::ModifyMap::NOT_MASKED);
drawPolygon(msg->region, msg->maskAction, msg->mapLayer);
diff --git a/homer_map_manager/src/map_manager_node.cpp b/homer_map_manager/src/map_manager_node.cpp
index 1060e1a3..185dce9f 100644
--- a/homer_map_manager/src/map_manager_node.cpp
+++ b/homer_map_manager/src/map_manager_node.cpp
@@ -132,8 +132,9 @@ MapManagerNode::~MapManagerNode() {
void MapManagerNode::callbackSLAMMap(
const nav_msgs::OccupancyGrid::ConstPtr& msg) {
+ nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MaskingManager->updateMapInfo(msg->info);
+ m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER, maskingMap);
m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER, msg);
- m_MaskingManager->updateMapInfo(msg->info);
}
void MapManagerNode::callbackRapidMap(
diff --git a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
index 464a914b..a1f8b468 100644
--- a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
+++ b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
@@ -818,15 +818,15 @@ void OccupancyMap::applyChanges()
if ( m_CurrentChanges[i] == ::OCCUPIED && m_OccupancyCount[i] < USHRT_MAX )
{
- if(m_MeasurementCount[x + m_metaData.width * (y+1)] > 1)
- m_MeasurementCount[x + m_metaData.width * (y+1)]++;
- if(m_MeasurementCount[x + m_metaData.width * (y-1)] > 1)
- m_MeasurementCount[x + m_metaData.width * (y-1)]++;
- if(m_MeasurementCount[i-1] > 1)
- m_MeasurementCount[i-1]++;
- if(m_MeasurementCount[i+1] > 1)
- m_MeasurementCount[i+1]++;
- m_OccupancyCount[i] += 4;
+ //if(m_MeasurementCount[x + m_metaData.width * (y+1)] > 1)
+ //m_MeasurementCount[x + m_metaData.width * (y+1)]++;
+ //if(m_MeasurementCount[x + m_metaData.width * (y-1)] > 1)
+ //m_MeasurementCount[x + m_metaData.width * (y-1)]++;
+ //if(m_MeasurementCount[i-1] > 1)
+ //m_MeasurementCount[i-1]++;
+ //if(m_MeasurementCount[i+1] > 1)
+ //m_MeasurementCount[i+1]++;
+ m_OccupancyCount[i] ++;
}
}
}
--
GitLab