Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Implement your own controller for Kobuki

New in groovy

Overview

In this tutorial you will learn how to write and run a simple nodelet-based controller for Kobuki.

Implementing a new controller

Overview

In this section we will create a simple "blink, when a bumper is pressed" controller, which will be similar to the Kobuki's safety controller.

You find the code explained in the following paragraphs in the kobuki_controller_tutorial package. We will also make use of the default controller from yocs_controllers.

Setting up the package

First create a new package for this controller in your workspace. We use roscreate-pkg to add the dependencies right away:

$ catkin_create_pkg kobuki_controller_tutorial roscpp yocs_controllers std_msgs kobuki_msgs nodelet

Defining the controller

Next create a new class for your controller, e.g. yocl::BumpBlinkController, in kobuki_controller_tutorial/include/kobuki_controller_tutorial/bump_blink_controller.hpp. This class inherits from yocl::DefaultController, which defines enable/disable/getState methods and also forces you do write an init() function, which is important for nodelets as we will see later.

Here is the class and methods definition in:

https://raw.github.com/yujinrobot/kobuki/devel/kobuki_controller_tutorial/include/kobuki_controller_tutorial/bump_blink_controller.hpp

  50 #include <ros/ros.h>
  51 #include <std_msgs/Empty.h>
  52 #include <yocs_controllers/default_controller.hpp>
  53 #include <kobuki_msgs/BumperEvent.h>
  54 #include <kobuki_msgs/Led.h>
  55 
  56 namespace kobuki
  57 {
  58 
  59 /**
  60  * @ brief A simple bump-blink-controller
  61  *
  62  * A simple nodelet-based controller for Kobuki, which makes one of Kobuki's LEDs blink, when a bumper is pressed.
  63  */
  64 class BumpBlinkController : public yocs::Controller
  65 {
  66 public:
  67   BumpBlinkController(ros::NodeHandle& nh, std::string& name) : Controller(), nh_(nh), name_(name){};
  68   ~BumpBlinkController(){};
  69 
  70   /**
  71    * Set-up necessary publishers/subscribers
  72    * @return true, if successful
  73    */
  74   bool init()
  75   {
  76     enable_controller_subscriber_ = nh_.subscribe("enable", 10, &BumpBlinkController::enableCB, this);
  77     disable_controller_subscriber_ = nh_.subscribe("disable", 10, &BumpBlinkController::disableCB, this);
  78     bumper_event_subscriber_ = nh_.subscribe("events/bumper", 10, &BumpBlinkController::bumperEventCB, this);
  79     // choose between led1 and led2
  80     blink_publisher_ = nh_.advertise< kobuki_msgs::Led >("commands/led1", 10);
  81     return true;
  82   };
  83 
  84 private:
  85   ros::NodeHandle nh_;
  86   std::string name_;
  87   ros::Subscriber enable_controller_subscriber_, disable_controller_subscriber_;
  88   ros::Subscriber bumper_event_subscriber_;
  89   ros::Publisher blink_publisher_;
  90 
  91   /**
  92    * @brief ROS logging output for enabling the controller
  93    * @param msg incoming topic message
  94    */
  95   void enableCB(const std_msgs::EmptyConstPtr msg);
  96 
  97   /**
  98    * @brief ROS logging output for disabling the controller
  99    * @param msg incoming topic message
 100    */
 101   void disableCB(const std_msgs::EmptyConstPtr msg);
 102 
 103   /**
 104    * @brief Turns on/off a LED, when a bumper is pressed/released
 105    * @param msg incoming topic message
 106    */
 107   void bumperEventCB(const kobuki_msgs::BumperEventConstPtr msg);
 108 };
 109 
 110 void BumpBlinkController::enableCB(const std_msgs::EmptyConstPtr msg)
 111 {
 112   if (this->enable())
 113   {
 114     ROS_INFO_STREAM("Controller has been enabled. [" << name_ << "]");
 115   }
 116   else
 117   {
 118     ROS_INFO_STREAM("Controller was already enabled. [" << name_ <<"]");
 119   }
 120 };
 121 
 122 void BumpBlinkController::disableCB(const std_msgs::EmptyConstPtr msg)
 123 {
 124   if (this->disable())
 125   {
 126     ROS_INFO_STREAM("Controller has been disabled. [" << name_ <<"]");
 127   }
 128   else
 129   {
 130     ROS_INFO_STREAM("Controller was already disabled. [" << name_ <<"]");
 131   }
 132 };
 133 
 134 void BumpBlinkController::bumperEventCB(const kobuki_msgs::BumperEventConstPtr msg)
 135 {
 136   if (this->getState()) // check, if the controller is active
 137   {
 138     // Preparing LED message
 139     kobuki_msgs::LedPtr led_msg_ptr;
 140     led_msg_ptr.reset(new kobuki_msgs::Led());
 141 
 142     if (msg->state == kobuki_msgs::BumperEvent::PRESSED)
 143     {
 144       ROS_INFO_STREAM("Bumper pressed. Turning LED on. [" << name_ << "]");
 145       led_msg_ptr->value = kobuki_msgs::Led::GREEN;
 146       blink_publisher_.publish(led_msg_ptr);
 147     }
 148     else // kobuki_msgs::BumperEvent::RELEASED
 149     {
 150       ROS_INFO_STREAM("Bumper released. Turning LED off. [" << name_ << "]");
 151       led_msg_ptr->value = kobuki_msgs::Led::BLACK;
 152       blink_publisher_.publish(led_msg_ptr);
 153     }
 154   }
 155 };
 156 
 157 } // namespace kobuki
 158 

This class implementation already defines the full behaviour of the controller. The callback bumperEventCB is triggered every time a bumper of Kobuki is pressed or released. It is using a publisher to turn the LED on or off depending on whether the bumper is pressed or released.

Now, we will create a nodelet. To do so, we simple wrap the controller class in a nodelet class. We will put the implementation in kobuki_controller_tutorial/src/nodelet.cpp:

https://raw.github.com/yujinrobot/kobuki/devel/kobuki_controller_tutorial/src/nodelet.cpp

  41 #include <nodelet/nodelet.h>
  42 #include <pluginlib/class_list_macros.h>
  43 #include "kobuki_controller_tutorial/bump_blink_controller.hpp"
  44 
  45 
  46 namespace kobuki
  47 {
  48 
  49 /**
  50  * @brief Nodelet-wrapper of the BumpBlinkController class
  51  */
  52 class BumpBlinkControllerNodelet : public nodelet::Nodelet
  53 {
  54 public:
  55   BumpBlinkControllerNodelet(){};
  56   ~BumpBlinkControllerNodelet(){}
  57 
  58   /**
  59    * @brief Initialise the nodelet
  60    *
  61    * This function is called, when the nodelet manager loads the nodelet.
  62    */
  63   virtual void onInit()
  64   {
  65     ros::NodeHandle nh = this->getPrivateNodeHandle();
  66 
  67     // resolve node(let) name
  68     std::string name = nh.getUnresolvedNamespace();
  69     int pos = name.find_last_of('/');
  70     name = name.substr(pos + 1);
  71 
  72     NODELET_INFO_STREAM("Initialising nodelet... [" << name << "]");
  73     controller_.reset(new BumpBlinkController(nh, name));
  74 
  75     // Initialises the controller
  76     if (controller_->init())
  77     {
  78       NODELET_INFO_STREAM("Nodelet initialised. [" << name << "]");
  79     }
  80     else
  81     {
  82       NODELET_ERROR_STREAM("Couldn't initialise nodelet! Please restart. [" << name << "]");
  83     }
  84   }
  85 private:
  86   boost::shared_ptr<BumpBlinkController> controller_;
  87 };
  88 
  89 } // namespace kobuki
  90 
  91 PLUGINLIB_EXPORT_CLASS(kobuki::BumpBlinkControllerNodelet,
  92                        nodelet::Nodelet);

The macro at the bottom of the file together with the information we will add in the next section enables loading this nodelet class by a nodelet manager.

Nodelet prerequisites

In order to let nodelet managers find and load your new nodelet, we need to add few information to our controller package.

1. Make sure our new nodelet is compiled as a library. If you are not already familiar with catkin, head over there to find out how to configure your catkin package.

2. Add a plugins folder to your package and put the lines below into kobuki_controller_tutorial/plugins/nodelet_plugins.xml:

<library path="lib/libbump_blink_controller_nodelet">
  <class name="kobuki_controller_tutorial/BumpBlinkControllerNodelet" type="kobuki::BumpBlinkControllerNodelet" base_class_type="nodelet::Nodelet">
    <description>
      Nodelet for a simple blink when bump controller
    </description>
  </class>
</library>

3. Let other packages know of our new nodelet. To do so we export the information of the XML file above in the manifest.xml. Add the lines following lines:

  <export>
    <nodelet plugin="${prefix}/plugins/nodelet_plugins.xml" />
  </export>

Compile everything

Simply do:

catkin_make --pkg kobuki_controller_tutorial

Add a launcher

We will create a simple app (launch file) to fire up our new controller on top of Kobuki basics (kobuki_node/minimal.launch). In order to use a nodelet, we need to first launch a nodelet manager and then load the desired nodelet into it. Since the kobuki_node - also know as mobile_base - is a nodelet itself, Kobuki's minimal.launch already starts a nodelet manager and loads it into that manager. Hence, we can use the same nodelet manager.

Our launch file looks like this:

<launch>
  <node pkg="nodelet" type="nodelet" name="bump_blink_controller" args="load kobuki_controller_tutorial/BumpBlinkControllerNodelet mobile_base_nodelet_manager">
    <remap from="bump_blink_controller/events/bumper" to="mobile_base/events/bumper"/>
    <remap from="bump_blink_controller/commands/led1" to="mobile_base/commands/led1"/>
  </node>
</launch>

Testing the new controller

First, we need to start kobuki_node/launch/minimal.launch or turtlebot_bringup/launch/minimal.launch. If one of them is already running, you will need to kill and restart it. Otherwise the nodelet manager will not know of our new nodelet.

So, execute in one terminal:

$ roslaunch kobuki_node minimal.launch --screen

Notice the --screen. Use this argument to enable verbose logging on the screen. We will use this output in a minute.

Launch the BumpBlink app in a separate terminal:

$ roslaunch kobuki_controller_tutorial bump_blink_app.launch --screen

The first terminal should output logging, which shows you that the nodelet has been loaded. For example:

Debug:   class_loader::class_loader_core: Attempting to load library /opt/kobuki_workspace/kobuki/kobuki_controller_tutorial/lib/libbump_blink_controller_nodelet.so...

Debug:   class_loader::class_loader_core: Registering plugin factory for class = kobuki::BumpBlinkControllerNodelet, ClassLoader* = 0xcc1ee0 and library name /opt/kobuki_workspace/kobuki/kobuki_controller_tutorial/lib/libbump_blink_controller_nodelet.so.

Debug:   class_loader::class_loader_core: Registration of kobuki::BumpBlinkControllerNodelet complete.

Debug:   class_loader::MultiLibraryClassLoader: Attempting to create instance of class type kobuki::BumpBlinkControllerNodelet.
[ INFO] [1354517909.555323591]: Initialising nodelet... [bump_blink_controller]
[ INFO] [1354517909.559157066]: Nodelet initialised. [bump_blink_controller

Next, we need to activate our controller. To do so execute the following line in a third terminal:

$ rostopic pub /bump_blink_controller/enable std_msgs/Empty

You should see a message in the first terminal indicating that we turned on the controller:

[ INFO] [1354442317.388062241]: Controller has been enabled. [bump_blink_controller]

Now, when you press Kobuki's bumper, one of the Kobuki's programmable LEDs (depending on which topic you send the LED message to) should light up.

The output on the first terminal should be something like this:

[ INFO] [1354442318.210606516]: Bumper pressed. Turning LED on. [bump_blink_controller]
[ INFO] [1354442318.569076592]: Bumper released. Turning LED off. [bump_blink_controller]

Now, you can enjoy the Kobuki disco!

Level Up!

You just have reached the next level and a new tutorial has been unlocked: Test your new skills at Implement a complex controller.