Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

use std::unordered_map with std::unique_ptr for boost::ptr_map #91

Open
wants to merge 3 commits into
base: indigo-devel
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 6 additions & 0 deletions nodelet/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,12 @@ catkin_package(
DEPENDS Boost
)

# http://www.ros.org/reps/rep-0003.html
# require c++11 since kinetic
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 11)
endif()

include_directories(
include
${catkin_INCLUDE_DIRS}
Expand Down
37 changes: 21 additions & 16 deletions nodelet/src/loader.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,10 @@
* POSSIBILITY OF SUCH DAMAGE.
*/

#include <memory>
#include <unordered_map>
#include <utility>

#include <nodelet/loader.h>
#include <nodelet/nodelet.h>
#include <nodelet/detail/callback_queue.h>
Expand All @@ -40,7 +44,6 @@
#include <nodelet/NodeletList.h>
#include <nodelet/NodeletUnload.h>

#include <boost/ptr_container/ptr_map.hpp>
#include <boost/utility.hpp>

/*
Expand Down Expand Up @@ -111,8 +114,10 @@ class LoaderROS
// If requested, create bond to sister process
if (res.success && !req.bond_id.empty())
{
bond::Bond* bond = new bond::Bond(nh_.getNamespace() + "/bond", req.bond_id);
bond_map_.insert(req.name, bond);
/// @todo Report error if a bond with same id has already been created
bond_map_.insert(std::make_pair(req.name, std::move(std::unique_ptr<bond::Bond>(new bond::Bond(nh_.getNamespace() + "/bond", req.bond_id)))));

const auto& bond = bond_map_[req.name];
bond->setCallbackQueue(&bond_callback_queue_);
bond->setBrokenCallback(boost::bind(&LoaderROS::unload, this, req.name));
bond->start();
Expand All @@ -139,11 +144,12 @@ class LoaderROS
}

// break the bond, if there is one
M_stringToBond::iterator it = bond_map_.find(name);
const auto it = bond_map_.find(name);
if (it != bond_map_.end()) {
// disable callback for broken bond, as we are breaking it intentially now
it->second->setBrokenCallback(boost::function<void(void)>());
// erase (and break) bond

// erase and break bond
bond_map_.erase(name);
}

Expand All @@ -167,8 +173,7 @@ class LoaderROS

ros::CallbackQueue bond_callback_queue_;
ros::AsyncSpinner bond_spinner_;
typedef boost::ptr_map<std::string, bond::Bond> M_stringToBond;
M_stringToBond bond_map_;
std::unordered_map<std::string, std::unique_ptr<bond::Bond> > bond_map_;
};

// Owns a Nodelet and its callback queues
Expand Down Expand Up @@ -207,8 +212,8 @@ struct Loader::Impl
boost::function<void ()> refresh_classes_;
boost::shared_ptr<detail::CallbackQueueManager> callback_manager_; // Must outlive nodelets_

typedef boost::ptr_map<std::string, ManagedNodelet> M_stringToNodelet;
M_stringToNodelet nodelets_; ///<! A map of name to currently constructed nodelets
// map of name to currently constructed nodelets
std::unordered_map<std::string, std::unique_ptr<ManagedNodelet> > nodelets_;

Impl()
{
Expand Down Expand Up @@ -309,8 +314,9 @@ bool Loader::load(const std::string &name, const std::string& type, const ros::M
}
ROS_DEBUG("Done loading nodelet %s", name.c_str());

ManagedNodelet* mn = new ManagedNodelet(p, impl_->callback_manager_.get());
impl_->nodelets_.insert(const_cast<std::string&>(name), mn); // mn now owned by boost::ptr_map
impl_->nodelets_.insert(std::make_pair(name, std::move(std::unique_ptr<ManagedNodelet>(new ManagedNodelet(p, impl_->callback_manager_.get())))));

const auto& mn = impl_->nodelets_[name];
try {
mn->st_queue->disable();
mn->mt_queue->disable();
Expand All @@ -323,7 +329,7 @@ bool Loader::load(const std::string &name, const std::string& type, const ros::M

ROS_DEBUG("Done initing nodelet %s", name.c_str());
} catch(...) {
Impl::M_stringToNodelet::iterator it = impl_->nodelets_.find(name);
const auto it = impl_->nodelets_.find(name);
if (it != impl_->nodelets_.end())
{
impl_->nodelets_.erase(it);
Expand All @@ -337,7 +343,7 @@ bool Loader::load(const std::string &name, const std::string& type, const ros::M
bool Loader::unload (const std::string & name)
{
boost::mutex::scoped_lock lock (lock_);
Impl::M_stringToNodelet::iterator it = impl_->nodelets_.find(name);
const auto it = impl_->nodelets_.find(name);
if (it != impl_->nodelets_.end())
{
impl_->nodelets_.erase(it);
Expand All @@ -359,10 +365,9 @@ std::vector<std::string> Loader::listLoadedNodelets()
{
boost::mutex::scoped_lock lock (lock_);
std::vector<std::string> output;
Impl::M_stringToNodelet::iterator it = impl_->nodelets_.begin();
for (; it != impl_->nodelets_.end(); ++it)
for (const auto& it : impl_->nodelets_)
{
output.push_back(it->first);
output.push_back(it.first);
}
return output;
}
Expand Down