diff --git a/homer_map_manager/src/map_manager_node.cpp b/homer_map_manager/src/map_manager_node.cpp
index babc26f262d16443b18ac1ce61d6d9975dcccc31..d8989a6734c096712c6faf421e2b14aafdf4dca6 100644
--- a/homer_map_manager/src/map_manager_node.cpp
+++ b/homer_map_manager/src/map_manager_node.cpp
@@ -392,7 +392,7 @@ void MapManagerNode::poseCallback(
bool MapManagerNode::saveMapService(homer_mapnav_msgs::SaveMap::Request& req,
homer_mapnav_msgs::SaveMap::Response& res) {
- // ROS_INFO_STREAM("Saving map "<<req->folder);
+ ROS_INFO_STREAM("Saving map "<<req.folder.data);
MapGenerator map_saver(std::string(req.folder.data));
nav_msgs::OccupancyGrid::ConstPtr SLAMMap =
m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER);
diff --git a/homer_mapping/config/homer_mapping.yaml b/homer_mapping/config/homer_mapping.yaml
index 4ae14b9b3c1ff7e89ea8d2d757092c24ad006cc7..12c2d79d9fb2d13a50194e425f9e9bd6db0b96ea 100644
--- a/homer_mapping/config/homer_mapping.yaml
+++ b/homer_mapping/config/homer_mapping.yaml
@@ -18,7 +18,6 @@
/particlefilter/particle_num: 1000
/particlefilter/max_rotation_per_second: 0.1 # maximal rotation in radiants if mapping is performed. if rotation is bigger, mapping is interrupted
-/particlefilter/wait_time: 0.05 # minimum time to wait between two slam steps in seconds
#the map is only updated when the robot has turned a minimal angle, has moved a minimal distance or a maximal time has passed
/particlefilter/update_min_move_angle: 2 # degrees
diff --git a/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h b/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
index ecf26ef618910be8e86089a03027b327429eb75a..dc87774afffb2714bed6854c44405d98fbe21fe4 100644
--- a/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
+++ b/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
@@ -124,9 +124,7 @@ public:
/**
* Constructor for a loaded map.
*/
- OccupancyMap(float*& occupancyProbability, geometry_msgs::Pose origin,
- float resolution, int width, int height,
- Box2D<int> exploredRegion);
+ OccupancyMap(const nav_msgs::OccupancyGrid::ConstPtr& msg);
/**
* Copy constructor, copies all members inclusive the arrays that lie behind
diff --git a/homer_mapping/include/homer_mapping/slam_node.h b/homer_mapping/include/homer_mapping/slam_node.h
index cd4a6ff22b0b246f8092e04ceed8ca56e9185f08..9fc6f8fbc53bf4c2c312e588f8b58e22793d6725 100644
--- a/homer_mapping/include/homer_mapping/slam_node.h
+++ b/homer_mapping/include/homer_mapping/slam_node.h
@@ -34,7 +34,8 @@ class HyperSlamFilter;
* @class SlamNode
*
* @author Malte Knauf, Stephan Wirth, Susanne Maur (RX), David Gossow (RX),
- * Christian Fuchs (R12), Nicolai Wojke (R14), Susanne Thierfelder (R16)
+ * Christian Fuchs (R12), Nicolai Wojke (R14), Susanne Thierfelder
+ * (R16), Florian Polster (R28)
*
*
* @brief The Simultaneous localization and mapping module
@@ -82,11 +83,6 @@ private:
*/
void resetMaps();
- /**
- * This function processes the current odometry data in combination with the
- * last send odometry and laser informations to pass on corresponding data
- * to the filter threads.
-
/**
* This method retrieves the current map of the slam filter and sends a map
* data message containing the map.
@@ -104,12 +100,10 @@ private:
/**
* This variables stores the last odometry measurement as reference that is
* used
- * to compute the pose of the robot during a specific laserscan.
+ * to compute the transformation to the new odometry measurement
*/
Pose m_lastUsedPose;
- Pose m_LastLikeliestPose;
-
/**
* This variable stores the time the last map message was sent to be able to
* compute the time for the next map send.
@@ -139,11 +133,6 @@ private:
std::vector<sensor_msgs::LaserScan::ConstPtr> m_laser_queue;
std::vector<nav_msgs::Odometry::ConstPtr> m_odom_queue;
- /**
- * duration to wait between two particle filter steps
- */
- ros::Duration m_WaitDuration;
-
/**
* Broadcasts the transform map -> base_link
*/
diff --git a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
index 02979b3f8729b7bdbcda360839f03fd5720ccb1b..a1e009fef3aca81161aeaa60fb0abd3bb8109f9f 100644
--- a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
+++ b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
@@ -26,25 +26,17 @@ OccupancyMap::OccupancyMap()
initMembers();
}
-OccupancyMap::OccupancyMap(float*& occupancyProbability,
- geometry_msgs::Pose origin, float resolution,
- int width, int height, Box2D<int> exploredRegion)
+OccupancyMap::OccupancyMap(const nav_msgs::OccupancyGrid::ConstPtr& msg)
{
- m_metaData.origin = origin;
- m_metaData.resolution = resolution;
- m_metaData.width = width;
- m_metaData.height = height;
+ m_metaData = msg->info;
m_ByteSize = m_metaData.width * m_metaData.height;
initMembers();
- m_ExploredRegion = exploredRegion;
- m_ChangedRegion = exploredRegion;
-
- for (unsigned i = 0; i < m_ByteSize; i++)
+ for (unsigned i = 0; i < msg->data.size(); i++)
{
- if (occupancyProbability[i] != 0.5)
+ if (msg->data[i] != -1)
{
- m_MapPoints[i].OccupancyProbability = occupancyProbability[i];
+ m_MapPoints[i].OccupancyProbability = msg->data[i] / 100.0;
m_MapPoints[i].MeasurementCount = LOADED_MEASURECOUNT;
m_MapPoints[i].OccupancyCount =
m_MapPoints[i].OccupancyProbability * LOADED_MEASURECOUNT;
diff --git a/homer_mapping/src/slam_node.cpp b/homer_mapping/src/slam_node.cpp
index 64c99eede061a5a4119db3fecdff0ddb137449d6..c3b3e9c72412339d206635a399e4ee162098339d 100644
--- a/homer_mapping/src/slam_node.cpp
+++ b/homer_mapping/src/slam_node.cpp
@@ -2,18 +2,18 @@
SlamNode::SlamNode(ros::NodeHandle* nh) : m_HyperSlamFilter(0)
{
- init();
-
// subscribe to topics
m_LaserScanSubscriber = nh->subscribe<sensor_msgs::LaserScan>(
"/scan", 1, &SlamNode::callbackLaserScan, this);
m_OdometrySubscriber = nh->subscribe<nav_msgs::Odometry>(
"/odom", 1, &SlamNode::callbackOdometry, this);
+
m_UserDefPoseSubscriber = nh->subscribe<geometry_msgs::Pose>(
"/homer_mapping/userdef_pose", 1, &SlamNode::callbackUserDefPose, this);
m_InitialPoseSubscriber =
nh->subscribe<geometry_msgs::PoseWithCovarianceStamped>(
"/initialpose", 1, &SlamNode::callbackInitialPose, this);
+
m_DoMappingSubscriber = nh->subscribe<std_msgs::Bool>(
"/homer_mapping/do_mapping", 1, &SlamNode::callbackDoMapping, this);
m_ResetMapSubscriber = nh->subscribe<std_msgs::Empty>(
@@ -30,27 +30,22 @@ SlamNode::SlamNode(ros::NodeHandle* nh) : m_HyperSlamFilter(0)
nh->advertise<geometry_msgs::PoseStamped>("/pose", 2);
m_PoseArrayPublisher =
nh->advertise<geometry_msgs::PoseArray>("/pose_array", 2);
- m_SLAMMapPublisher =
- nh->advertise<nav_msgs::OccupancyGrid>("/homer_mapping/slam_map", 1);
+ m_SLAMMapPublisher = nh->advertise<nav_msgs::OccupancyGrid>(
+ "/homer_mapping/slam_map", 1, true);
- sendTfAndPose(Pose(0, 0, 0), ros::Time::now());
- m_HyperSlamFilter->setRobotPose(Pose(0, 0, 0), m_ScatterVarXY,
- m_ScatterVarTheta);
+ init();
}
void SlamNode::init()
{
- double waitTime;
- ros::param::get("/particlefilter/wait_time", waitTime);
- m_WaitDuration = ros::Duration(waitTime);
ros::param::get("/selflocalization/scatter_var_xy", m_ScatterVarXY);
ros::param::get("/selflocalization/scatter_var_theta", m_ScatterVarTheta);
m_DoMapping = true;
int particleNum;
- ros::param::get("/particlefilter/particle_num", particleNum);
int particleFilterNum;
+ ros::param::get("/particlefilter/particle_num", particleNum);
ros::param::get("/particlefilter/hyper_slamfilter/particlefilter_num",
particleFilterNum);
m_HyperSlamFilter = new HyperSlamFilter(particleFilterNum, particleNum);
@@ -59,6 +54,10 @@ void SlamNode::init()
m_laser_queue.clear();
m_odom_queue.clear();
+
+ sendTfAndPose(Pose(0, 0, 0), ros::Time::now());
+ m_HyperSlamFilter->setRobotPose(Pose(0, 0, 0), m_ScatterVarXY,
+ m_ScatterVarTheta);
}
SlamNode::~SlamNode()
@@ -70,19 +69,13 @@ void SlamNode::resetMaps()
{
ROS_INFO("Resetting maps..");
- delete m_HyperSlamFilter;
+ if (m_HyperSlamFilter)
+ {
+ delete m_HyperSlamFilter;
+ }
m_HyperSlamFilter = 0;
init();
- sendTfAndPose(Pose(0, 0, 0), ros::Time::now());
-
- m_LastLikeliestPose.set(0.0, 0.0);
- m_LastLikeliestPose.setTheta(0.0f);
-
- m_HyperSlamFilter->setRobotPose(Pose(0, 0, 0), m_ScatterVarXY,
- m_ScatterVarTheta);
-
- // sendMapDataMessage();
}
void SlamNode::callbackResetHigh(const std_msgs::Empty::ConstPtr& msg)
@@ -95,15 +88,13 @@ void SlamNode::sendMapDataMessage(ros::Time mapTime)
std::vector<int8_t> mapData;
nav_msgs::MapMetaData metaData;
- OccupancyMap* occMap =
- m_HyperSlamFilter->getBestSlamFilter()->getLikeliestMap();
- occMap->getOccupancyProbabilityImage(mapData, metaData);
+ m_HyperSlamFilter->getBestSlamFilter()
+ ->getLikeliestMap()
+ ->getOccupancyProbabilityImage(mapData, metaData);
nav_msgs::OccupancyGrid mapMsg;
- std_msgs::Header header;
- header.stamp = mapTime;
- header.frame_id = "map";
- mapMsg.header = header;
+ mapMsg.header.stamp = mapTime;
+ mapMsg.header.frame_id = "map";
mapMsg.info = metaData;
mapMsg.data = mapData;
@@ -130,7 +121,7 @@ void SlamNode::callbackInitialPose(
void SlamNode::callbackLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg)
{
m_laser_queue.push_back(msg);
- if (m_laser_queue.size() > 5) // Todo param
+ if (m_laser_queue.size() > 5)
{
m_laser_queue.erase(m_laser_queue.begin());
}
@@ -147,13 +138,13 @@ void SlamNode::sendTfAndPose(Pose pose, ros::Time stamp)
tf::Quaternion quatTF = tf::createQuaternionFromYaw(pose.theta());
geometry_msgs::Quaternion quatMsg;
tf::quaternionTFToMsg(quatTF, quatMsg);
-
+
poseMsg.pose.orientation = quatMsg;
m_PoseStampedPublisher.publish(poseMsg);
tf::Transform transform(quatTF, tf::Vector3(pose.x(), pose.y(), 0.0));
- m_tfBroadcaster.sendTransform(tf::StampedTransform(
- transform, stamp, "map", "base_link"));
+ m_tfBroadcaster.sendTransform(
+ tf::StampedTransform(transform, stamp, "map", "base_link"));
}
void SlamNode::sendPoseArray(std::vector<Pose> poses)
@@ -180,7 +171,7 @@ void SlamNode::sendPoseArray(std::vector<Pose> poses)
void SlamNode::callbackOdometry(const nav_msgs::Odometry::ConstPtr& msg)
{
m_odom_queue.push_back(msg);
- if (m_odom_queue.size() > 5) // Todo param
+ if (m_odom_queue.size() > 20)
{
m_odom_queue.erase(m_odom_queue.begin());
}
@@ -219,6 +210,7 @@ void SlamNode::callbackOdometry(const nav_msgs::Odometry::ConstPtr& msg)
m_odom_queue.at(i - 1), m_odom_queue.at(i), laserData->header.stamp);
Transformation2D trans = last_interpolatedPose - m_lastUsedPose;
+ m_lastUsedPose = last_interpolatedPose;
// Rotate transformation to pose theta
float x = trans.x() * cos(-last_interpolatedPose.theta()) -
@@ -231,23 +223,20 @@ void SlamNode::callbackOdometry(const nav_msgs::Odometry::ConstPtr& msg)
// SLAM STEP
m_HyperSlamFilter->filter(rotTrans, laserData);
- m_lastUsedPose = last_interpolatedPose;
- m_LastLikeliestPose =
- m_HyperSlamFilter->getBestSlamFilter()->getLikeliestPose();
+ Pose pose = m_HyperSlamFilter->getBestSlamFilter()->getLikeliestPose();
- sendTfAndPose(m_LastLikeliestPose, laserData->header.stamp);
+ sendTfAndPose(pose, laserData->header.stamp);
// send map max. every 500 ms
- if ((laserData->header.stamp - m_LastMapSendTime).toSec() > 0.5)
+ if ((laserData->header.stamp - m_LastMapSendTime).toSec() > 0.5 &&
+ m_DoMapping)
{
sendMapDataMessage(laserData->header.stamp);
m_LastMapSendTime = laserData->header.stamp;
}
sendPoseArray(m_HyperSlamFilter->getBestSlamFilter()->getParticlePoses());
-
- //Remove already used data from queues
-
+ // Remove already used data from queues
m_odom_queue.erase(m_odom_queue.begin(), m_odom_queue.begin() + (i - 1));
m_laser_queue.erase(m_laser_queue.begin(), m_laser_queue.begin() + j);
}
@@ -269,7 +258,6 @@ Pose SlamNode::getInterpolatedPose(nav_msgs::Odometry::ConstPtr pose1,
pose2->pose.pose.position.y,
tf::getYaw(pose2->pose.pose.orientation));
-
ros::Duration d1 = laserTime - lastOdometryTime;
ros::Duration d2 = currentOdometryTime - lastOdometryTime;
@@ -300,56 +288,18 @@ void SlamNode::callbackDoMapping(const std_msgs::Bool::ConstPtr& msg)
void SlamNode::callbackResetMap(const std_msgs::Empty::ConstPtr& msg)
{
+ (void)msg;
resetMaps();
}
void SlamNode::callbackLoadedMap(const nav_msgs::OccupancyGrid::ConstPtr& msg)
{
- float res = msg->info.resolution;
- int height = msg->info.height; // cell size
- int width = msg->info.width; // cell size
- // if(height!=width) {
- // ROS_ERROR("Height != width in loaded map");
- // return;
- //}
-
- // convert map vector from ros format to robbie probability array
- float* map = new float[msg->data.size()];
- // generate exploredRegion
- int minX = INT_MIN;
- int minY = INT_MIN;
- int maxX = INT_MAX;
- int maxY = INT_MAX;
- 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++)
- {
- int i = yOffset + x;
- if (msg->data[i] == -1)
- map[i] = 0.5;
- else
- map[i] = msg->data[i] / 100.0;
-
- if (map[i] != 0.5)
- {
- if (minX == INT_MIN || minX > (int)x)
- minX = (int)x;
- if (minY == INT_MIN || minY > (int)y)
- minY = (int)y;
- if (maxX == INT_MAX || maxX < (int)x)
- maxX = (int)x;
- if (maxY == INT_MAX || maxY < (int)y)
- maxY = (int)y;
- }
- }
- }
- Box2D<int> exploredRegion = Box2D<int>(minX, minY, maxX, maxY);
- OccupancyMap* occMap = new OccupancyMap(map, msg->info.origin, res, width,
- height, exploredRegion);
+ OccupancyMap* occMap = new OccupancyMap(msg);
m_HyperSlamFilter->setOccupancyMap(occMap);
- m_HyperSlamFilter->setMapping(false); // is this already done by gui message?
+ m_HyperSlamFilter->setMapping(false);
+ m_DoMapping = false;
ROS_INFO_STREAM("Replacing occupancy map");
+ sendMapDataMessage(ros::Time::now());
}
void SlamNode::callbackMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg)