nodelet.cpp

Go to the documentation of this file.

/*****************************************************************************
** Includes
*****************************************************************************/
#include <string>
#include <ecl/threads/thread.hpp>
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include "kobuki_random_walker/random_walker_controller.hpp"


namespace kobuki
{

class RandomWalkerControllerNodelet : public nodelet::Nodelet
{
public:
  RandomWalkerControllerNodelet(): shutdown_requested_(false){};
  ~RandomWalkerControllerNodelet()
  {
    shutdown_requested_ = true;
    NODELET_DEBUG_STREAM("Waiting for update thread to finish. [" << name_ << "]");
    update_thread_.join();
    NODELET_INFO_STREAM("Controller is shutting down. [" << name_ << "]");
  }

  virtual void onInit()
  {
    ros::NodeHandle nh_priv = this->getPrivateNodeHandle();

    // resolve node(let) name
    std::string name = nh_priv.getUnresolvedNamespace();
    int pos = name.find_last_of('/');
    name_ = name.substr(pos + 1);

    NODELET_INFO_STREAM("Initialising nodelet ... [" << name_ << "]");
    controller_.reset(new RandomWalkerController(nh_priv, name_));

    nh_priv.param("update_rate", update_rate_, 10.0);
    NODELET_INFO_STREAM("Controller will spin at " << update_rate_ << " hz. [" << name_ << "]");

    // Initialises the controller
    if (controller_->init())
    {
      NODELET_INFO_STREAM("Nodelet initialised. Spinning up update thread. [" << name_ << "]");
      update_thread_.start(&RandomWalkerControllerNodelet::update, *this);
    }
    else
    {
      NODELET_ERROR_STREAM("Couldn't initialise nodelet! Please restart. [" << name_ << "]");
    }
  }

private:
  boost::shared_ptr<RandomWalkerController> controller_;
  double update_rate_;
  std::string name_;
  ecl::Thread update_thread_;
  bool shutdown_requested_;

  void update()
  {
    ros::Rate spin_rate(update_rate_);
    while (ros::ok() && !shutdown_requested_)
    {
      controller_->spin();
      spin_rate.sleep();
    }
  }
};

} // namespace kobuki

PLUGINLIB_EXPORT_CLASS(kobuki::RandomWalkerControllerNodelet,
                       nodelet::Nodelet);