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]