Commit befa60d7 authored by Daniel Müller's avatar Daniel Müller
Browse files

Added more const and calls by const reference where applicable; Added TODO...

Added more const and calls by const reference where applicable; Added TODO file because of Qt dependency
parent 851536ff
2019-067-02 muellerd There is some QtGui dependency only in OccupancyMap. Is it at all still in use?
......@@ -151,7 +151,6 @@ public:
*/
~OccupancyMap();
/*
/**
* @return The resolution of the map in m.
*/
......@@ -177,7 +176,7 @@ public:
/**
* @return Probability of pixel p being occupied.
*/
float getOccupancyProbability(Eigen::Vector2i p);
float getOccupancyProbability(const Eigen::Vector2i& p) const;
/**
* @brief This function inserts the data of a laserscan into the map.
......@@ -189,9 +188,10 @@ public:
* called for the endpoint.
* @param laserData The laser data msg.
*/
void insertLaserData(sensor_msgs::LaserScan::ConstPtr laserData, tf::Transform transform);
void insertLaserData(
sensor_msgs::LaserScan::ConstPtr laserData, const tf::Transform& transform);
void insertRanges(vector<RangeMeasurement> ranges, ros::Time stamp = ros::Time::now());
void insertRanges(const vector<RangeMeasurement>& ranges, ros::Time stamp = ros::Time::now());
/**
* @brief gives a list specially processed coordinates to be used for
......@@ -208,12 +208,14 @@ public:
* This factor is NOT normalized, it is a positive float between 0
* and FLOAT_MAX
*/
float computeScore(Pose robotPose, const std::vector<MeasurePoint>& measurePoints);
float computeScore(const Pose& robotPose, const std::vector<MeasurePoint>& measurePoints) const;
/**
* @return QImage of size m_metaData.width, m_metaData.height with values of
* m_OccupancyProbability scaled to 0-254
*/
// TODO (DM): This is the only reference to Qt functionality! Seperate from basic class if still
// needed somewhere.
QImage getProbabilityQImage(int trancparencyThreshold, bool showInaccessible) const;
// puma2::ColorImageRGB8* getUpdateImage( bool withMap=true ); TODO
......@@ -232,7 +234,7 @@ public:
* @param[out] height Height of data array
* @param[out] resolution Resolution of the map (m_metaData.resolution)
*/
void getOccupancyProbabilityImage(vector<int8_t>& data, nav_msgs::MapMetaData& metaData);
void getOccupancyProbabilityImage(vector<int8_t>& data, nav_msgs::MapMetaData& metaData) const;
/**
* This method marks free the position of the robot (according to its
......@@ -275,14 +277,14 @@ protected:
* This method increments m_MeasurementCount for pixel p.
* @param p Pixel that has been measured.
*/
void incrementMeasurementCount(Eigen::Vector2i p);
void incrementMeasurementCount(const Eigen::Vector2i& p);
/**
* This method increments the occupancy count int m_OccupancyCount for pixel
* p.
* @param p Occupied pixel.
*/
void incrementOccupancyCount(Eigen::Vector2i p);
void incrementOccupancyCount(const Eigen::Vector2i& p);
/**
* This method increments m_MeasurementCount and if neccessary
......
......@@ -123,7 +123,7 @@ int OccupancyMap::height() const
return m_metaData.height;
}
float OccupancyMap::getOccupancyProbability(Eigen::Vector2i p)
float OccupancyMap::getOccupancyProbability(const Eigen::Vector2i& p) const
{
if (p.y() >= m_metaData.height || p.x() >= m_metaData.width)
{
......@@ -182,7 +182,7 @@ void OccupancyMap::computeOccupancyProbabilities()
}
void OccupancyMap::insertLaserData(
sensor_msgs::LaserScan::ConstPtr laserData, tf::Transform transform)
sensor_msgs::LaserScan::ConstPtr laserData, const tf::Transform& transform)
{
m_latestMapTransform = transform;
markRobotPositionFree();
......@@ -258,12 +258,11 @@ void OccupancyMap::insertLaserData(
insertRanges(ranges, laserData->header.stamp);
}
void OccupancyMap::insertRanges(vector<RangeMeasurement> ranges, ros::Time stamp)
void OccupancyMap::insertRanges(const vector<RangeMeasurement>& ranges, ros::Time stamp)
{
if (ranges.size() < 1)
{
if (ranges.empty())
return;
}
clearChanges();
Eigen::Vector2i lastEndPixel;
......@@ -579,7 +578,8 @@ vector<MeasurePoint> OccupancyMap::getMeasurePoints(sensor_msgs::LaserScanConstP
return result;
}
float OccupancyMap::computeScore(Pose robotPose, const std::vector<MeasurePoint>& measurePoints)
float OccupancyMap::computeScore(
const Pose& robotPose, const std::vector<MeasurePoint>& measurePoints) const
{
// This is a very simple implementation, using only the end point of the
// beam.
......@@ -772,7 +772,7 @@ void OccupancyMap::clearChanges()
m_ChangedRegion = Box2D<int>(m_metaData.width - 1, m_metaData.height - 1, 0, 0);
}
void OccupancyMap::incrementMeasurementCount(Eigen::Vector2i p)
void OccupancyMap::incrementMeasurementCount(const Eigen::Vector2i& p)
{
if (p.x() < 0 || p.x() >= m_metaData.width || p.y() < 0 || p.y() >= m_metaData.height)
{
......@@ -786,7 +786,7 @@ void OccupancyMap::incrementMeasurementCount(Eigen::Vector2i p)
}
}
void OccupancyMap::incrementOccupancyCount(Eigen::Vector2i p)
void OccupancyMap::incrementOccupancyCount(const Eigen::Vector2i& p)
{
if (p.x() < 0 || p.x() >= m_metaData.width || p.y() < 0 || p.y() >= m_metaData.height)
{
......@@ -870,7 +870,7 @@ QImage OccupancyMap::getProbabilityQImage(int trancparencyThreshold, bool showIn
}
void OccupancyMap::getOccupancyProbabilityImage(
vector<int8_t>& data, nav_msgs::MapMetaData& metaData)
vector<int8_t>& data, nav_msgs::MapMetaData& metaData) const
{
metaData = m_metaData;
data.resize(m_metaData.width * m_metaData.height);
......
Markdown is supported
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