From 08845a129975b8b83eb4a48309a3e05f9c4d8683 Mon Sep 17 00:00:00 2001
From: Florian Polster <fpolster@uni-koblenz.de>
Date: Sun, 19 Mar 2017 15:02:49 +0100
Subject: [PATCH] when map was loaded transform wasn't fetched -> fixed
---
homer_map_manager/src/map_manager_node.cpp | 1 +
.../homer_mapping/OccupancyMap/OccupancyMap.h | 5 +-
.../src/OccupancyMap/OccupancyMap.cpp | 59 +++++++++++--------
.../src/ParticleFilter/SlamFilter.cpp | 2 +-
homer_mapping/src/slam_node.cpp | 2 +-
5 files changed, 43 insertions(+), 26 deletions(-)
diff --git a/homer_map_manager/src/map_manager_node.cpp b/homer_map_manager/src/map_manager_node.cpp
index d8989a67..2c21a8aa 100644
--- a/homer_map_manager/src/map_manager_node.cpp
+++ b/homer_map_manager/src/map_manager_node.cpp
@@ -400,6 +400,7 @@ bool MapManagerNode::saveMapService(homer_mapnav_msgs::SaveMap::Request& req,
m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER);
map_saver.save(SLAMMap, maskingMap, m_POIManager->getList(),
m_ROIManager->getList());
+ return true;
}
bool MapManagerNode::loadMapService(homer_mapnav_msgs::LoadMap::Request& req,
diff --git a/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h b/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
index dc87774a..9660e7ce 100644
--- a/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
+++ b/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
@@ -280,6 +280,8 @@ public:
void changeMapSize(int x_add_left, int y_add_up, int x_add_right,
int y_add_down);
+ tf::StampedTransform getLaserTransform(std::string frame_id);
+
protected:
/**
* This method increments m_MeasurementCount for pixel p.
@@ -449,7 +451,8 @@ protected:
/**
* ros transformation laser to base_link
*/
- tf::StampedTransform m_laserTransform;
+ std::map<std::string, tf::StampedTransform> m_savedTransforms;
+
tf::Transform m_latestMapTransform;
};
#endif
diff --git a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
index a1e009fe..a02330a8 100644
--- a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
+++ b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
@@ -29,7 +29,7 @@ OccupancyMap::OccupancyMap()
OccupancyMap::OccupancyMap(const nav_msgs::OccupancyGrid::ConstPtr& msg)
{
m_metaData = msg->info;
- m_ByteSize = m_metaData.width * m_metaData.height;
+ m_ByteSize = msg->data.size();
initMembers();
for (unsigned i = 0; i < msg->data.size(); i++)
@@ -64,10 +64,10 @@ void OccupancyMap::initMembers()
m_MapPoints.resize(m_ByteSize);
- 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();
+ m_ChangedRegion.enclose(
+ Box2D<int>(0, 0, m_metaData.width - 1, m_metaData.height - 1));
+ m_ExploredRegion.enclose(
+ Box2D<int>(0, 0, m_metaData.width - 1, m_metaData.height - 1));
}
OccupancyMap& OccupancyMap::operator=(const OccupancyMap& occupancyMap)
@@ -194,19 +194,6 @@ void OccupancyMap::insertLaserData(sensor_msgs::LaserScan::ConstPtr laserData,
tf::Transform transform)
{
m_latestMapTransform = transform;
- try
- {
- 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;
@@ -216,8 +203,8 @@ void OccupancyMap::insertLaserData(sensor_msgs::LaserScan::ConstPtr laserData,
float lastValidRange = m_FreeReadingDistance;
RangeMeasurement rangeMeasurement;
- rangeMeasurement.sensorPos.x = m_laserTransform.getOrigin().getX();
- rangeMeasurement.sensorPos.y = m_laserTransform.getOrigin().getY();
+ rangeMeasurement.sensorPos.x = getLaserTransform(laserData->header.frame_id).getOrigin().getX();
+ rangeMeasurement.sensorPos.y = getLaserTransform(laserData->header.frame_id).getOrigin().getY();
for (unsigned int i = 0; i < laserData->ranges.size(); i++)
{
@@ -248,7 +235,7 @@ void OccupancyMap::insertLaserData(sensor_msgs::LaserScan::ConstPtr laserData,
pin.setX(cos(alpha) * range);
pin.setY(sin(alpha) * range);
pin.setZ(0);
- pout = m_laserTransform * pin;
+ pout = getLaserTransform(laserData->header.frame_id) * pin;
rangeMeasurement.endPos.x = pout.x();
rangeMeasurement.endPos.y = pout.y();
rangeMeasurement.range = range;
@@ -262,7 +249,7 @@ void OccupancyMap::insertLaserData(sensor_msgs::LaserScan::ConstPtr laserData,
pin.setX(cos(alpha) * laserData->ranges[i]);
pin.setY(sin(alpha) * laserData->ranges[i]);
pin.setZ(0);
- pout = m_laserTransform * pin;
+ pout = getLaserTransform(laserData->header.frame_id) * pin;
rangeMeasurement.endPos.x = pout.x();
rangeMeasurement.endPos.y = pout.y();
rangeMeasurement.range = laserData->ranges[i];
@@ -484,6 +471,32 @@ double OccupancyMap::evaluateByContrast()
return (0);
}
+tf::StampedTransform OccupancyMap::getLaserTransform(std::string frame_id)
+{
+if(m_savedTransforms.find(frame_id) != m_savedTransforms.end())
+{
+ return m_savedTransforms[frame_id];
+}
+else
+{
+ try
+ {
+ m_tfListener.waitForTransform("/base_link", frame_id,
+ ros::Time(0), ros::Duration(0.2));
+ m_tfListener.lookupTransform("/base_link", frame_id,
+ ros::Time(0), m_savedTransforms[frame_id]);
+ return m_savedTransforms[frame_id];
+ }
+ catch (tf::TransformException ex)
+ {
+ ROS_ERROR_STREAM(ex.what());
+ ROS_ERROR_STREAM("need transformation from base_link to laser!");
+ }
+}
+
+return tf::StampedTransform();
+}
+
vector<MeasurePoint>
OccupancyMap::getMeasurePoints(sensor_msgs::LaserScanConstPtr laserData)
{
@@ -508,7 +521,7 @@ OccupancyMap::getMeasurePoints(sensor_msgs::LaserScanConstPtr laserData)
pin.setY(sin(alpha) * laserData->ranges[i]);
pin.setZ(0);
- pout = m_laserTransform * pin;
+ pout = getLaserTransform(laserData->header.frame_id) * pin;
Point2D hitPos(pout.x(), pout.y());
diff --git a/homer_mapping/src/ParticleFilter/SlamFilter.cpp b/homer_mapping/src/ParticleFilter/SlamFilter.cpp
index 5bc9c1c9..6e1dab2e 100644
--- a/homer_mapping/src/ParticleFilter/SlamFilter.cpp
+++ b/homer_mapping/src/ParticleFilter/SlamFilter.cpp
@@ -280,7 +280,7 @@ void SlamFilter::filter(Transformation2D trans,
{
ROS_INFO_STREAM("first run!");
m_FirstRun = false;
- // only do mapping, save first pose as reference
+ // only do mapping
if (m_DoMapping)
{
tf::Transform transform(tf::createQuaternionFromYaw(0.0),
diff --git a/homer_mapping/src/slam_node.cpp b/homer_mapping/src/slam_node.cpp
index c3b3e9c7..3e079014 100644
--- a/homer_mapping/src/slam_node.cpp
+++ b/homer_mapping/src/slam_node.cpp
@@ -299,7 +299,7 @@ void SlamNode::callbackLoadedMap(const nav_msgs::OccupancyGrid::ConstPtr& msg)
m_HyperSlamFilter->setMapping(false);
m_DoMapping = false;
ROS_INFO_STREAM("Replacing occupancy map");
- sendMapDataMessage(ros::Time::now());
+ sendMapDataMessage(msg->info.map_load_time);
}
void SlamNode::callbackMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg)
--
GitLab