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

use dynamic laser frame and don't hang on startup when transform is not there

parent da1805d7
No related branches found
No related tags found
1 merge request!4Recent changes
Hide whitespace changes
Inline Side-by-side
homer_mapping/src/OccupancyMap/OccupancyMap.cpp
+ 817
− 916
View file @ dc3341b5
......@@ -2,11 +2,11 @@
#include <homer_nav_libs/Math/Math.h>
#include <QtGui/QImage>
#include <cmath>
#include <vector>
#include <fstream>
#include <sstream>
#include <QtGui/QImage>
#include <vector>
#include <Eigen/Geometry>
......@@ -16,7 +16,7 @@
#include <homer_mapnav_msgs/ModifyMap.h>
#include <homer_nav_libs/tools.h>
//uncomment this to get extended information on the tracer
// uncomment this to get extended information on the tracer
//#define TRACER_OUTPUT
using namespace std;
......@@ -27,1032 +27,933 @@ const float UNKNOWN_LIKELIHOOD = 0.3;
const char NO_CHANGE = 0;
const char OCCUPIED = 1;
const char FREE = 2;
//the safety border around occupied pixels which is left unchanged
// the safety border around occupied pixels which is left unchanged
const char SAFETY_BORDER = 3;
///////////////////////////////
//assumed laser measure count for loaded maps
// assumed laser measure count for loaded maps
const int LOADED_MEASURECOUNT = 10;
OccupancyMap::OccupancyMap() {
float mapSize, resolution;
ros::param::get("/homer_mapping/size", mapSize);
ros::param::get("/homer_mapping/resolution", resolution);
// add one safety pixel
m_metaData.resolution = resolution;
m_metaData.width = mapSize / m_metaData.resolution + 1;
m_metaData.height = mapSize / m_metaData.resolution + 1;
m_ByteSize = m_metaData.width * m_metaData.height;
OccupancyMap::OccupancyMap()
{
float mapSize, resolution;
ros::param::get("/homer_mapping/size", mapSize);
ros::param::get("/homer_mapping/resolution", resolution);
//add one safety pixel
m_metaData.resolution = resolution;
m_metaData.width = mapSize / m_metaData.resolution + 1;
m_metaData.height = mapSize / m_metaData.resolution + 1;
m_ByteSize = m_metaData.width * m_metaData.height;
m_metaData.origin.position.x = - (m_metaData.width * m_metaData.resolution / 2.0);
m_metaData.origin.position.y = - (m_metaData.height * m_metaData.resolution / 2.0);
m_metaData.origin.orientation.w = 1.0;
m_metaData.origin.orientation.x = 0.0;
m_metaData.origin.orientation.y = 0.0;
m_metaData.origin.orientation.z = 0.0;
initMembers();
m_metaData.origin.position.x =
-(m_metaData.width * m_metaData.resolution / 2.0);
m_metaData.origin.position.y =
-(m_metaData.height * m_metaData.resolution / 2.0);
m_metaData.origin.orientation.w = 1.0;
m_metaData.origin.orientation.x = 0.0;
m_metaData.origin.orientation.y = 0.0;
m_metaData.origin.orientation.z = 0.0;
initMembers();
}
OccupancyMap::OccupancyMap(float *&occupancyProbability, geometry_msgs::Pose origin, float resolution, int width, int height, Box2D<int> exploredRegion)
{
OccupancyMap::OccupancyMap(float*& occupancyProbability,
geometry_msgs::Pose origin, float resolution,
int width, int height, Box2D<int> exploredRegion) {
m_metaData.origin = origin;
m_metaData.resolution = resolution;
m_metaData.width = width;
m_metaData.height = height;
m_metaData.width = width;
m_metaData.height = height;
m_ByteSize = m_metaData.width * m_metaData.height;
initMembers();
m_ExploredRegion = exploredRegion;
m_ChangedRegion = exploredRegion;
if ( m_OccupancyProbability )
{
delete[] m_OccupancyProbability;
if (m_OccupancyProbability) {
delete[] m_OccupancyProbability;
}
m_OccupancyProbability = occupancyProbability;
for(unsigned i = 0; i < m_ByteSize; i++)
{
if(m_OccupancyProbability[i] != 0.5)
{
for (unsigned i = 0; i < m_ByteSize; i++) {
if (m_OccupancyProbability[i] != 0.5) {
m_MeasurementCount[i] = LOADED_MEASURECOUNT;
m_OccupancyCount[i] = m_OccupancyProbability[i] * (float)LOADED_MEASURECOUNT;
m_OccupancyCount[i] =
m_OccupancyProbability[i] * (float)LOADED_MEASURECOUNT;
}
}
}
OccupancyMap::OccupancyMap(const OccupancyMap& occupancyMap) {
m_OccupancyProbability = 0;
m_MeasurementCount = 0;
m_OccupancyCount = 0;
m_CurrentChanges = 0;
m_HighSensitive = 0;
OccupancyMap::OccupancyMap ( const OccupancyMap& occupancyMap )
{
m_OccupancyProbability = 0;
m_MeasurementCount = 0;
m_OccupancyCount = 0;
m_CurrentChanges = 0;
m_HighSensitive = 0;
*this = occupancyMap;
*this = occupancyMap;
}
OccupancyMap::~OccupancyMap()
{
cleanUp();
}
OccupancyMap::~OccupancyMap() { cleanUp(); }
void OccupancyMap::initMembers() {
ros::param::get("/homer_mapping/backside_checking", m_BacksideChecking);
ros::param::get("/homer_mapping/obstacle_borders", m_ObstacleBorders);
ros::param::get("/homer_mapping/measure_sampling_step",
m_MeasureSamplingStep);
ros::param::get("/homer_mapping/laser_scanner/free_reading_distance",
m_FreeReadingDistance);
m_OccupancyProbability = new float[m_ByteSize];
m_MeasurementCount = new unsigned short[m_ByteSize];
m_OccupancyCount = new unsigned short[m_ByteSize];
m_CurrentChanges = new unsigned char[m_ByteSize];
m_HighSensitive = new unsigned short[m_ByteSize];
for (unsigned i = 0; i < m_ByteSize; i++) {
m_OccupancyProbability[i] = UNKNOWN_LIKELIHOOD;
m_OccupancyCount[i] = 0;
m_MeasurementCount[i] = 0;
m_CurrentChanges[i] = NO_CHANGE;
m_HighSensitive[i] = 0;
}
void OccupancyMap::initMembers()
{
ros::param::get("/homer_mapping/backside_checking", m_BacksideChecking);
ros::param::get("/homer_mapping/obstacle_borders", m_ObstacleBorders);
ros::param::get("/homer_mapping/measure_sampling_step", m_MeasureSamplingStep);
ros::param::get("/homer_mapping/laser_scanner/free_reading_distance", m_FreeReadingDistance);
m_OccupancyProbability = new float[m_ByteSize];
m_MeasurementCount = new unsigned short[m_ByteSize];
m_OccupancyCount = new unsigned short[m_ByteSize];
m_CurrentChanges = new unsigned char[m_ByteSize];
m_HighSensitive = new unsigned short[m_ByteSize];
for ( unsigned i=0; i<m_ByteSize; i++ )
{
m_OccupancyProbability[i]=UNKNOWN_LIKELIHOOD;
m_OccupancyCount[i]=0;
m_MeasurementCount[i]=0;
m_CurrentChanges[i]=NO_CHANGE;
m_HighSensitive[i] = 0;
}
m_ExploredRegion=Box2D<int> ( m_metaData.width/2.1, m_metaData.height/2.1, m_metaData.width/1.9, m_metaData.height/1.9 );
maximizeChangedRegion();
try
{
bool got_transform = m_tfListener.waitForTransform("/base_link","/laser", ros::Time(0), ros::Duration(1));
while(!got_transform);
{
got_transform = m_tfListener.waitForTransform("/base_link","/laser", ros::Time(0), ros::Duration(1));
if(!got_transform)
{
ROS_ERROR_STREAM("need transformation from base_link to laser!");
}
}
m_tfListener.lookupTransform("/base_link", "/laser", ros::Time(0), m_laserTransform);
}
catch (tf::TransformException ex) {
ROS_ERROR_STREAM(ex.what());
}
m_ExploredRegion =
Box2D<int>(m_metaData.width / 2.1, m_metaData.height / 2.1,
m_metaData.width / 1.9, m_metaData.height / 1.9);
maximizeChangedRegion();
}
OccupancyMap& OccupancyMap::operator=(const OccupancyMap& occupancyMap) {
// free allocated memory
cleanUp();
m_metaData = occupancyMap.m_metaData;
m_ExploredRegion = occupancyMap.m_ExploredRegion;
m_ByteSize = occupancyMap.m_ByteSize;
ros::param::get("/homer_mapping/backside_checking", m_BacksideChecking);
// re-allocate all arrays
m_OccupancyProbability = new float[m_ByteSize];
m_MeasurementCount = new unsigned short[m_ByteSize];
m_OccupancyCount = new unsigned short[m_ByteSize];
m_CurrentChanges = new unsigned char[m_ByteSize];
m_HighSensitive = new unsigned short[m_ByteSize];
// copy array data
memcpy(m_OccupancyProbability, occupancyMap.m_OccupancyProbability,
m_ByteSize * sizeof(*m_OccupancyProbability));
memcpy(m_MeasurementCount, occupancyMap.m_MeasurementCount,
m_ByteSize * sizeof(*m_MeasurementCount));
memcpy(m_OccupancyCount, occupancyMap.m_OccupancyCount,
m_ByteSize * sizeof(*m_OccupancyCount));
memcpy(m_CurrentChanges, occupancyMap.m_CurrentChanges,
m_ByteSize * sizeof(*m_CurrentChanges));
memcpy(m_HighSensitive, occupancyMap.m_HighSensitive,
m_ByteSize * sizeof(*m_HighSensitive));
return *this;
}
OccupancyMap& OccupancyMap::operator= ( const OccupancyMap & occupancyMap )
{
// free allocated memory
cleanUp();
m_metaData = occupancyMap.m_metaData;
void OccupancyMap::changeMapSize(int x_add_left, int y_add_up, int x_add_right,
int y_add_down) {
int new_width = m_metaData.width + x_add_left + x_add_right;
int new_height = m_metaData.height + y_add_up + y_add_down;
m_ByteSize = new_width * new_height;
// allocate all arrays
float* OccupancyProbability = new float[m_ByteSize];
unsigned short* MeasurementCount = new unsigned short[m_ByteSize];
unsigned short* OccupancyCount = new unsigned short[m_ByteSize];
unsigned char* CurrentChanges = new unsigned char[m_ByteSize];
unsigned short* HighSensitive = new unsigned short[m_ByteSize];
for (unsigned i = 0; i < m_ByteSize; i++) {
OccupancyProbability[i] = UNKNOWN_LIKELIHOOD;
OccupancyCount[i] = 0;
MeasurementCount[i] = 0;
CurrentChanges[i] = NO_CHANGE;
HighSensitive[i] = 0;
}
m_ExploredRegion = occupancyMap.m_ExploredRegion;
m_ByteSize = occupancyMap.m_ByteSize;
for (int y = 0; y < m_metaData.height; y++) {
for (int x = 0; x < m_metaData.width; x++) {
int i = y * m_metaData.width + x;
int in = (y + y_add_up) * new_width + (x + x_add_left);
OccupancyProbability[in] = m_OccupancyProbability[i];
MeasurementCount[in] = m_MeasurementCount[i];
OccupancyCount[in] = m_OccupancyCount[i];
CurrentChanges[in] = m_CurrentChanges[i];
HighSensitive[in] = m_HighSensitive[i];
}
}
ros::param::get("/homer_mapping/backside_checking", m_BacksideChecking);
m_ExploredRegion.setMinX(m_ExploredRegion.minX() + x_add_left);
m_ExploredRegion.setMaxX(m_ExploredRegion.maxX() + x_add_left);
m_ExploredRegion.setMinY(m_ExploredRegion.minY() + y_add_up);
m_ExploredRegion.setMaxY(m_ExploredRegion.maxY() + y_add_up);
// re-allocate all arrays
m_OccupancyProbability = new float[m_ByteSize];
m_MeasurementCount = new unsigned short[m_ByteSize];
m_OccupancyCount = new unsigned short[m_ByteSize];
m_CurrentChanges = new unsigned char[m_ByteSize];
m_HighSensitive = new unsigned short[m_ByteSize];
m_ChangedRegion.setMinX(m_ChangedRegion.minX() + x_add_left);
m_ChangedRegion.setMaxX(m_ChangedRegion.maxX() + x_add_left);
m_ChangedRegion.setMinY(m_ChangedRegion.minY() + y_add_up);
m_ChangedRegion.setMaxY(m_ChangedRegion.maxY() + y_add_up);
// copy array data
memcpy ( m_OccupancyProbability, occupancyMap.m_OccupancyProbability, m_ByteSize * sizeof ( *m_OccupancyProbability ) );
memcpy ( m_MeasurementCount, occupancyMap.m_MeasurementCount, m_ByteSize * sizeof ( *m_MeasurementCount ) );
memcpy ( m_OccupancyCount, occupancyMap.m_OccupancyCount, m_ByteSize * sizeof ( *m_OccupancyCount ) );
memcpy ( m_CurrentChanges, occupancyMap.m_CurrentChanges, m_ByteSize * sizeof ( *m_CurrentChanges ) );
memcpy ( m_HighSensitive, occupancyMap.m_HighSensitive, m_ByteSize * sizeof ( *m_HighSensitive) );
m_metaData.width = new_width;
m_metaData.height = new_height;
m_metaData.origin.position.x -= (x_add_left)*m_metaData.resolution;
m_metaData.origin.position.y -= (y_add_up)*m_metaData.resolution;
cleanUp();
return *this;
m_OccupancyProbability = OccupancyProbability;
m_MeasurementCount = MeasurementCount;
m_OccupancyCount = OccupancyCount;
m_CurrentChanges = CurrentChanges;
m_HighSensitive = HighSensitive;
}
void OccupancyMap::changeMapSize( int x_add_left, int y_add_up, int x_add_right, int y_add_down)
{
int new_width = m_metaData.width + x_add_left + x_add_right;
int new_height = m_metaData.height + y_add_up + y_add_down;
m_ByteSize = new_width * new_height;
// allocate all arrays
float* OccupancyProbability = new float[m_ByteSize];
unsigned short* MeasurementCount = new unsigned short[m_ByteSize];
unsigned short* OccupancyCount = new unsigned short[m_ByteSize];
unsigned char* CurrentChanges = new unsigned char[m_ByteSize];
unsigned short* HighSensitive = new unsigned short[m_ByteSize];
for ( unsigned i=0; i<m_ByteSize; i++ )
{
OccupancyProbability[i]=UNKNOWN_LIKELIHOOD;
OccupancyCount[i]=0;
MeasurementCount[i]=0;
CurrentChanges[i]=NO_CHANGE;
HighSensitive[i] = 0;
}
for(int y = 0; y < m_metaData.height; y++)
{
for(int x = 0; x < m_metaData.width; x++)
{
int i = y * m_metaData.width + x;
int in = (y + y_add_up) * new_width + (x + x_add_left);
OccupancyProbability[in] = m_OccupancyProbability[i];
MeasurementCount[in] = m_MeasurementCount[i];
OccupancyCount[in] = m_OccupancyCount[i];
CurrentChanges[in] = m_CurrentChanges[i];
HighSensitive[in] = m_HighSensitive[i];
}
}
m_ExploredRegion.setMinX( m_ExploredRegion.minX() + x_add_left);
m_ExploredRegion.setMaxX( m_ExploredRegion.maxX() + x_add_left);
m_ExploredRegion.setMinY( m_ExploredRegion.minY() + y_add_up);
m_ExploredRegion.setMaxY( m_ExploredRegion.maxY() + y_add_up);
m_ChangedRegion.setMinX( m_ChangedRegion.minX() + x_add_left);
m_ChangedRegion.setMaxX( m_ChangedRegion.maxX() + x_add_left);
m_ChangedRegion.setMinY( m_ChangedRegion.minY() + y_add_up);
m_ChangedRegion.setMaxY( m_ChangedRegion.maxY() + y_add_up);
m_metaData.width = new_width;
m_metaData.height = new_height;
m_metaData.origin.position.x -= (x_add_left ) * m_metaData.resolution;
m_metaData.origin.position.y -= (y_add_up ) * m_metaData.resolution;
cleanUp();
m_OccupancyProbability = OccupancyProbability;
m_MeasurementCount = MeasurementCount;
m_OccupancyCount = OccupancyCount;
m_CurrentChanges = CurrentChanges;
m_HighSensitive = HighSensitive;
int OccupancyMap::width() const { return m_metaData.width; }
}
int OccupancyMap::width() const
{
return m_metaData.width;
}
int OccupancyMap::height() const { return m_metaData.height; }
int OccupancyMap::height() const
{
return m_metaData.height;
}
float OccupancyMap::getOccupancyProbability ( Eigen::Vector2i p )
{
if(p.y() >= m_metaData.height || p.x() >= m_metaData.width)
{
return UNKNOWN_LIKELIHOOD;
}
unsigned offset = m_metaData.width * p.y() + p.x();
return m_OccupancyProbability[ offset ];
float OccupancyMap::getOccupancyProbability(Eigen::Vector2i p) {
if (p.y() >= m_metaData.height || p.x() >= m_metaData.width) {
return UNKNOWN_LIKELIHOOD;
}
unsigned offset = m_metaData.width * p.y() + p.x();
return m_OccupancyProbability[offset];
}
void OccupancyMap::resetHighSensitive()
{
ROS_INFO_STREAM("High sensitive Areas reseted");
m_reset_high = true;
void OccupancyMap::resetHighSensitive() {
ROS_INFO_STREAM("High sensitive Areas reseted");
m_reset_high = true;
}
void OccupancyMap::computeOccupancyProbabilities()
{
for ( int y = m_ChangedRegion.minY(); y <= m_ChangedRegion.maxY(); y++ )
{
int yOffset = m_metaData.width * y;
for ( int x = m_ChangedRegion.minX(); x <= m_ChangedRegion.maxX(); x++ )
{
int i = x + yOffset;
if ( m_MeasurementCount[i] > 0 )
{
//int maxCount = 100; //TODO param
//if(m_MeasurementCount[i] > maxCount * 2 -1)
//{
//int scalingFactor = m_MeasurementCount[i] / maxCount;
//if ( scalingFactor != 0 )
//{
//m_MeasurementCount[i] /= scalingFactor;
//m_OccupancyCount[i] /= scalingFactor;
//}
//}
m_OccupancyProbability[i] = m_OccupancyCount[i] / static_cast<float> ( m_MeasurementCount[i] );
if (m_HighSensitive[i] == 1)
{
if(m_reset_high == true)
{
m_OccupancyCount[i] = 0;
m_OccupancyProbability[i] = 0;
}
if(m_MeasurementCount[i] > 20 )
{
m_MeasurementCount[i] = 10;
m_OccupancyCount[i] = 10 * m_OccupancyProbability[i];
}
if(m_OccupancyProbability[i] > 0.3)
{
m_OccupancyProbability[i] = 1 ;
}
}
}
else
{
m_OccupancyProbability[i] = UNKNOWN_LIKELIHOOD;
}
void OccupancyMap::computeOccupancyProbabilities() {
for (int y = m_ChangedRegion.minY(); y <= m_ChangedRegion.maxY(); y++) {
int yOffset = m_metaData.width * y;
for (int x = m_ChangedRegion.minX(); x <= m_ChangedRegion.maxX(); x++) {
int i = x + yOffset;
if (m_MeasurementCount[i] > 0) {
// int maxCount = 100; //TODO param
// if(m_MeasurementCount[i] > maxCount * 2 -1)
//{
// int scalingFactor = m_MeasurementCount[i] / maxCount;
// if ( scalingFactor != 0 )
//{
// m_MeasurementCount[i] /= scalingFactor;
// m_OccupancyCount[i] /= scalingFactor;
//}
//}
m_OccupancyProbability[i] =
m_OccupancyCount[i] /
static_cast<float>(m_MeasurementCount[i]);
if (m_HighSensitive[i] == 1) {
if (m_reset_high == true) {
m_OccupancyCount[i] = 0;
m_OccupancyProbability[i] = 0;
}
if (m_MeasurementCount[i] > 20) {
m_MeasurementCount[i] = 10;
m_OccupancyCount[i] = 10 * m_OccupancyProbability[i];
}
if (m_OccupancyProbability[i] > 0.3) {
m_OccupancyProbability[i] = 1;
}
}
} else {
m_OccupancyProbability[i] = UNKNOWN_LIKELIHOOD;
}
}
}
if (m_reset_high == true) {
m_reset_high = false;
}
}
if(m_reset_high == true)
{
m_reset_high = false;
}
}
void OccupancyMap::insertLaserData ( sensor_msgs::LaserScan::ConstPtr laserData, tf::Transform transform)
{
m_latestMapTransform = transform;
markRobotPositionFree();
std::vector<RangeMeasurement> ranges;
ranges.reserve ( laserData->ranges.size() );
bool errorFound=false;
int lastValidIndex=-1;
float lastValidRange=m_FreeReadingDistance;
RangeMeasurement rangeMeasurement;
rangeMeasurement.sensorPos.x = m_laserTransform.getOrigin().getX();
rangeMeasurement.sensorPos.y = m_laserTransform.getOrigin().getY();
for ( unsigned int i = 0; i < laserData->ranges.size(); i++ )
{
if ( ( laserData->ranges[i] >= laserData->range_min ) && ( laserData->ranges[i] <= laserData->range_max ) )
{
//if we're at the end of an errorneous segment, interpolate
//between last valid point and current point
if ( errorFound )
{
//smaller of the two ranges belonging to end points
float range = Math::min ( lastValidRange, laserData->ranges[i] );
range -= m_metaData.resolution * 2;
if ( range < m_FreeReadingDistance )
{
range = m_FreeReadingDistance;
void OccupancyMap::insertLaserData(sensor_msgs::LaserScan::ConstPtr laserData,
tf::Transform transform) {
m_latestMapTransform = transform;
try {
bool got_transform = m_tfListener.waitForTransform(
"/base_link", laserData->header.frame_id, ros::Time(0),
ros::Duration(0.2));
m_tfListener.lookupTransform("/base_link", laserData->header.frame_id,
ros::Time(0), m_laserTransform);
} catch (tf::TransformException ex) {
ROS_ERROR_STREAM(ex.what());
ROS_ERROR_STREAM("need transformation from base_link to laser!");
return;
}
markRobotPositionFree();
std::vector<RangeMeasurement> ranges;
ranges.reserve(laserData->ranges.size());
bool errorFound = false;
int lastValidIndex = -1;
float lastValidRange = m_FreeReadingDistance;
RangeMeasurement rangeMeasurement;
rangeMeasurement.sensorPos.x = m_laserTransform.getOrigin().getX();
rangeMeasurement.sensorPos.y = m_laserTransform.getOrigin().getY();
for (unsigned int i = 0; i < laserData->ranges.size(); i++) {
if ((laserData->ranges[i] >= laserData->range_min) &&
(laserData->ranges[i] <= laserData->range_max)) {
// if we're at the end of an errorneous segment, interpolate
// between last valid point and current point
if (errorFound) {
// smaller of the two ranges belonging to end points
float range = Math::min(lastValidRange, laserData->ranges[i]);
range -= m_metaData.resolution * 2;
if (range < m_FreeReadingDistance) {
range = m_FreeReadingDistance;
} else if (range > laserData->range_max) {
range = laserData->range_max;
}
// choose smaller range
for (unsigned j = lastValidIndex + 1; j < i; j++) {
float alpha =
laserData->angle_min + j * laserData->angle_increment;
tf::Vector3 pin;
tf::Vector3 pout;
pin.setX(cos(alpha) * range);
pin.setY(sin(alpha) * range);
pin.setZ(0);
pout = m_laserTransform * pin;
rangeMeasurement.endPos.x = pout.x();
rangeMeasurement.endPos.y = pout.y();
rangeMeasurement.free = true;
ranges.push_back(rangeMeasurement);
}
}
float alpha = laserData->angle_min + i * laserData->angle_increment;
tf::Vector3 pin;
tf::Vector3 pout;
pin.setX(cos(alpha) * laserData->ranges[i]);
pin.setY(sin(alpha) * laserData->ranges[i]);
pin.setZ(0);
pout = m_laserTransform * pin;
rangeMeasurement.endPos.x = pout.x();
rangeMeasurement.endPos.y = pout.y();
rangeMeasurement.free = false;
ranges.push_back(rangeMeasurement);
errorFound = false;
lastValidIndex = i;
lastValidRange = laserData->ranges[i];
} else {
errorFound = true;
}
else
if ( range > laserData->range_max )
{
range = laserData->range_max;
}
//choose smaller range
for ( unsigned j=lastValidIndex+1; j<i; j++ )
{
float alpha = laserData->angle_min + j * laserData->angle_increment;
tf::Vector3 pin;
tf::Vector3 pout;
pin.setX( cos(alpha) * range);
pin.setY( sin(alpha) * range);
pin.setZ(0);
pout = m_laserTransform * pin;
rangeMeasurement.endPos.x = pout.x();
rangeMeasurement.endPos.y = pout.y();
rangeMeasurement.free = true;
ranges.push_back ( rangeMeasurement );
}
}
float alpha = laserData->angle_min + i * laserData->angle_increment;
tf::Vector3 pin;
tf::Vector3 pout;
pin.setX( cos(alpha) * laserData->ranges[i]);
pin.setY( sin(alpha) * laserData->ranges[i]);
pin.setZ(0);
pout = m_laserTransform * pin;
rangeMeasurement.endPos.x = pout.x();
rangeMeasurement.endPos.y = pout.y();
rangeMeasurement.free = false;
ranges.push_back ( rangeMeasurement );
errorFound=false;
lastValidIndex=i;
lastValidRange=laserData->ranges[i];
}
else
{
errorFound=true;
insertRanges(ranges, laserData->header.stamp);
}
void OccupancyMap::insertRanges(vector<RangeMeasurement> ranges,
ros::Time stamp) {
if (ranges.size() < 1) {
return;
}
}
clearChanges();
insertRanges ( ranges , laserData->header.stamp);
Eigen::Vector2i lastEndPixel;
int need_x_left = 0;
int need_x_right = 0;
int need_y_up = 0;
int need_y_down = 0;
}
std::vector<Eigen::Vector2i> map_pixel;
for (int i = 0; i < ranges.size(); i++) {
geometry_msgs::Point endPosWorld =
map_tools::transformPoint(ranges[i].endPos, m_latestMapTransform);
map_pixel.push_back(map_tools::toMapCoords(
endPosWorld, m_metaData.origin, m_metaData.resolution));
}
geometry_msgs::Point sensorPosWorld =
map_tools::transformPoint(ranges[0].sensorPos, m_latestMapTransform);
Eigen::Vector2i sensorPixel = map_tools::toMapCoords(
sensorPosWorld, m_metaData.origin, m_metaData.resolution);
m_ChangedRegion.enclose(sensorPixel.x(), sensorPixel.y());
////paint safety borders
// if ( m_ObstacleBorders )
//{
// for ( unsigned i=0; i<ranges.size(); i++ )
//{
// Eigen::Vector2i endPixel = map_pixel[i];
// for ( int y=endPixel.y()-1; y <= endPixel.y() +1; y++ )
//{
// if(y > m_metaData.height) continue;
// for ( int x=endPixel.x()-1; x <= endPixel.x() +1; x++ )
//{
// if(x > m_metaData.width) continue;
// unsigned offset=x+m_metaData.width*y;
// if ( offset < unsigned ( m_ByteSize ) )
//{
// m_CurrentChanges[ offset ] = SAFETY_BORDER;
//}
//}
//}
//}
//}
////paint safety ranges
// for ( unsigned i=0; i<ranges.size(); i++ )
//{
// Eigen::Vector2i startPixel = map_pixel[i];
// geometry_msgs::Point endPos;
// endPos.x = ranges[i].endPos.x * 4;
// endPos.y = ranges[i].endPos.y * 4;
// geometry_msgs::Point endPosWorld = map_tools::transformPoint(endPos,
// m_latestMapTransform);
// Eigen::Vector2i endPixel = map_tools::toMapCoords(endPosWorld,
// m_metaData.origin, m_metaData.resolution);
// if(endPixel.x() < 0) endPixel.x() = 0;
// if(endPixel.y() < 0) endPixel.y() = 0;
// if(endPixel.x() >= m_metaData.width) endPixel.x() = m_metaData.width - 1;
// if(endPixel.y() >= m_metaData.height) endPixel.y() = m_metaData.height -
// 1;
// drawLine ( m_CurrentChanges, startPixel, endPixel, SAFETY_BORDER );
//}
// paint end pixels
for (unsigned i = 0; i < ranges.size(); i++) {
Eigen::Vector2i endPixel = map_pixel[i];
if (endPixel != lastEndPixel) {
bool outside = false;
if (endPixel.x() >= m_metaData.width) {
need_x_right = std::max(
(int)(endPixel.x() - m_metaData.width + 10), need_x_right);
outside = true;
}
if (endPixel.x() < 0) {
need_x_left = std::max((int)(-endPixel.x()) + 10, need_x_left);
outside = true;
}
if (endPixel.y() >= m_metaData.height) {
need_y_down = std::max(
(int)(endPixel.y() - m_metaData.height) + 10, need_y_down);
outside = true;
}
if (endPixel.y() < 0) {
need_y_up = std::max((int)(-endPixel.y()) + 10, need_y_up);
outside = true;
}
if (outside) {
continue;
}
m_ChangedRegion.enclose(endPixel.x(), endPixel.y());
// paint free ranges
drawLine(m_CurrentChanges, sensorPixel, endPixel, ::FREE);
if (!ranges[i].free) {
unsigned offset =
endPixel.x() + m_metaData.width * endPixel.y();
m_CurrentChanges[offset] = ::OCCUPIED;
}
}
lastEndPixel = endPixel;
}
void OccupancyMap::insertRanges ( vector<RangeMeasurement> ranges, ros::Time stamp )
{
if(ranges.size() < 1)
{
return;
}
clearChanges();
Eigen::Vector2i lastEndPixel;
int need_x_left = 0;
int need_x_right = 0;
int need_y_up = 0;
int need_y_down = 0;
std::vector<Eigen::Vector2i> map_pixel;
for(int i = 0; i < ranges.size(); i++)
{
geometry_msgs::Point endPosWorld = map_tools::transformPoint(ranges[i].endPos, m_latestMapTransform);
map_pixel.push_back(map_tools::toMapCoords(endPosWorld, m_metaData.origin, m_metaData.resolution));
}
geometry_msgs::Point sensorPosWorld = map_tools::transformPoint(ranges[0].sensorPos, m_latestMapTransform);
Eigen::Vector2i sensorPixel = map_tools::toMapCoords(sensorPosWorld, m_metaData.origin, m_metaData.resolution);
m_ChangedRegion.enclose ( sensorPixel.x(), sensorPixel.y() );
////paint safety borders
//if ( m_ObstacleBorders )
//{
//for ( unsigned i=0; i<ranges.size(); i++ )
//{
//Eigen::Vector2i endPixel = map_pixel[i];
//for ( int y=endPixel.y()-1; y <= endPixel.y() +1; y++ )
//{
//if(y > m_metaData.height) continue;
//for ( int x=endPixel.x()-1; x <= endPixel.x() +1; x++ )
//{
//if(x > m_metaData.width) continue;
//unsigned offset=x+m_metaData.width*y;
//if ( offset < unsigned ( m_ByteSize ) )
//{
//m_CurrentChanges[ offset ] = SAFETY_BORDER;
//}
//}
//}
//}
//}
////paint safety ranges
//for ( unsigned i=0; i<ranges.size(); i++ )
//{
//Eigen::Vector2i startPixel = map_pixel[i];
//geometry_msgs::Point endPos;
//endPos.x = ranges[i].endPos.x * 4;
//endPos.y = ranges[i].endPos.y * 4;
//geometry_msgs::Point endPosWorld = map_tools::transformPoint(endPos, m_latestMapTransform);
//Eigen::Vector2i endPixel = map_tools::toMapCoords(endPosWorld, m_metaData.origin, m_metaData.resolution);
//if(endPixel.x() < 0) endPixel.x() = 0;
//if(endPixel.y() < 0) endPixel.y() = 0;
//if(endPixel.x() >= m_metaData.width) endPixel.x() = m_metaData.width - 1;
//if(endPixel.y() >= m_metaData.height) endPixel.y() = m_metaData.height - 1;
//drawLine ( m_CurrentChanges, startPixel, endPixel, SAFETY_BORDER );
//}
//paint end pixels
for ( unsigned i=0; i<ranges.size(); i++ )
{
Eigen::Vector2i endPixel = map_pixel[i];
if ( endPixel != lastEndPixel )
{
bool outside = false;
if(endPixel.x() >= m_metaData.width)
{
need_x_right = std::max((int)( endPixel.x() - m_metaData.width + 10), need_x_right);
outside = true;
}
if(endPixel.x() < 0)
{
need_x_left = std::max((int)(-endPixel.x()) + 10, need_x_left);
outside = true;
}
if(endPixel.y() >= m_metaData.height)
{
need_y_down = std::max((int)(endPixel.y() - m_metaData.height) + 10, need_y_down);
outside = true;
}
if(endPixel.y() < 0)
{
need_y_up = std::max((int)(- endPixel.y()) + 10, need_y_up);
outside = true;
}
if(outside)
{
continue;
}
m_ChangedRegion.enclose ( endPixel.x(), endPixel.y() );
//paint free ranges
drawLine ( m_CurrentChanges, sensorPixel, endPixel, ::FREE );
if ( !ranges[i].free )
{
unsigned offset = endPixel.x() + m_metaData.width * endPixel.y();
m_CurrentChanges[ offset ] = ::OCCUPIED;
}
}
lastEndPixel=endPixel;
}
m_ChangedRegion.clip ( Box2D<int> ( 0,0,m_metaData.width-1,m_metaData.height-1 ) );
m_ExploredRegion.enclose ( m_ChangedRegion );
applyChanges();
computeOccupancyProbabilities();
if(need_x_left + need_x_right + need_y_down + need_y_up > 0)
{
//keep square aspect ration till homer_gui can handle other maps
//int need_x = need_x_left + need_x_right;
//int need_y = need_y_up + need_y_down;
//if(need_x > need_y)
//{
//need_y_down += need_x - need_y;
//}
//else if (need_y > need_x)
//{
//need_x_right += need_y - need_x;
//}
ROS_INFO_STREAM("new map size!");
ROS_INFO_STREAM(" "<<need_x_left<<" "<<need_y_up<<" "<<need_x_right<<" "<<need_y_down);
changeMapSize(need_x_left, need_y_up, need_x_right, need_y_down);
}
m_ChangedRegion.clip(
Box2D<int>(0, 0, m_metaData.width - 1, m_metaData.height - 1));
m_ExploredRegion.enclose(m_ChangedRegion);
applyChanges();
computeOccupancyProbabilities();
if (need_x_left + need_x_right + need_y_down + need_y_up > 0) {
// keep square aspect ration till homer_gui can handle other maps
// int need_x = need_x_left + need_x_right;
// int need_y = need_y_up + need_y_down;
// if(need_x > need_y)
//{
// need_y_down += need_x - need_y;
//}
// else if (need_y > need_x)
//{
// need_x_right += need_y - need_x;
//}
ROS_INFO_STREAM("new map size!");
ROS_INFO_STREAM(" " << need_x_left << " " << need_y_up << " "
<< need_x_right << " " << need_y_down);
changeMapSize(need_x_left, need_y_up, need_x_right, need_y_down);
}
}
double OccupancyMap::contrastFromProbability ( int8_t prob )
{
// range from 0..126 (=127 values) and 128..255 (=128 values)
double diff = ( ( double ) prob - UNKNOWN );
double contrast;
if ( prob <= UNKNOWN )
{
contrast = ( diff / UNKNOWN ); // 0..1
}
else
{
contrast = ( diff / ( UNKNOWN+1 ) ); // 0..1
}
return ( contrast * contrast );
double OccupancyMap::contrastFromProbability(int8_t prob) {
// range from 0..126 (=127 values) and 128..255 (=128 values)
double diff = ((double)prob - UNKNOWN);
double contrast;
if (prob <= UNKNOWN) {
contrast = (diff / UNKNOWN); // 0..1
} else {
contrast = (diff / (UNKNOWN + 1)); // 0..1
}
return (contrast * contrast);
}
double OccupancyMap::evaluateByContrast()
{
double contrastSum = 0.0;
unsigned int contrastCnt = 0;
for ( int y = m_ExploredRegion.minY(); y <= m_ExploredRegion.maxY(); y++ )
{
for ( int x = m_ExploredRegion.minX(); x <= m_ExploredRegion.maxX(); x++ )
{
int i = x + y * m_metaData.width;
if ( m_MeasurementCount [i] > 1 )
{
int prob = m_OccupancyProbability[i] * 100;
if ( prob != NOT_SEEN_YET ) // ignore not yet seen cells
{
contrastSum += contrastFromProbability ( prob );
contrastCnt++;
double OccupancyMap::evaluateByContrast() {
double contrastSum = 0.0;
unsigned int contrastCnt = 0;
for (int y = m_ExploredRegion.minY(); y <= m_ExploredRegion.maxY(); y++) {
for (int x = m_ExploredRegion.minX(); x <= m_ExploredRegion.maxX();
x++) {
int i = x + y * m_metaData.width;
if (m_MeasurementCount[i] > 1) {
int prob = m_OccupancyProbability[i] * 100;
if (prob != NOT_SEEN_YET) // ignore not yet seen cells
{
contrastSum += contrastFromProbability(prob);
contrastCnt++;
}
}
}
}
}
}
if ( ( contrastCnt ) > 0 )
{
return ( ( contrastSum / contrastCnt ) * 100 );
}
return ( 0 );
if ((contrastCnt) > 0) {
return ((contrastSum / contrastCnt) * 100);
}
return (0);
}
vector<MeasurePoint> OccupancyMap::getMeasurePoints (sensor_msgs::LaserScanConstPtr laserData)
{
vector<MeasurePoint> result;
result.reserve ( laserData->ranges.size() );
double minDist = m_MeasureSamplingStep;
Point2D lastHitPos;
Point2D lastUsedHitPos;
//extract points for measuring
for ( unsigned int i=0; i < laserData->ranges.size(); i++ )
{
if ( laserData->ranges[i] <= laserData->range_max && laserData->ranges[i] >= laserData->range_min )
{
float alpha = laserData->angle_min + i * laserData->angle_increment;
tf::Vector3 pin;
tf::Vector3 pout;
pin.setX( cos(alpha) * laserData->ranges[i]);
pin.setY( sin(alpha) * laserData->ranges[i]);
pin.setZ(0);
pout = m_laserTransform * pin;
Point2D hitPos(pout.x(), pout.y());
if ( hitPos.distance ( lastUsedHitPos ) >= minDist )
{
MeasurePoint p;
//preserve borders of segments
if ( ( i!=0 ) &&
( lastUsedHitPos.distance ( lastHitPos ) > m_metaData.resolution*0.5 ) &&
( hitPos.distance ( lastHitPos ) >= minDist*1.5 ) )
{
p.hitPos = lastHitPos;
p.borderType = RightBorder;
result.push_back ( p );
p.hitPos = hitPos;
p.borderType = LeftBorder;
result.push_back ( p );
lastUsedHitPos = hitPos;
}
else
{
//save current point
p.hitPos = hitPos;
p.borderType = NoBorder;
result.push_back ( p );
lastUsedHitPos = hitPos;
vector<MeasurePoint> OccupancyMap::getMeasurePoints(
sensor_msgs::LaserScanConstPtr laserData) {
vector<MeasurePoint> result;
result.reserve(laserData->ranges.size());
double minDist = m_MeasureSamplingStep;
Point2D lastHitPos;
Point2D lastUsedHitPos;
// extract points for measuring
for (unsigned int i = 0; i < laserData->ranges.size(); i++) {
if (laserData->ranges[i] <= laserData->range_max &&
laserData->ranges[i] >= laserData->range_min) {
float alpha = laserData->angle_min + i * laserData->angle_increment;
tf::Vector3 pin;
tf::Vector3 pout;
pin.setX(cos(alpha) * laserData->ranges[i]);
pin.setY(sin(alpha) * laserData->ranges[i]);
pin.setZ(0);
pout = m_laserTransform * pin;
Point2D hitPos(pout.x(), pout.y());
if (hitPos.distance(lastUsedHitPos) >= minDist) {
MeasurePoint p;
// preserve borders of segments
if ((i != 0) && (lastUsedHitPos.distance(lastHitPos) >
m_metaData.resolution * 0.5) &&
(hitPos.distance(lastHitPos) >= minDist * 1.5)) {
p.hitPos = lastHitPos;
p.borderType = RightBorder;
result.push_back(p);
p.hitPos = hitPos;
p.borderType = LeftBorder;
result.push_back(p);
lastUsedHitPos = hitPos;
} else {
// save current point
p.hitPos = hitPos;
p.borderType = NoBorder;
result.push_back(p);
lastUsedHitPos = hitPos;
}
}
lastHitPos = hitPos;
}
}
lastHitPos = hitPos;
}
}
//the first and last one are border pixels
if ( result.size() > 0 )
{
result[0].borderType = LeftBorder;
result[result.size()-1].borderType = RightBorder;
}
//calculate front check points
for ( unsigned i=0; i < result.size(); i++ )
{
CVec2 diff;
switch ( result[i].borderType )
{
case NoBorder:
diff = result[i-1].hitPos - result[i+1].hitPos;
break;
case LeftBorder:
diff = result[i].hitPos - result[i+1].hitPos;
break;
case RightBorder:
diff = result[i-1].hitPos - result[i].hitPos;
break;
// the first and last one are border pixels
if (result.size() > 0) {
result[0].borderType = LeftBorder;
result[result.size() - 1].borderType = RightBorder;
}
CVec2 normal = diff.rotate ( Math::Pi * 0.5 );
normal.normalize();
normal *= m_metaData.resolution * sqrt ( 2.0 ) * 10.0;
// calculate front check points
for (unsigned i = 0; i < result.size(); i++) {
CVec2 diff;
result[i].frontPos = result[i].hitPos + normal;
}
switch (result[i].borderType) {
case NoBorder:
diff = result[i - 1].hitPos - result[i + 1].hitPos;
break;
case LeftBorder:
diff = result[i].hitPos - result[i + 1].hitPos;
break;
case RightBorder:
diff = result[i - 1].hitPos - result[i].hitPos;
break;
}
CVec2 normal = diff.rotate(Math::Pi * 0.5);
normal.normalize();
normal *= m_metaData.resolution * sqrt(2.0) * 10.0;
result[i].frontPos = result[i].hitPos + normal;
}
return result;
return result;
}
float OccupancyMap::computeScore(Pose robotPose,
std::vector<MeasurePoint> measurePoints) {
// This is a very simple implementation, using only the end point of the
// beam.
// For every beam the end cell is computed and tested if the cell is
// occupied.
unsigned lastOffset = 0;
unsigned hitOffset = 0;
unsigned frontOffset = 0;
float fittingFactor = 0.0;
float sinTheta = sin(robotPose.theta());
float cosTheta = cos(robotPose.theta());
for (unsigned int i = 0; i < measurePoints.size(); i++) {
// fast variant:
float x = cosTheta * measurePoints[i].hitPos.x() -
sinTheta * measurePoints[i].hitPos.y() + robotPose.x();
float y = sinTheta * measurePoints[i].hitPos.x() +
cosTheta * measurePoints[i].hitPos.y() + robotPose.y();
geometry_msgs::Point hitPos;
hitPos.x = x;
hitPos.y = y;
Eigen::Vector2i hitPixel = map_tools::toMapCoords(
hitPos, m_metaData.origin, m_metaData.resolution);
hitOffset = hitPixel.x() + m_metaData.width * hitPixel.y();
// avoid multiple measuring of same pixel or unknown pixel
if ((hitOffset == lastOffset) || (hitOffset >= unsigned(m_ByteSize)) ||
(m_MeasurementCount[hitOffset] == 0)) {
continue;
}
float OccupancyMap::computeScore ( Pose robotPose, std::vector<MeasurePoint> measurePoints )
{
// This is a very simple implementation, using only the end point of the beam.
// For every beam the end cell is computed and tested if the cell is occupied.
unsigned lastOffset=0;
unsigned hitOffset=0;
unsigned frontOffset=0;
float fittingFactor = 0.0;
float sinTheta = sin ( robotPose.theta() );
float cosTheta = cos ( robotPose.theta() );
for ( unsigned int i = 0; i < measurePoints.size(); i++ )
{
//fast variant:
float x = cosTheta * measurePoints[i].hitPos.x() - sinTheta * measurePoints[i].hitPos.y() + robotPose.x();
float y = sinTheta * measurePoints[i].hitPos.x() + cosTheta * measurePoints[i].hitPos.y() + robotPose.y();
geometry_msgs::Point hitPos;
hitPos.x = x;
hitPos.y = y;
Eigen::Vector2i hitPixel = map_tools::toMapCoords(hitPos, m_metaData.origin, m_metaData.resolution);
hitOffset = hitPixel.x() + m_metaData.width*hitPixel.y();
//avoid multiple measuring of same pixel or unknown pixel
if ( ( hitOffset == lastOffset ) || ( hitOffset >= unsigned ( m_ByteSize ) ) || ( m_MeasurementCount[hitOffset] == 0 ) )
{
continue;
}
if (m_BacksideChecking) {
// avoid matching of back and front pixels of obstacles
x = cosTheta * measurePoints[i].frontPos.x() -
sinTheta * measurePoints[i].frontPos.y() + robotPose.x();
y = sinTheta * measurePoints[i].frontPos.x() +
cosTheta * measurePoints[i].frontPos.y() + robotPose.y();
geometry_msgs::Point frontPos;
frontPos.x = x;
frontPos.y = y;
Eigen::Vector2i frontPixel = map_tools::toMapCoords(
frontPos, m_metaData.origin, m_metaData.resolution);
frontOffset = frontPixel.x() + m_metaData.width * frontPixel.y();
if ((frontOffset >= unsigned(m_ByteSize)) ||
(m_MeasurementCount[frontOffset] == 0)) {
continue;
}
}
if ( m_BacksideChecking )
{
//avoid matching of back and front pixels of obstacles
x = cosTheta * measurePoints[i].frontPos.x() - sinTheta * measurePoints[i].frontPos.y() + robotPose.x();
y = sinTheta * measurePoints[i].frontPos.x() + cosTheta * measurePoints[i].frontPos.y() + robotPose.y();
geometry_msgs::Point frontPos;
frontPos.x = x;
frontPos.y = y;
Eigen::Vector2i frontPixel = map_tools::toMapCoords(frontPos, m_metaData.origin, m_metaData.resolution);
frontOffset = frontPixel.x() + m_metaData.width*frontPixel.y();
if ( ( frontOffset >= unsigned ( m_ByteSize ) ) || ( m_MeasurementCount[frontOffset] == 0 ) )
{
continue;
}
lastOffset = hitOffset;
// fittingFactor += m_SmoothOccupancyProbability[ offset ];
fittingFactor += m_OccupancyProbability[hitOffset];
}
lastOffset=hitOffset;
//fittingFactor += m_SmoothOccupancyProbability[ offset ];
fittingFactor += m_OccupancyProbability[ hitOffset ];
}
return fittingFactor;
return fittingFactor;
}
template<class DataT>
void OccupancyMap::drawLine ( DataT* data, Eigen::Vector2i& startPixel, Eigen::Vector2i& endPixel, char value )
{
//bresenham algorithm
int xstart = startPixel.x();
int ystart = startPixel.y();
int xend = endPixel.x();
int yend = endPixel.y();
int x, y, t, dist, xerr, yerr, dx, dy, incx, incy;
// compute distances
dx = xend - xstart;
dy = yend - ystart;
// 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 = xstart;
y = ystart;
xerr = dx;
yerr = dy;
// compute cells
for ( t = 0; t < dist; t++ )
{
int index = x + m_metaData.width * y;
// set flag to free if no flag is set
// (do not overwrite occupied cells)
if(index < 0 || index > m_ByteSize)
{
continue;
}
if ( data[index] == NO_CHANGE )
{
data[index] = value;
template <class DataT>
void OccupancyMap::drawLine(DataT* data, Eigen::Vector2i& startPixel,
Eigen::Vector2i& endPixel, char value) {
// bresenham algorithm
int xstart = startPixel.x();
int ystart = startPixel.y();
int xend = endPixel.x();
int yend = endPixel.y();
int x, y, t, dist, xerr, yerr, dx, dy, incx, incy;
// compute distances
dx = xend - xstart;
dy = yend - ystart;
// compute increment
if (dx < 0) {
incx = -1;
dx = -dx;
} else {
incx = dx ? 1 : 0;
}
/* if ( data[index] == OCCUPIED || data[index] == SAFETY_BORDER )
{
return;
}*/
xerr += dx;
yerr += dy;
if ( xerr > dist )
{
xerr -= dist;
x += incx;
if (dy < 0) {
incy = -1;
dy = -dy;
} else {
incy = dy ? 1 : 0;
}
if ( yerr > dist )
{
yerr -= dist;
y += incy;
// which distance is greater?
dist = (dx > dy) ? dx : dy;
// initializing
x = xstart;
y = ystart;
xerr = dx;
yerr = dy;
// compute cells
for (t = 0; t < dist; t++) {
int index = x + m_metaData.width * y;
// set flag to free if no flag is set
// (do not overwrite occupied cells)
if (index < 0 || index > m_ByteSize) {
continue;
}
if (data[index] == NO_CHANGE) {
data[index] = value;
}
/* if ( data[index] == OCCUPIED || data[index] == SAFETY_BORDER )
{
return;
}*/
xerr += dx;
yerr += dy;
if (xerr > dist) {
xerr -= dist;
x += incx;
}
if (yerr > dist) {
yerr -= dist;
y += incy;
}
}
}
}
void OccupancyMap::applyChanges()
{
for ( int y = m_ChangedRegion.minY()+1; y <= m_ChangedRegion.maxY()-1; y++ )
{
int yOffset = m_metaData.width * y;
for ( int x = m_ChangedRegion.minX()+1; x <= m_ChangedRegion.maxX()-1; x++ )
{
int i = x + yOffset;
if ( ( m_CurrentChanges[i] == ::FREE || m_CurrentChanges[i] == ::OCCUPIED ) && m_MeasurementCount[i] < SHRT_MAX )
{
m_MeasurementCount[i]++;
}
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]++ ;
}
void OccupancyMap::applyChanges() {
for (int y = m_ChangedRegion.minY() + 1; y <= m_ChangedRegion.maxY() - 1;
y++) {
int yOffset = m_metaData.width * y;
for (int x = m_ChangedRegion.minX() + 1;
x <= m_ChangedRegion.maxX() - 1; x++) {
int i = x + yOffset;
if ((m_CurrentChanges[i] == ::FREE ||
m_CurrentChanges[i] == ::OCCUPIED) &&
m_MeasurementCount[i] < SHRT_MAX) {
m_MeasurementCount[i]++;
}
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]++;
}
}
}
for (int y = m_ChangedRegion.minY() + 1; y <= m_ChangedRegion.maxY() - 1;
y++) {
int yOffset = m_metaData.width * y;
for (int x = m_ChangedRegion.minX() + 1;
x <= m_ChangedRegion.maxX() - 1; x++) {
int i = x + yOffset;
if (m_OccupancyCount[i] > m_MeasurementCount[i])
m_OccupancyCount[i] = m_MeasurementCount[i];
}
}
}
for ( int y = m_ChangedRegion.minY()+1; y <= m_ChangedRegion.maxY()-1; y++ )
{
int yOffset = m_metaData.width * y;
for ( int x = m_ChangedRegion.minX()+1; x <= m_ChangedRegion.maxX()-1; x++ )
{
int i = x + yOffset;
if(m_OccupancyCount[i]> m_MeasurementCount[i])
m_OccupancyCount[i]=m_MeasurementCount[i];
}}
}
void OccupancyMap::clearChanges()
{
m_ChangedRegion.expand ( 2 );
m_ChangedRegion.clip ( Box2D<int> ( 0,0,m_metaData.width-1,m_metaData.height-1 ) );
for ( int y = m_ChangedRegion.minY(); y <= m_ChangedRegion.maxY(); y++ )
{
int yOffset = m_metaData.width * y;
for ( int x = m_ChangedRegion.minX(); x <= m_ChangedRegion.maxX(); x++ )
{
int i = x + yOffset;
m_CurrentChanges[i] = NO_CHANGE;
void OccupancyMap::clearChanges() {
m_ChangedRegion.expand(2);
m_ChangedRegion.clip(
Box2D<int>(0, 0, m_metaData.width - 1, m_metaData.height - 1));
for (int y = m_ChangedRegion.minY(); y <= m_ChangedRegion.maxY(); y++) {
int yOffset = m_metaData.width * y;
for (int x = m_ChangedRegion.minX(); x <= m_ChangedRegion.maxX(); x++) {
int i = x + yOffset;
m_CurrentChanges[i] = NO_CHANGE;
}
}
}
m_ChangedRegion=Box2D<int> ( m_metaData.width - 1, m_metaData.height - 1, 0, 0 );
m_ChangedRegion =
Box2D<int>(m_metaData.width - 1, m_metaData.height - 1, 0, 0);
}
void OccupancyMap::incrementMeasurementCount ( Eigen::Vector2i p )
{
if(p.x() >= m_metaData.width || p.y() >= m_metaData.height)
{
return;
}
unsigned index = p.x() + m_metaData.width * p.y();
if ( m_CurrentChanges[index] == NO_CHANGE && m_MeasurementCount[index] < USHRT_MAX )
{
m_CurrentChanges[index] = ::FREE;
m_MeasurementCount[index]++;
}
void OccupancyMap::incrementMeasurementCount(Eigen::Vector2i p) {
if (p.x() >= m_metaData.width || p.y() >= m_metaData.height) {
return;
}
unsigned index = p.x() + m_metaData.width * p.y();
if (m_CurrentChanges[index] == NO_CHANGE &&
m_MeasurementCount[index] < USHRT_MAX) {
m_CurrentChanges[index] = ::FREE;
m_MeasurementCount[index]++;
}
}
void OccupancyMap::incrementOccupancyCount ( Eigen::Vector2i p )
{
if(p.x() >= m_metaData.width || p.y() >= m_metaData.height)
{
return;
}
unsigned index = p.x() + m_metaData.width * p.y();
if ( ( m_CurrentChanges[index] == NO_CHANGE || m_CurrentChanges[index] == ::FREE ) && m_MeasurementCount[index] < USHRT_MAX )
{
m_CurrentChanges[index] = ::OCCUPIED;
m_OccupancyCount[index]++;
}
void OccupancyMap::incrementOccupancyCount(Eigen::Vector2i p) {
if (p.x() >= m_metaData.width || p.y() >= m_metaData.height) {
return;
}
unsigned index = p.x() + m_metaData.width * p.y();
if ((m_CurrentChanges[index] == NO_CHANGE ||
m_CurrentChanges[index] == ::FREE) &&
m_MeasurementCount[index] < USHRT_MAX) {
m_CurrentChanges[index] = ::OCCUPIED;
m_OccupancyCount[index]++;
}
}
void OccupancyMap::scaleDownCounts ( int maxCount )
{
clearChanges();
if ( maxCount <= 0 )
{
ROS_WARN("WARNING: argument maxCount is choosen to small, resetting map.");
memset ( m_MeasurementCount, 0, m_ByteSize );
memset ( m_OccupancyCount, 0, m_ByteSize );
}
else
{
for ( unsigned i = 0; i < m_ByteSize; i++ )
{
int scalingFactor = m_MeasurementCount[i] / maxCount;
if ( scalingFactor != 0 )
{
m_MeasurementCount[i] /= scalingFactor;
m_OccupancyCount[i] /= scalingFactor;
}
void OccupancyMap::scaleDownCounts(int maxCount) {
clearChanges();
if (maxCount <= 0) {
ROS_WARN(
"WARNING: argument maxCount is choosen to small, resetting map.");
memset(m_MeasurementCount, 0, m_ByteSize);
memset(m_OccupancyCount, 0, m_ByteSize);
} else {
for (unsigned i = 0; i < m_ByteSize; i++) {
int scalingFactor = m_MeasurementCount[i] / maxCount;
if (scalingFactor != 0) {
m_MeasurementCount[i] /= scalingFactor;
m_OccupancyCount[i] /= scalingFactor;
}
}
}
}
maximizeChangedRegion();
applyChanges();
computeOccupancyProbabilities();
maximizeChangedRegion();
applyChanges();
computeOccupancyProbabilities();
}
void OccupancyMap::markRobotPositionFree()
{
geometry_msgs::Point point;
point.x = 0;
point.y = 0;
point.z = 0;
geometry_msgs::Point endPosWorld = map_tools::transformPoint(point, m_latestMapTransform);
Eigen::Vector2i robotPixel = map_tools::toMapCoords(endPosWorld, m_metaData.origin, m_metaData.resolution);
int width = 0.35 / m_metaData.resolution;
for ( int i = robotPixel.y() - width; i <= robotPixel.y() + width; i++ )
{
for ( int j = robotPixel.x() - width; j <= robotPixel.x() + width; j++ )
{
incrementMeasurementCount ( Eigen::Vector2i ( j, i ) );
void OccupancyMap::markRobotPositionFree() {
geometry_msgs::Point point;
point.x = 0;
point.y = 0;
point.z = 0;
geometry_msgs::Point endPosWorld =
map_tools::transformPoint(point, m_latestMapTransform);
Eigen::Vector2i robotPixel = map_tools::toMapCoords(
endPosWorld, m_metaData.origin, m_metaData.resolution);
int width = 0.35 / m_metaData.resolution;
for (int i = robotPixel.y() - width; i <= robotPixel.y() + width; i++) {
for (int j = robotPixel.x() - width; j <= robotPixel.x() + width; j++) {
incrementMeasurementCount(Eigen::Vector2i(j, i));
}
}
}
Box2D<int> robotBox ( robotPixel.x()-width, robotPixel.y()-width, robotPixel.x() +width, robotPixel.y() +width );
m_ChangedRegion.enclose ( robotBox );
m_ExploredRegion.enclose ( robotBox );
Box2D<int> robotBox(robotPixel.x() - width, robotPixel.y() - width,
robotPixel.x() + width, robotPixel.y() + width);
m_ChangedRegion.enclose(robotBox);
m_ExploredRegion.enclose(robotBox);
}
QImage OccupancyMap::getProbabilityQImage ( int trancparencyThreshold, bool showInaccessible ) const
{
QImage retImage ( m_metaData.width, m_metaData.height, QImage::Format_RGB32 );
for ( int y = 0; y < m_metaData.height; y++ )
{
for ( int x = 0; x < m_metaData.width; x++ )
{
int index = x + y * m_metaData.width;
int value = UNKNOWN;
if ( m_MeasurementCount[index] > 0 )
{
value = static_cast<int> ( ( 1.0 - m_OccupancyProbability[index] ) * 255 );
if ( m_MeasurementCount[index] < trancparencyThreshold )
{
value = static_cast<int> ( ( 0.75 + 0.025 * m_MeasurementCount[index] ) * ( 1.0 - m_OccupancyProbability[index] ) * 255 );
QImage OccupancyMap::getProbabilityQImage(int trancparencyThreshold,
bool showInaccessible) const {
QImage retImage(m_metaData.width, m_metaData.height, QImage::Format_RGB32);
for (int y = 0; y < m_metaData.height; y++) {
for (int x = 0; x < m_metaData.width; x++) {
int index = x + y * m_metaData.width;
int value = UNKNOWN;
if (m_MeasurementCount[index] > 0) {
value = static_cast<int>((1.0 - m_OccupancyProbability[index]) *
255);
if (m_MeasurementCount[index] < trancparencyThreshold) {
value = static_cast<int>(
(0.75 + 0.025 * m_MeasurementCount[index]) *
(1.0 - m_OccupancyProbability[index]) * 255);
}
}
retImage.setPixel(x, y, qRgb(value, value, value));
}
}
retImage.setPixel ( x, y, qRgb ( value, value, value ) );
}
}
return retImage;
return retImage;
}
void OccupancyMap::getOccupancyProbabilityImage ( vector<int8_t>& data, nav_msgs::MapMetaData& metaData)
{
metaData = m_metaData;
data.resize(m_metaData.width * m_metaData.height);
std::fill(data.begin(), data.end(), (int8_t)NOT_SEEN_YET); //note: linker error without strange cast from int8_t to int8_t
for ( int y = m_ExploredRegion.minY(); y <= m_ExploredRegion.maxY(); y++ )
{
int yOffset = m_metaData.width * y;
for ( int x = m_ExploredRegion.minX(); x <= m_ExploredRegion.maxX(); x++ )
{
int i = x + yOffset;
if ( m_MeasurementCount[i] < 1 )
{
continue;
}
if(m_OccupancyProbability[i] == UNKNOWN_LIKELIHOOD)
{
data[i] = NOT_SEEN_YET;
}
else
{
data[i] = (int)(m_OccupancyProbability[i] * 99); //TODO maybe - 2 (or *0.99 or smth)
}
void OccupancyMap::getOccupancyProbabilityImage(
vector<int8_t>& data, nav_msgs::MapMetaData& metaData) {
metaData = m_metaData;
data.resize(m_metaData.width * m_metaData.height);
std::fill(data.begin(), data.end(),
(int8_t)NOT_SEEN_YET); // note: linker error without strange cast
// from int8_t to int8_t
for (int y = m_ExploredRegion.minY(); y <= m_ExploredRegion.maxY(); y++) {
int yOffset = m_metaData.width * y;
for (int x = m_ExploredRegion.minX(); x <= m_ExploredRegion.maxX();
x++) {
int i = x + yOffset;
if (m_MeasurementCount[i] < 1) {
continue;
}
if (m_OccupancyProbability[i] == UNKNOWN_LIKELIHOOD) {
data[i] = NOT_SEEN_YET;
} else {
data[i] = (int)(m_OccupancyProbability[i] *
99); // TODO maybe - 2 (or *0.99 or smth)
}
}
}
}
}
void OccupancyMap::maximizeChangedRegion()
{
m_ChangedRegion=m_ExploredRegion;
void OccupancyMap::maximizeChangedRegion() {
m_ChangedRegion = m_ExploredRegion;
}
void OccupancyMap::applyMasking(const nav_msgs::OccupancyGrid::ConstPtr &msg)
{
if(msg->data.size() != m_ByteSize)
{
ROS_ERROR_STREAM("Size Mismatch between SLAM map (" << m_ByteSize << ") and masking map (" << msg->data.size() << ")");
void OccupancyMap::applyMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg) {
if (msg->data.size() != m_ByteSize) {
ROS_ERROR_STREAM("Size Mismatch between SLAM map ("
<< m_ByteSize << ") and masking map ("
<< msg->data.size() << ")");
return;
}
for(size_t y = 0; y < msg->info.height; y++)
{
for (size_t y = 0; y < msg->info.height; y++) {
int yOffset = msg->info.width * y;
for(size_t x = 0; x < msg->info.width; x++)
{
for (size_t x = 0; x < msg->info.width; x++) {
int i = yOffset + x;
switch(msg->data[i])
{
case homer_mapnav_msgs::ModifyMap::BLOCKED:
case homer_mapnav_msgs::ModifyMap::OBSTACLE:
//increase measure count of cells which were not yet visible to be able to modify unknown areas
if(m_MeasurementCount[i] == 0)
m_MeasurementCount[i] = 10;
m_OccupancyCount[i] = m_MeasurementCount[i];
m_OccupancyProbability[i] = 1.0;
m_ExploredRegion.enclose(x, y);
break;
case homer_mapnav_msgs::ModifyMap::FREE:
//see comment above
if(m_MeasurementCount[i] == 0)
m_MeasurementCount[i] = 10;
m_OccupancyCount[i] = 0;
m_OccupancyProbability[i] = 0.0;
m_ExploredRegion.enclose(x, y);
break;
case homer_mapnav_msgs::ModifyMap::HIGH_SENSITIV:
m_HighSensitive[i] = 1;
break;
switch (msg->data[i]) {
case homer_mapnav_msgs::ModifyMap::BLOCKED:
case homer_mapnav_msgs::ModifyMap::OBSTACLE:
// increase measure count of cells which were not yet
// visible to be able to modify unknown areas
if (m_MeasurementCount[i] == 0) m_MeasurementCount[i] = 10;
m_OccupancyCount[i] = m_MeasurementCount[i];
m_OccupancyProbability[i] = 1.0;
m_ExploredRegion.enclose(x, y);
break;
case homer_mapnav_msgs::ModifyMap::FREE:
// see comment above
if (m_MeasurementCount[i] == 0) m_MeasurementCount[i] = 10;
m_OccupancyCount[i] = 0;
m_OccupancyProbability[i] = 0.0;
m_ExploredRegion.enclose(x, y);
break;
case homer_mapnav_msgs::ModifyMap::HIGH_SENSITIV:
m_HighSensitive[i] = 1;
break;
}
}
}
}
void OccupancyMap::cleanUp()
{
if ( m_OccupancyProbability )
{
delete[] m_OccupancyProbability;
}
if ( m_MeasurementCount )
{
delete[] m_MeasurementCount;
}
if ( m_OccupancyCount )
{
delete[] m_OccupancyCount;
}
if ( m_CurrentChanges )
{
delete[] m_CurrentChanges;
}
if ( m_HighSensitive )
{
delete[] m_HighSensitive;
}
void OccupancyMap::cleanUp() {
if (m_OccupancyProbability) {
delete[] m_OccupancyProbability;
}
if (m_MeasurementCount) {
delete[] m_MeasurementCount;
}
if (m_OccupancyCount) {
delete[] m_OccupancyCount;
}
if (m_CurrentChanges) {
delete[] m_CurrentChanges;
}
if (m_HighSensitive) {
delete[] m_HighSensitive;
}
}
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