ROS基础 - Plugin插件开发代码示例

主要步骤：
1、定义自定义组件类；
2、CMakefile.txt文件编写
3、plugin_description.xml 文件编写
4、package.xml 文件编写
5、编译，设置运行环境并运行
6、rviz::PoseTool 接口源码分析

1. 基本概念

在ROS的开发中，常常会接触到一个名词——插件（plugin）。

在ROS中，插件的概念类似，简单来讲，ROS中的插件（plugin）就是可以动态加载的扩展功能类。

ROS中的pluginlib功能包，提供了加载和卸载plugin的C++库，开发者在使用plugin时，不需要考虑plugin类的链接位置，只需要将plugin注册到pluginlib中，即可直接动态加载。这种插件机制非常方便，开发者不需要改动原本软件的代码，直接将需要的功能通过plugin进行扩展即可。

本文重点描述基于Rviz 中的组件（rviz::PoseTool接口），进行二次开发，形成自定义插件的过程。

功能包结构如下：

2. 定义自定义组件类：class WtsInitialPoseTool : public rviz::PoseTool 2.1. wts_initial_pose_tool.h

#ifndef RVIZ_INITIAL_POSE_TOOL_H
#define RVIZ_INITIAL_POSE_TOOL_H

#include 
#include 
#include 
#include 
#include "rviz/default_plugin/tools/pose_tool.h"

using namespace rviz;

namespace rviz
{
class Arrow;
class DisplayContext;
class StringProperty;
class FloatProperty;

class WtsInitialPoseTool : public rviz::PoseTool
{
    Q_OBJECT
public:
    WtsInitialPoseTool();
    ~WtsInitialPoseTool() override
    {
    }
    void onInitialize() override;
protected:
  void onPoseSet(double x, double y, double theta) override;

private Q_SLOTS:
  void updateTopic();

private:
  ros::NodeHandle nh_;
  ros::Publisher pub_;

  ros::NodeHandle nh2_;
  ros::Publisher pub2_;

  rviz::StringProperty* topic_property_;
    rviz::FloatProperty* std_dev_x_;
    rviz::FloatProperty* std_dev_y_;
    rviz::FloatProperty* std_dev_theta_;
};

} // namespace rviz

#endif

2.2. wts_initial_pose_tool.cpp

#include 
#include 
#include 

#include 
#include 
#include 
#include 

#include "wts_initial_pose_tool.h"


using namespace  rviz;

namespace rviz
{

WtsInitialPoseTool::WtsInitialPoseTool()
{
  shortcut_key_ = 'i'; //edit by limg

  topic_property_ =
      new StringProperty("Topic", "initialpose", "The topic on which to publish initial pose estimates.",
                         getPropertyContainer(), SLOT(updateTopic()), this);
  std_dev_x_ = new FloatProperty("X std deviation", 0.5, "X standard deviation for initial pose [m]",
                                 getPropertyContainer());
  std_dev_y_ = new FloatProperty("Y std deviation", 0.5, "Y standard deviation for initial pose [m]",
                                 getPropertyContainer());
  std_dev_theta_ =
      new FloatProperty("Theta std deviation", M_PI / 12.0,
                        "Theta standard deviation for initial pose [rad]", getPropertyContainer());
  std_dev_x_->setMin(0);
  std_dev_y_->setMin(0);
  std_dev_theta_->setMin(0);

  try
  {
    pub2_ = nh2_.advertise("/2DPoseEstimate", 1);
  }
  catch (const ros::Exception& e)
  {
    ROS_ERROR_STREAM_NAMED("2DPoseEstimate", e.what());
  }

}

void WtsInitialPoseTool::onInitialize()
{
  PoseTool::onInitialize();
  setName("ParkingSpot");
  updateTopic();
}

void WtsInitialPoseTool::updateTopic()
{
  try
  {
    pub_ = nh_.advertise(topic_property_->getStdString(), 1);
  }
  catch (const ros::Exception& e)
  {
    ROS_ERROR_STREAM_NAMED("WtsInitialPoseTool", e.what());
  }
}

void WtsInitialPoseTool::onPoseSet(double x, double y, double theta)
{
  std::string fixed_frame = context_->getFixedFrame().toStdString();
  geometry_msgs::PoseWithCovarianceStamped pose;
  pose.header.frame_id = fixed_frame;
  pose.header.stamp = ros::Time::now();
  pose.pose.pose.position.x = x;
  pose.pose.pose.position.y = y;


  geometry_msgs::Quaternion quat_msg;
  tf2::Quaternion quat;
  quat.setRPY(0.0, 0.0, theta);
  pose.pose.pose.orientation = tf2::toMsg(quat);
  pose.pose.covariance[6 * 0 + 0] = std::pow(std_dev_x_->getFloat(), 2);
  pose.pose.covariance[6 * 1 + 1] = std::pow(std_dev_y_->getFloat(), 2);
  pose.pose.covariance[6 * 5 + 5] = std::pow(std_dev_theta_->getFloat(), 2);
  ROS_INFO("Setting pose: %.3f %.3f %.3f [frame=%s]", x, y, theta, fixed_frame.c_str());
  pub_.publish(pose);

  if (pub2_)
  {
    // unstamped Pose2D message
    geometry_msgs::Pose2D::Ptr pose_msg;
    pose_msg = boost::make_shared();
    pose_msg->x = x;
    pose_msg->y = y;
    pose_msg->theta = theta;
    pub2_.publish(pose_msg);
  }
}

} // end namespace rviz

#include 
PLUGINLIB_EXPORT_CLASS(rviz::WtsInitialPoseTool, rviz::Tool)

注意，这里最后两行，主要用于注册插件。
PLUGINLIB_EXPORT_CLASS两个参数：
参数1：实现类，即子类；
参数2：基类，即父类。

3. CMakefile.txt文件编写

cmake_minimum_required(VERSION 3.0.2) 
project(parking_spot_plot_plugin)

add_definitions(-std=c++17)     
add_compile_options(-std=c++17)

find_package(catkin REQUIRED COMPONENTS rviz)
catkin_package()
include_directories(${catkin_INCLUDE_DIRS})
link_directories(${catkin_LIBRARY_DIRS})

include_directories(/opt/ros/noetic/include)

find_package(Qt5 REQUIRED COMPONENTS Core Widgets Gui)  

add_definitions(-DQT_NO_KEYWORDS)

qt5_wrap_cpp(MOC_FILES
        src/wts_initial_pose_tool.h
)

set(SOURCE_FILES
        src/wts_initial_pose_tool.cpp
        ${MOC_FILES}
)

add_library(${PROJECT_NAME} ${SOURCE_FILES})
qt5_use_modules(${PROJECT_NAME} Core)        #may be not need
qt5_use_modules(${PROJECT_NAME} Widgets)   #may be not need
qt5_use_modules(${PROJECT_NAME} Gui)           #may be not need

target_link_libraries(${PROJECT_NAME}
                        librviz.so librviz_default_plugin.so      #as base rviz develp ,so need librviz.so and librviz_default_plugin.so 
                        ${QT_LIBRARIES} ${catkin_LIBRARIES})
                        

install(TARGETS
  ${PROJECT_NAME}
  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

install(FILES 
  plugin_description.xml
  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

4. plugin_description.xml 文件编写


 
    
        Publish an initial pose for the robot.  After one use, reverts to default tool.

name: 自定义插件名称
type: 实现类的名字（含命名空间）
base_class_type: 基类的名字（含命名空间）

5. package.xml 文件编写

在package.xml中添加下边的代码：

6. 编译，设置运行环境并运行

注：在源代码包根目录下执行catkin_make，并执行source devel/setup.bash，使之生效。

catkin_make
source devel/setup.bash
rosrun rviz rviz

类似效果如下：

打开rviz后，做如下设置：

选择“ParkingSpot”插件，在主操作区进行画图操作，可以监听到‘2DPoseEstimate’话题信息：

7. rviz::PoseTool 接口源码分析 7.1. class WtsInitialPoseTool : public rviz::PoseTool

#include "rviz/default_plugin/tools/pose_tool.h"

7.2. class PoseTool : public Tool

#include

7.3. class RVIZ_EXPORT Tool : public QObject

#ifndef RVIZ_TOOL_H
#define RVIZ_TOOL_H

#include 
#include 
#include 
#include 

#include 
#include 

class QMouseEvent;
class QKeyEvent;

namespace Ogre
{
class SceneManager;
}

namespace rviz
{
class DisplayContext;
class Property;
class RenderPanel;
class ViewportMouseEvent;

class RVIZ_EXPORT Tool : public QObject
{
  Q_OBJECT
public:
  
  Tool();
  ~Tool() override;

  
  void initialize(DisplayContext* context);

  
  virtual Property* getPropertyContainer() const
  {
    return property_container_;
  }

  char getShortcutKey()
  {
    return shortcut_key_;
  }

  bool accessAllKeys()
  {
    return access_all_keys_;
  }

  virtual void activate() = 0;
  virtual void deactivate() = 0;

  virtual void update(float wall_dt, float ros_dt)
  {
    (void)wall_dt;
    (void)ros_dt;
  }

  enum
  {
    Render = 1,
    Finished = 2
  };

  
  virtual int processMouseEvent(ViewportMouseEvent& event)
  {
    (void)event;
    return 0;
  }

  
  virtual int processKeyEvent(QKeyEvent* event, RenderPanel* panel)
  {
    (void)event;
    (void)panel;
    return 0;
  }

  QString getName() const
  {
    return name_;
  }

  
  void setName(const QString& name);

  
  QString getDescription() const
  {
    return description_;
  }
  void setDescription(const QString& description);

  
  virtual QString getClassId() const
  {
    return class_id_;
  }

  
  virtual void setClassId(const QString& class_id)
  {
    class_id_ = class_id;
  }

  
  virtual void load(const Config& config);

  
  virtual void save(Config config) const;

  
  void setIcon(const QIcon& icon);

  
  const QIcon& getIcon()
  {
    return icon_;
  }

  
  void setCursor(const QCursor& cursor);

  
  const QCursor& getCursor()
  {
    return cursor_;
  }

  void setStatus(const QString& message);

Q_SIGNALS:
  void close();
  void nameChanged(const QString& name);

protected:
  
  virtual void onInitialize()
  {
  }

  Ogre::SceneManager* scene_manager_;
  DisplayContext* context_; //rviz/display_context.h virtual

  char shortcut_key_;
  bool access_all_keys_;

  QIcon icon_;

  QCursor cursor_;

private:
  QString class_id_;
  Property* property_container_;
  QString name_;
  QString description_;
};

} // namespace rviz

#endif

参考网址：

ROS基础 - 自动驾驶之高精地图基本概念
https://aduandniuniu.blog.csdn.net/article/details/123037674?spm=1001.2014.3001.5502

ROS基础 - Linux环境下，ROS工程创建示例
https://aduandniuniu.blog.csdn.net/article/details/123100280?spm=1001.2014.3001.5502

ROS基础 - dynamic_reconfigure 动态参数使用示例详解
https://aduandniuniu.blog.csdn.net/article/details/124235396?spm=1001.2014.3001.5502

ROS基础 - ROS Service 服务代码示例
https://aduandniuniu.blog.csdn.net/article/details/124278399?spm=1001.2014.3001.5502

PCL系列：自定义Point 读、写文件代码示例
https://aduandniuniu.blog.csdn.net/article/details/123175013?spm=1001.2014.3001.5502

Linux(Ubuntu) C++ Map使用示例代码（结构体）
https://aduandniuniu.blog.csdn.net/article/details/123082274?spm=1001.2014.3001.5502

Linux环境下，使用Eclipse，创建C++工程 HelloWord
https://aduandniuniu.blog.csdn.net/article/details/123452283?spm=1001.2014.3001.5502

Linux环境下，通过Socket实现进程间通讯代码示例
https://aduandniuniu.blog.csdn.net/article/details/123322139?spm=1001.2014.3001.5502

Linux(Ubuntu) C++文件读写代码示例
https://aduandniuniu.blog.csdn.net/article/details/123082101?spm=1001.2014.3001.5502

apt命令详解
https://wenku.baidu.com/view/f557a6a4f221dd36a32d7375a417866fb84ac0df?fr=shopSearch-pc

Linux C函数实例说明
https://wenku.baidu.com/view/84ab33c01be8b8f67c1cfad6195f312b3169eb08.html

ROS基础 - Plugin插件开发代码示例

C/C++/C#相关栏目本月热门文章