Skip to content
Snippets Groups Projects
Commit 60972339 authored by Florian Polster's avatar Florian Polster
Browse files

masking manager dynamic map

parent 8da12a60
No related branches found
No related tags found
1 merge request!2Feature/dynamic map size
This commit is part of merge request !2. Comments created here will be created in the context of that merge request.
Expand all Hide whitespace changes
Inline Side-by-side
homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h
+ 17
− 17
View file @ 60972339
......@@ -3,31 +3,32 @@
#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/MapMetaData.h>
#include <homer_mapnav_msgs/ModifyMap.h>
#include <nav_msgs/MapMetaData.h>
#include <nav_msgs/OccupancyGrid.h>
#include <sstream>
/**
* @class MaskingManager
* @brief Manages a map that can overwrite values in the SLAM map or store it in a separate layer
* @brief Manages a map that can overwrite values in the SLAM map or store it
* in a separate layer
* @author Malte Knauf, David Gossow
*/
class MaskingManager
{
public:
class MaskingManager {
public:
/** @brief The constructor. */
MaskingManager(nav_msgs::MapMetaData mapInfo);
/** @brief The destructor. */
virtual ~MaskingManager();
void updateMapInfo(const nav_msgs::MapMetaData::ConstPtr mapInfo);
void updateMapInfo(const nav_msgs::MapMetaData &mapInfo);
/** modifies either the masking layer or the slam layer (accordingly to the given map layer in the msg */
nav_msgs::OccupancyGrid::ConstPtr modifyMap(homer_mapnav_msgs::ModifyMap::ConstPtr msg);
/** modifies either the masking layer or the slam layer (accordingly to the
* given map layer in the msg */
nav_msgs::OccupancyGrid::ConstPtr modifyMap(
homer_mapnav_msgs::ModifyMap::ConstPtr msg);
/** resets the masking map layer */
nav_msgs::OccupancyGrid::ConstPtr resetMap();
......@@ -35,19 +36,18 @@ class MaskingManager
/** replaces the masking map layer */
void replaceMap(nav_msgs::OccupancyGrid map);
private:
private:
/** stores the masking values in the dedicated masking map */
nav_msgs::OccupancyGrid m_MaskingMap;
/** stores the masking values that are afterwards sent to the slam map */
nav_msgs::OccupancyGrid m_SlamMap;
/** tools to draw masking polygons */
void drawPolygon ( std::vector< geometry_msgs::Point > vertices, int value, int mapLayer );
void drawLine ( std::vector<int> &data, int startX, int startY, int endX, int endY, int value );
void fillPolygon ( std::vector<int> &data, int x, int y, int value );
void drawPolygon(std::vector<geometry_msgs::Point> vertices, int value,
int mapLayer);
void drawLine(std::vector<int> &data, int startX, int startY, int endX,
int endY, int value);
void fillPolygon(std::vector<int> &data, int x, int y, int value);
};
#endif
homer_map_manager/src/Managers/MaskingManager.cpp
+ 138
− 147
View file @ 60972339
......@@ -2,190 +2,181 @@
using namespace std;
MaskingManager::MaskingManager(nav_msgs::MapMetaData mapInfo)
{
MaskingManager::MaskingManager(nav_msgs::MapMetaData mapInfo) {
m_MaskingMap.info = mapInfo;
m_MaskingMap.data.resize(m_MaskingMap.info.width * m_MaskingMap.info.height);
std::fill( m_MaskingMap.data.begin(), m_MaskingMap.data.end(), homer_mapnav_msgs::ModifyMap::NOT_MASKED );
m_MaskingMap.data.resize(m_MaskingMap.info.width *
m_MaskingMap.info.height);
std::fill(m_MaskingMap.data.begin(), m_MaskingMap.data.end(),
homer_mapnav_msgs::ModifyMap::NOT_MASKED);
m_SlamMap.info = mapInfo;
m_SlamMap.data.resize(m_SlamMap.info.width * m_SlamMap.info.height);
std::fill( m_SlamMap.data.begin(), m_SlamMap.data.end(), homer_mapnav_msgs::ModifyMap::NOT_MASKED );
std::fill(m_SlamMap.data.begin(), m_SlamMap.data.end(),
homer_mapnav_msgs::ModifyMap::NOT_MASKED);
}
MaskingManager::~MaskingManager()
{}
MaskingManager::~MaskingManager() {}
void MaskingManager::updateMapInfo(const nav_msgs::MapMetaData::ConstPtr mapInfo)
{
if(m_SlamMap.info.width != mapInfo.width)
{
void MaskingManager::updateMapInfo(const nav_msgs::MapMetaData &mapInfo) {
if (m_SlamMap.info.width != mapInfo.width) {
m_SlamMap.info = mapInfo;
std::vector<int> tmpData = m_MaskingMap.data;
int x_add_left =
(m_MaskingMap.info.origin.position.x - mapInfo.origin.position.x) /
mapInfo.resolution;
int y_add_up =
(m_MaskingMap.info.origin.position.y - mapInfo.origin.position.y) /
mapInfo.resolution;
std::vector<signed char> tmpData = m_MaskingMap.data;
m_MaskingMap.data.resize(mapInfo.width * mapInfo.height);
std::fill(m_MaskingMap.data.begin(), m_MaskingMap.data.end(), 0);
for(int x = 0; x < )
{
for(int y = 0;)
{
std::fill(m_MaskingMap.data.begin(), m_MaskingMap.data.end(),
homer_mapnav_msgs::ModifyMap::NOT_MASKED);
for (int x = 0; x < m_MaskingMap.info.width; x++) {
for (int y = 0; y < m_MaskingMap.info.height; y++) {
int i = y * m_MaskingMap.info.width + x;
int j = (y + y_add_up) * mapInfo.width + x + x_add_left;
m_MaskingMap.data[j] = tmpData[i];
}
}
m_MaskingMap.info = mapInfo;
m_MaskingMap.info = mapInfo;
}
}
nav_msgs::OccupancyGrid::ConstPtr MaskingManager::modifyMap(homer_mapnav_msgs::ModifyMap::ConstPtr msg)
{
//reset SLAM mask map before each masking
std::fill( m_SlamMap.data.begin(), m_SlamMap.data.end(), homer_mapnav_msgs::ModifyMap::NOT_MASKED );
nav_msgs::OccupancyGrid::ConstPtr MaskingManager::modifyMap(
homer_mapnav_msgs::ModifyMap::ConstPtr msg) {
// reset SLAM mask map before each masking
std::fill(m_SlamMap.data.begin(), m_SlamMap.data.end(),
homer_mapnav_msgs::ModifyMap::NOT_MASKED);
drawPolygon(msg->region, msg->maskAction, msg->mapLayer);
nav_msgs::OccupancyGrid::ConstPtr ret;
if(msg->mapLayer == 0)
{
ret = boost::make_shared<const::nav_msgs::OccupancyGrid>(m_SlamMap);
}
else
{
ret = boost::make_shared<const::nav_msgs::OccupancyGrid>(m_MaskingMap);
if (msg->mapLayer == 0) {
ret = boost::make_shared<const ::nav_msgs::OccupancyGrid>(m_SlamMap);
} else {
ret = boost::make_shared<const ::nav_msgs::OccupancyGrid>(m_MaskingMap);
}
return ret;
}
nav_msgs::OccupancyGrid::ConstPtr MaskingManager::resetMap()
{
std::fill( m_MaskingMap.data.begin(), m_MaskingMap.data.end(), homer_mapnav_msgs::ModifyMap::NOT_MASKED );
nav_msgs::OccupancyGrid::ConstPtr ret = boost::make_shared<const::nav_msgs::OccupancyGrid>(m_MaskingMap);
nav_msgs::OccupancyGrid::ConstPtr MaskingManager::resetMap() {
std::fill(m_MaskingMap.data.begin(), m_MaskingMap.data.end(),
homer_mapnav_msgs::ModifyMap::NOT_MASKED);
nav_msgs::OccupancyGrid::ConstPtr ret =
boost::make_shared<const ::nav_msgs::OccupancyGrid>(m_MaskingMap);
return ret;
}
void MaskingManager::replaceMap(nav_msgs::OccupancyGrid map)
{
if(map.data.size() != 0)
void MaskingManager::replaceMap(nav_msgs::OccupancyGrid map) {
if (map.data.size() != 0)
m_MaskingMap = map;
else
std::fill( m_MaskingMap.data.begin(), m_MaskingMap.data.end(), homer_mapnav_msgs::ModifyMap::NOT_MASKED );
std::fill(m_MaskingMap.data.begin(), m_MaskingMap.data.end(),
homer_mapnav_msgs::ModifyMap::NOT_MASKED);
}
void MaskingManager::drawPolygon ( vector< geometry_msgs::Point > vertices , int value , int mapLayer)
{
if ( vertices.size() == 0 )
{
ROS_INFO_STREAM( "No vertices given!" );
return;
}
//make temp. map
void MaskingManager::drawPolygon(vector<geometry_msgs::Point> vertices,
int value, int mapLayer) {
if (vertices.size() == 0) {
ROS_INFO_STREAM("No vertices given!");
return;
}
// make temp. map
std::vector<int> data(m_MaskingMap.info.width * m_MaskingMap.info.height);
std::fill (data.begin(), data.end(), 0);
//draw the lines surrounding the polygon
for ( unsigned int i = 0; i < vertices.size(); i++ )
{
int i2 = ( i+1 ) % vertices.size();
drawLine ( data, vertices[i].x, vertices[i].y, vertices[i2].x, vertices[i2].y, 1);
}
//calculate a point in the middle of the polygon
float midX = 0;
float midY = 0;
for ( unsigned int i = 0; i < vertices.size(); i++ )
{
midX += vertices[i].x;
midY += vertices[i].y;
}
midX /= vertices.size();
midY /= vertices.size();
//fill polygon
fillPolygon ( data, (int)midX, (int)midY, 1 );
//copy polygon to masking map or slam map (according to parameter mapLayer)
for ( int i = 0; i < data.size(); i++ )
{
if ( data[i] != 0 )
{
switch(mapLayer)
{
case 0: //SLAM map
m_SlamMap.data[i] = value;
break;
case 1: //Kinect Map. apply masking to masking map
case 2: //masking map
m_MaskingMap.data[i] = value;
break;
std::fill(data.begin(), data.end(), 0);
// draw the lines surrounding the polygon
for (unsigned int i = 0; i < vertices.size(); i++) {
int i2 = (i + 1) % vertices.size();
drawLine(data, vertices[i].x, vertices[i].y, vertices[i2].x,
vertices[i2].y, 1);
}
// calculate a point in the middle of the polygon
float midX = 0;
float midY = 0;
for (unsigned int i = 0; i < vertices.size(); i++) {
midX += vertices[i].x;
midY += vertices[i].y;
}
midX /= vertices.size();
midY /= vertices.size();
// fill polygon
fillPolygon(data, (int)midX, (int)midY, 1);
// copy polygon to masking map or slam map (according to parameter mapLayer)
for (int i = 0; i < data.size(); i++) {
if (data[i] != 0) {
switch (mapLayer) {
case 0: // SLAM map
m_SlamMap.data[i] = value;
break;
case 1: // Kinect Map. apply masking to masking map
case 2: // masking map
m_MaskingMap.data[i] = value;
break;
}
}
}
}
void MaskingManager::drawLine ( std::vector<int> &data, int startX, int startY, int endX, int endY, int value )
{
//bresenham algorithm
int x, y, t, dist, xerr, yerr, dx, dy, incx, incy;
// compute distances
dx = endX - startX;
dy = endY - startY;
// compute increment
if ( dx < 0 )
{
incx = -1;
dx = -dx;
}
else
{
incx = dx ? 1 : 0;
}
if ( dy < 0 )
{
incy = -1;
dy = -dy;
}
else
{
incy = dy ? 1 : 0;
}
// which distance is greater?
dist = ( dx > dy ) ? dx : dy;
// initializing
x = startX;
y = startY;
xerr = dx;
yerr = dy;
// compute cells
for ( t = 0; t < dist; t++ )
{
data[x + m_MaskingMap.info.width * y] = value;
xerr += dx;
yerr += dy;
if ( xerr > dist )
{
xerr -= dist;
x += incx;
void MaskingManager::drawLine(std::vector<int> &data, int startX, int startY,
int endX, int endY, int value) {
// bresenham algorithm
int x, y, t, dist, xerr, yerr, dx, dy, incx, incy;
// compute distances
dx = endX - startX;
dy = endY - startY;
// compute increment
if (dx < 0) {
incx = -1;
dx = -dx;
} else {
incx = dx ? 1 : 0;
}
if ( yerr > dist )
{
yerr -= dist;
y += incy;
if (dy < 0) {
incy = -1;
dy = -dy;
} else {
incy = dy ? 1 : 0;
}
}
}
// which distance is greater?
dist = (dx > dy) ? dx : dy;
// initializing
x = startX;
y = startY;
xerr = dx;
yerr = dy;
// compute cells
for (t = 0; t < dist; t++) {
data[x + m_MaskingMap.info.width * y] = value;
xerr += dx;
yerr += dy;
if (xerr > dist) {
xerr -= dist;
x += incx;
}
if (yerr > dist) {
yerr -= dist;
y += incy;
}
}
}
void MaskingManager::fillPolygon ( std::vector<int> &data, int x, int y, int value )
{
int index = x + m_MaskingMap.info.width * y;
if ( value != data[index] )
{
data[index] = value;
fillPolygon ( data, x + 1, y, value );
fillPolygon ( data, x - 1, y, value );
fillPolygon ( data, x, y + 1, value );
fillPolygon ( data, x, y - 1, value );
}
void MaskingManager::fillPolygon(std::vector<int> &data, int x, int y,
int value) {
int index = x + m_MaskingMap.info.width * y;
if (value != data[index]) {
data[index] = value;
fillPolygon(data, x + 1, y, value);
fillPolygon(data, x - 1, y, value);
fillPolygon(data, x, y + 1, value);
fillPolygon(data, x, y - 1, value);
}
}
homer_map_manager/src/map_manager_node.cpp
+ 335
− 312
View file @ 60972339
This diff is collapsed.
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