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

Added c++14 support and use of unique_ptr instead of raw pointer for...

Added c++14 support and use of unique_ptr instead of raw pointer for HyperParticleFilter attribute in slam node
parent 7cd01402
cmake_minimum_required(VERSION 2.8.3)
project(homer_mapping)
add_compile_options(-std=c++14 -fpermissive)
find_package(
catkin REQUIRED COMPONENTS
......
......@@ -4,6 +4,7 @@
#include <fstream>
#include <iostream>
#include <map>
#include <memory>
#include <sstream>
#include <stdlib.h>
#include <vector>
......@@ -58,11 +59,6 @@ public:
*/
virtual void init();
/**
* The destructor deletes the filter thread instance.
*/
virtual ~SlamNode();
private:
/**
* Callback methods for all incoming messages
......@@ -90,7 +86,7 @@ private:
void sendTfAndPose(Pose pose, ros::Time stamp);
void sendPoseArray(std::vector<Pose> poses);
void sendPoseArray(const std::vector<Pose>& poses);
Pose getInterpolatedPose(nav_msgs::Odometry::ConstPtr pose1, nav_msgs::Odometry::ConstPtr pose2,
ros::Time laserTime);
......@@ -111,7 +107,7 @@ private:
/**
* This variable stores a pointer to the hyper slam filter
*/
HyperSlamFilter* m_HyperSlamFilter;
std::unique_ptr<HyperSlamFilter> m_HyperSlamFilter;
/**
* Scatter variances in self localization.
......@@ -159,5 +155,12 @@ private:
ros::Publisher m_PoseStampedPublisher;
ros::Publisher m_PoseArrayPublisher;
ros::Publisher m_SLAMMapPublisher;
/**
* Config values
*/
const unsigned int MAX_ODOM_QUEUE_SIZE = 20;
};
#include <homer_mapping/slam_node.h>
SlamNode::SlamNode(ros::NodeHandle* nh)
: m_HyperSlamFilter(0)
{
// subscribe to topics
std::string scan_topic;
......@@ -34,14 +33,7 @@ SlamNode::SlamNode(ros::NodeHandle* nh)
m_PoseArrayPublisher = nh->advertise<geometry_msgs::PoseArray>("/pose_array", 2);
m_SLAMMapPublisher = nh->advertise<nav_msgs::OccupancyGrid>("/homer_mapping/slam_map", 1, true);
bool base_fp_param;
if (nh->hasParam("/homer_mapping/map_childof_base_footprint"))
if (ros::param::get("/homer_mapping/map_childof_base_footprint", base_fp_param))
{
m_UseBaseFootprint = base_fp_param;
}
else
m_UseBaseFootprint = false;
nh->param<bool>("/homer_mapping/map_childof_base_footprint", m_UseBaseFootprint, false);
init();
}
......@@ -56,7 +48,7 @@ void SlamNode::init()
int particleFilterNum;
ros::param::get("/particlefilter/particle_num", particleNum);
ros::param::get("/particlefilter/hyper_slamfilter/particlefilter_num", particleFilterNum);
m_HyperSlamFilter = new HyperSlamFilter(particleFilterNum, particleNum);
m_HyperSlamFilter = std::make_unique<HyperSlamFilter>(particleFilterNum, particleNum);
m_LastMapSendTime = ros::Time(0);
......@@ -67,20 +59,12 @@ void SlamNode::init()
m_HyperSlamFilter->setRobotPose(Pose(0, 0, 0), m_ScatterVarXY, m_ScatterVarTheta);
}
SlamNode::~SlamNode()
{
delete m_HyperSlamFilter;
}
void SlamNode::resetMaps()
{
ROS_INFO("Resetting maps..");
if (m_HyperSlamFilter)
{
delete m_HyperSlamFilter;
}
m_HyperSlamFilter = 0;
m_HyperSlamFilter.reset();
init();
}
......@@ -176,10 +160,8 @@ void SlamNode::sendTfAndPose(Pose pose, ros::Time stamp)
if (m_UseBaseFootprint)
{
tf::Transform transform2 = transform.inverse();
// ROS_INFO_STREAM(transform2);
m_tfBroadcaster.sendTransform(
tf::StampedTransform(transform2, stamp, "base_footprint", "map"));
tf::StampedTransform(transform.inverse(), stamp, "base_footprint", "map"));
}
else
{
......@@ -187,7 +169,7 @@ void SlamNode::sendTfAndPose(Pose pose, ros::Time stamp)
}
}
void SlamNode::sendPoseArray(std::vector<Pose> poses)
void SlamNode::sendPoseArray(const std::vector<Pose>& poses)
{
if (m_PoseArrayPublisher.getNumSubscribers() > 0)
{
......@@ -214,7 +196,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() > 20)
if (m_odom_queue.size() > MAX_ODOM_QUEUE_SIZE)
{
m_odom_queue.erase(m_odom_queue.begin());
}
......@@ -228,7 +210,7 @@ void SlamNode::callbackOdometry(const nav_msgs::Odometry::ConstPtr& msg)
{
for (j = m_laser_queue.size() - 1; j > -1; j--)
{
// find a laserscan in between two odometry readings (or at
// find a laserscan in between two odometry readings (or at
// the same time)
if ((m_laser_queue.at(j)->header.stamp >= m_odom_queue.at(i - 1)->header.stamp)
&& (m_odom_queue.at(i)->header.stamp >= m_laser_queue.at(j)->header.stamp))
......@@ -238,9 +220,7 @@ void SlamNode::callbackOdometry(const nav_msgs::Odometry::ConstPtr& msg)
}
}
if (got_match)
{
break;
}
}
if (got_match)
......@@ -301,17 +281,11 @@ Pose SlamNode::getInterpolatedPose(
float timeFactor;
if (d1.toSec() == 0.0)
{
timeFactor = 0.0f;
}
else if (d2.toSec() == 0.0)
{
timeFactor = 1.0f;
}
else
{
timeFactor = d1.toSec() / d2.toSec();
}
return lastOdometryPose.interpolate(currentOdometryPose, timeFactor);
}
......@@ -325,12 +299,13 @@ 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)
{
// TODO (DM): Using shared_ptrs instead of raw pointers would be best here as they "clean up"
// themselves when not used anymore
OccupancyMap* occMap = new OccupancyMap(msg);
m_HyperSlamFilter->setOccupancyMap(occMap);
m_HyperSlamFilter->setMapping(false);
......@@ -345,13 +320,14 @@ void SlamNode::callbackMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg)
sendMapDataMessage(ros::Time::now());
}
//---------------------------------------------------------------------------------------------------------------------
int main(int argc, char** argv)
{
ros::init(argc, argv, "homer_mapping");
ros::NodeHandle nh;
SlamNode slamNode(&nh);
// TODO (DM): Set spinning rate via config and take 160 as default value?
ros::Rate loop_rate(160);
while (ros::ok())
{
......
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