Robotics StackExchange | Archived questions

ROS2 rviz2 dockable panel plugin

I am trying implement rviz2 plugin in ROS2. I have referred user guide from the rviz for the development. Currently there no tutorial available to develop rviz dockable panel in ROS2. Therefore I have referred MoveIt2 and Navigation 2 package for their rviz plugin implementation in ROS2. Here is my code.

demo_widget.h:

#ifndef DEMO_WIDGET_H
#define DEMO_WIDGET_H

#include <QWidget> 
#include <memory>
#include <vector>

#include "ui_pointcloud_controller.h"

#ifndef Q_MOC_RUN
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#endif

namespace Ui {
    class DemoWidgetUI;
}

namespace prc_demo_panel
{
    class DemoWidget : public QWidget
    {
        Q_OBJECT
        public:
            DemoWidget(QWidget * parent = 0);
            ~DemoWidget() override;
        public Q_SLOTS:            
        private Q_SLOTS:
        protected:
            std::unique_ptr<Ui::DemoWidgetUI> ui_;

        private:
            rclcpp::Node::SharedPtr _node;
            rclcpp::Publisher<std_msgs::msg::String>::SharedPtr _publisher;
    };
}

demo_widget.cpp:

#include <prc_demo_panel/demo_widget.h>


namespace prc_demo_panel
{
    DemoWidget::DemoWidget(QWidget *parent): QWidget(parent), ui_(new Ui::DemoWidgetUI)
    {
        ui_->setupUi(this);

        connect(ui_->pushButton_1, &QPushButton::clicked, this, &DemoWidget::buttonOne);
        connect(ui_->pushButton_2, &QPushButton::clicked, this, &DemoWidget::buttonTwo);

        auto options = rclcpp::NodeOptions().arguments({"--ros-args --remap __node:=dialog_action_client"});
        _node = std::make_shared<rclcpp::Node>("_", options);

        _publisher = _node->create_publisher<std_msgs::msg::String>("gui_control", 10);
    }

}

demo_panel.h:

#ifndef DEMO_PANEL_H
#define DEMO_PANEL_H

#include <QVBoxLayout>

#include <rviz_common/panel.hpp>
#include <prc_demo_panel/demo_widget.h>

namespace prc_demo_panel
{
    class Demo_widget;

    class DemoPanel : public rviz_common::Panel
    {
        Q_OBJECT
        public:
            explicit DemoPanel(QWidget * parent = 0);
            virtual ~DemoPanel();

            void onInitialize() override;
            void save(rviz_common::Config config) const override;
            void load(const rviz_common::Config &conf) override;


        private:
            DemoWidget *_widget;

    };
}

#endif // RVIZ_PANEL_H

demo_panel.cpp:

#include "prc_demo_panel/demo_panel.h"

#include <QVBoxLayout>

#include <memory>
#include <vector>
#include <utility>
#include <rviz_common/display_context.hpp>

namespace prc_demo_panel
{
    DemoPanel::DemoPanel(QWidget * parent) : Panel(parent)
    {
        _widget = new DemoWidget(parent);
        QVBoxLayout * layout = new QVBoxLayout;
        layout->addWidget(_widget);
        layout->setContentsMargins(10, 10, 10, 10);
        setLayout(layout);
    }

    void DemoPanel::save(rviz_common::Config config) const
    {
        Panel::save(config);
    }

    void DemoPanel::load(const rviz_common::Config &conf)
    {
        Panel::load(conf);
    }

    void DemoPanel::onInitialize()
    {
        auto node = getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
    }
}

#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(prc_demo_panel::DemoPanel, rviz_common::Panel)

CMakeLists.txt:

cmake_minimum_required(VERSION 3.5)
project(prc_demo_panel)
.
.
.

# find dependencies

find_package(Qt5 REQUIRED COMPONENTS Core Gui Widgets Test Concurrent)

set (THIS_PACKAGE_INCLUDE_DEPENDS
  rclcpp  
  class_loader
  pluginlib
  Qt5
  rviz2
  rviz_common
  rviz_default_plugins
  rviz_rendering
  rviz_ogre_vendor
)

include_directories(
  include
)


# I prefer the Qt signals and slots to avoid defining "emit", "slots",
# etc because they can conflict with boost signals, so define QT_NO_KEYWORDS here
# e.g. http://muddyazian.blogspot.de/2012/04/getting-qt-app-working-with-boost-using.html
set(CMAKE_INCLUDE_CURRENT_DIR ON)
set(CMAKE_AUTOMOC ON)
#set(CMAKE_AUTORCC ON)
#add_definitions(-DQT_NO_KEYWORDS)

# Define source file
set(${PROJECT_NAME}_SRCS
  src/demo_panel.cpp
  src/demo_widget.cpp
  #src/plugin_init.cpp
)

# Define header file
set(${PROJECT_NAME}_HDRS
  include/${PROJECT_NAME}/demo_panel.h
  include/${PROJECT_NAME}/demo_widget.h
)

# Define ui file
set(${PROJECT_NAME}_UIS
  resource/pointcloud_controller.ui
)

message(STATUS "Generate header for ui with rviz2_QT_VERSION: ${rviz2_QT_VERSION}")
qt5_wrap_ui(${PROJECT_NAME}_UIS_H ${${PROJECT_NAME}_UIS})
foreach(header "${${PROJECT_NAME}_HDRS}")
  qt5_wrap_cpp(${PROJECT_NAME}_MOCS ${header})
endforeach()


## Add library is needed in order to generate the header file from ui file.
add_library(${PROJECT_NAME} SHARED
  ${${PROJECT_NAME}_SRCS}
  ${${PROJECT_NAME}_UIS_H}
  ${${PROJECT_NAME}_MOCS}
)
ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS})
target_include_directories(${PROJECT_NAME} PUBLIC
  ${Qt5Widgets_INCLUDE_DIRS}
  ${OGRE_INCLUDE_DIRS}
)
target_link_libraries(${PROJECT_NAME} rviz_common::rviz_common)
# target_include_directories(${PROJECT_NAME} PUBLIC
#   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
#   $<INSTALL_INTERFACE:include>
# )
target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
target_compile_definitions(${PROJECT_NAME} PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY")


pluginlib_export_plugin_description_file(rviz_common demo_plugin.xml)

install(
  TARGETS ${PROJECT_NAME}
  EXPORT ${PROJECT_NAME}
  LIBRARY DESTINATION lib
  ARCHIVE DESTINATION lib
  RUNTIME DESTINATION bin
  INCLUDES DESTINATION include
)

install(
  DIRECTORY include/
  DESTINATION include
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # uncomment the line when a copyright and license is not present in all source files
  #set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # uncomment the line when this package is not in a git repo
  #set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_export_include_directories(include)
ament_export_libraries(${PROJECT_NAME})
ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET)
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})

ament_package()

package.xml

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>prc_demo_panel</name>
  <version>0.0.0</version>
  <description>Point Cloud Resolution Control</description>
  <maintainer email="vishnu.pbhat93@gmail.com">vishnu</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <build_depend>qtbase5-dev</build_depend>

  <exec_depend>libqt5-core</exec_depend>
  <exec_depend>libqt5-gui</exec_depend>
  <exec_depend>libqt5-opengl</exec_depend>
  <exec_depend>libqt5-widgets</exec_depend>

</package>

demo_plugin.xml:

<library path="/libprc_demo_panel" >
  <class 
    name="prc_demo_panel/Demo Panel"
    type="prc_demo_panel::DemoPanel" 
    base_class_type="rviz_common::Panel" 
  >
    <description>
      Point Cloud Resolution Control
    </description>
  </class>
</library>

This package can compile colcon build --symlink-install without any errors and rviz2 is able recognize the plugin. However when I try to add the plugin in rviz2 I am getting following error:

[ERROR] [1634722985.299621064] [rviz2]: PluginlibFactory: The plugin for class 'prc_demo_panel/Demo Panel' failed to load. 
Error: Could not find library corresponding to plugin prc_demo_panel/Demo Panel. Make sure the plugin description XML file has the correct name of the library and that the library actually exists.

I am not able to determine what is a mistake that I am doing. I would really appreciate any help.

Thank you

Asked by Vishnuprasad176 on 2021-10-26 03:02:17 UTC

Comments

Did you ever find a solution to this problem?

Asked by jodle001 on 2022-10-07 11:17:41 UTC

Answers

I am using a similar code template for rviz2 plugins and it works as expected. The only difference I've noticed is that I included header files to add_library macro. Like this :

 add_library(${PROJECT_NAME} SHARED
        ${${PROJECT_NAME}_SRCS}
        ${${PROJECT_NAME}_HDRS}
        ${${PROJECT_NAME}_UIS_H}
        ${${PROJECT_NAME}_MOCS}
        )

Also it is recommended to change the line below

<library path="/libprc_demo_panel" >

to

<library path="prc_demo_panel" >

for better portability. .

Asked by bay on 2023-03-20 06:14:55 UTC

Comments