Kobuki入门教程-实现自己的控制器

Kobuki入门教程-实现自己的控制器

说明

• 介绍开发自己基于Nodelets的控制器

• 编写控制器实现当发生碰撞会亮灯，类似kobuki_safety_controller

步骤

新的控制器包

• 进入工作空间，生成新的控制器包

$ cd ~/catkin_ws/src
$ catkin_create_pkg kobuki_controller_tutorial roscpp yocs_controllers std_msgs kobuki_msgs nodelet
$ cd ..
$ catkin_make

定义控制类

• 定义类 yocl::BumpBlinkController, 继承自yocl::DefaultController，定义enable/disable/getState和init函数

• 在kobuki_controller_tutorial/include/kobuki_controller_tutorial/bump_blink_controller.hpp

• 完整代码，点击访问

$ cd ~/catkin_ws/src/kobuki_controller_tutorial
$ mkdir -p include/kobuki_controller_tutorial
$ cd include/kobuki_controller_tutorial
$ vim bump_blink_controller.hpp

• 输入如下代码：

#include <ros/ros.h>
#include <std_msgs/Empty.h>
#include <yocs_controllers/default_controller.hpp>
#include <kobuki_msgs/BumperEvent.h>
#include <kobuki_msgs/Led.h>

namespace kobuki
{

/**
 * @ brief A simple bump-blink-controller
 *
 * A simple nodelet-based controller for Kobuki, which makes one of Kobuki's LEDs blink, when a bumper is pressed.
 */
class BumpBlinkController : public yocs::Controller
{
public:
  BumpBlinkController(ros::NodeHandle& nh, std::string& name) : Controller(), nh_(nh), name_(name){};
  ~BumpBlinkController(){};

  /**
   * Set-up necessary publishers/subscribers
   * @return true, if successful
   */
  bool init()
  {
    enable_controller_subscriber_ = nh_.subscribe("enable", 10, &BumpBlinkController::enableCB, this);
    disable_controller_subscriber_ = nh_.subscribe("disable", 10, &BumpBlinkController::disableCB, this);
    bumper_event_subscriber_ = nh_.subscribe("events/bumper", 10, &BumpBlinkController::bumperEventCB, this);
    // choose between led1 and led2
    blink_publisher_ = nh_.advertise< kobuki_msgs::Led >("commands/led1", 10);
    return true;
  };

private:
  ros::NodeHandle nh_;
  std::string name_;
  ros::Subscriber enable_controller_subscriber_, disable_controller_subscriber_;
  ros::Subscriber bumper_event_subscriber_;
  ros::Publisher blink_publisher_;

  /**
   * @brief ROS logging output for enabling the controller
   * @param msg incoming topic message
   */
  void enableCB(const std_msgs::EmptyConstPtr msg);

  /**
   * @brief ROS logging output for disabling the controller
   * @param msg incoming topic message
   */
  void disableCB(const std_msgs::EmptyConstPtr msg);

  /**
   * @brief Turns on/off a LED, when a bumper is pressed/released
   * @param msg incoming topic message
   */
  void bumperEventCB(const kobuki_msgs::BumperEventConstPtr msg);
};

void BumpBlinkController::enableCB(const std_msgs::EmptyConstPtr msg)
{
  if (this->enable())
  {
    ROS_INFO_STREAM("Controller has been enabled. [" << name_ << "]");
  }
  else
  {
    ROS_INFO_STREAM("Controller was already enabled. [" << name_ <<"]");
  }
};

void BumpBlinkController::disableCB(const std_msgs::EmptyConstPtr msg)
{
  if (this->disable())
  {
    ROS_INFO_STREAM("Controller has been disabled. [" << name_ <<"]");
  }
  else
  {
    ROS_INFO_STREAM("Controller was already disabled. [" << name_ <<"]");
  }
};

void BumpBlinkController::bumperEventCB(const kobuki_msgs::BumperEventConstPtr msg)
{
  if (this->getState()) // check, if the controller is active
  {
    // Preparing LED message
    kobuki_msgs::LedPtr led_msg_ptr;
    led_msg_ptr.reset(new kobuki_msgs::Led());

    if (msg->state == kobuki_msgs::BumperEvent::PRESSED)
    {
      ROS_INFO_STREAM("Bumper pressed. Turning LED on. [" << name_ << "]");
      led_msg_ptr->value = kobuki_msgs::Led::GREEN;
      blink_publisher_.publish(led_msg_ptr);
    }
    else // kobuki_msgs::BumperEvent::RELEASED
    {
      ROS_INFO_STREAM("Bumper released. Turning LED off. [" << name_ << "]");
      led_msg_ptr->value = kobuki_msgs::Led::BLACK;
      blink_publisher_.publish(led_msg_ptr);
    }
  }
};

} // namespace kobuki

• 代码分析

• 上面代码执行了所有的处理

• 在保险杆被撞击和释放时候触发，bumperEventCB 回调函数总会调用。

• 当撞击或释放，发布器会发布话题去打开或关闭LED灯

创建nodelet类

• 新增kobuki_controller_tutorial/src/nodelet.cpp文件

• 完整代码，点击访问

• 输入如下代码：

#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include "kobuki_controller_tutorial/bump_blink_controller.hpp"


namespace kobuki
{

/**
 * @brief Nodelet-wrapper of the BumpBlinkController class
 */
class BumpBlinkControllerNodelet : public nodelet::Nodelet
{
public:
  BumpBlinkControllerNodelet(){};
  ~BumpBlinkControllerNodelet(){}

  /**
   * @brief Initialise the nodelet
   *
   * This function is called, when the nodelet manager loads the nodelet.
   */
  virtual void onInit()
  {
    ros::NodeHandle nh = this->getPrivateNodeHandle();

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

    NODELET_INFO_STREAM("Initialising nodelet... [" << name << "]");
    controller_.reset(new BumpBlinkController(nh, name));

    // Initialises the controller
    if (controller_->init())
    {
      NODELET_INFO_STREAM("Nodelet initialised. [" << name << "]");
    }
    else
    {
      NODELET_ERROR_STREAM("Couldn't initialise nodelet! Please restart. [" << name << "]");
    }
  }
private:
  boost::shared_ptr<BumpBlinkController> controller_;
};

} // namespace kobuki

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

• 在底部增加的宏定义，让nodelet管理器加载这个nodelet类。

Nodelet的先决条件

• 为了让nodelet管理器加载这个nodelet类，需要加载信息到控制包

• 确保我们的新nodelet编译为库。使用catkin_make编译

• 增加plugins目录到控制包，新建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>

• 让其他的包知道我们的新nodelet,在控制包目录的manifest.xml，增加几行

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

• 编译

$ catkin_make --pkg kobuki_controller_tutorial

• 增加launch文件，为了使用nodelet,要使用nodelet管理器加载这个nodelet. kobuki_node,作为mobile_base也是一个nodelet. 我们可以使用同一个管理器，mobile_base_nodelet_manager。

• 在控制包下，新建目录launch, 新增bump_blink_app.launch

• 内容如下

<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>

测试新控制器

• 启动minimal.launch，增加--screen，输出更多日志信息

$ roslaunch kobuki_node minimal.launch --screen

• 新终端启动bump_blink_app.launch

$ roslaunch kobuki_controller_tutorial bump_blink_app.launch --screen

• 启动后第一个终端会显示日志：

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

• 显示nodelet已经加载

• 新终端打开，激活控制器

$ rostopic pub /bump_blink_controller/enable std_msgs/Empty

• 显示：

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

• 按下和释放保险杆显示

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

作者：znjrobot

北京智能佳科技有限公司 站点 QQ交谈