Bootstrap

Ros机器人Rviz Qt5插件开发(1)——机器人速度控制插件开发

本系列教程文章专栏：
ROS机器人GUI程序开发

一，创建功能包

在工作空间src目录下：

catkin_create_pkg cmd_control roscpp rviz std_msgs

二，配置功能包

2.1添加类到功能包src目录：
在这里插入图片描述
2.2添加plugin描述文件plugin_description.xml到功能包根目录：

  <library path="lib/libcmd_control">
      <class name="cmd_control/cmd_control"
             type="cmd_control:cmd_control"
             base_class_type="rviz::Panel">
        <description>
          A panel widget Control the Robot
        </description>
      </class>
    </library>

这里注意，第一行lib 需要在自己包名前面加个lib

 <library path="lib/libcmd_control">

2.3然后在 package.xml文件里添加plugin_description.xml。

  <export>
      <rviz plugin="${prefix}/plugin_description.xml"/>
  </export>

2.4CMakeLists.txt
添加如下（注意需要修改头文件和cpp名）：

## This plugin includes Qt widgets, so we must include Qt like so:
find_package(Qt5 REQUIRED Core Widgets)
set(QT_LIBRARIES Qt5::Widgets)
include_directories(${catkin_INCLUDE_DIRS})

## 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.
add_definitions(-DQT_NO_KEYWORDS)

## Here we specify which header files need to be run through "moc",
## Qt's meta-object compiler.
##修改这里的头文件名
qt5_wrap_cpp(MOC_FILES
src/cmd_control.h
)

## Here we specify the list of source files, including the output of
## the previous command which is stored in ``${MOC_FILES}``.
##修改这里的cpp名
set(SOURCE_FILES
src/cmd_control.cpp 
${MOC_FILES}
)

## An rviz plugin is just a shared library, so here we declare the
## library to be called ``${PROJECT_NAME}`` (which is
## "rviz_plugin_tutorials", or whatever your version of this project
## is called) and specify the list of source files we collected above
## in ``${SOURCE_FILES}``.
add_library(${PROJECT_NAME} ${SOURCE_FILES})

## Link the library with whatever Qt libraries have been defined by
## the ``find_package(Qt4 ...)`` line above, and with whatever libraries
## catkin has included.
##
## Although this puts "rviz_plugin_tutorials" (or whatever you have
## called the project) as the name of the library, cmake knows it is a
## library and names the actual file something like
## "librviz_plugin_tutorials.so", or whatever is appropriate for your
## particular OS.
target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES})

这样就能使用qt打开项目并编辑了
qt打开项目教程：
qt配置Ros gui开发

三，编写代码

cmd_control.h

#ifndef CMD_CONTROL_H
#define CMD_CONTROL_H

#include <stdio.h>

//所需要包含的头文件
#include <ros/ros.h>
#include <ros/console.h>
#include <rviz/panel.h>   //plugin基类的头文件


#include <QPainter>
#include <QLineEdit>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QLabel>
#include <QTimer>
#include <QPushButton>
#include <geometry_msgs/Twist.h>
#include <QDebug>
#include <QCheckBox>
#include <QSlider>
namespace cmd_control_space {
class cmd_control:public rviz::Panel
{
    Q_OBJECT
public:
    // 构造函数，在类中会用到QWidget的实例来实现GUI界面，这里先初始化为0即可
    cmd_control(QWidget* parent=0);
    // 重载rviz::Panel积累中的函数，用于保存、加载配置文件中的数据，在我们这个plugin中，数据就是topic的名称
//    virtual void load( const rviz::Config& config );
    virtual void save( rviz::Config config ) const;
    void move_base(char k,float speed_linear,float speed_trun);
public Q_SLOTS:
    void slot_cmd_control();
    void on_Slider_raw_valueChanged(int);
    // 内部变量.
protected:
    QPushButton* pushButton_i;
    QPushButton* pushButton_u;
    QPushButton* pushButton_o;
    QPushButton* pushButton_j;
    QPushButton* pushButton_l;
    QPushButton* pushButton_m;
    QPushButton* pushButton_back;
    QPushButton* pushButton_backr;

    QCheckBox* is_all_check;

    QSlider* yaw_slider;
    QSlider* linera_slider;
    // ROS的publisher，用来发布速度topic
    ros::Publisher velocity_publisher_;
    QString output_topic_="cmd_vel";

    // The ROS node handle.
    ros::NodeHandle nh_;

    // 当前保存的线速度和角速度值
    float linear_velocity_;
    float angular_velocity_;

};
}

#endif // CMD_CONTROL_H

cmd_control.cpp


#include "cmd_control.h"
#include <stdio.h>

#include <QPainter>
#include <QLineEdit>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QLabel>
#include <QTimer>

#include <geometry_msgs/Twist.h>
#include <QDebug>

namespace cmd_control_space {
cmd_control::cmd_control(QWidget* parent)
    :rviz::Panel (parent)
{


    //初始化ui
 QVBoxLayout *mainlayout=new QVBoxLayout;
 pushButton_i=new QPushButton;
 pushButton_i->setText("i");
 //快捷键
 pushButton_i->setShortcut(QKeySequence(QLatin1String("i")));
 pushButton_i->setStyleSheet("QPushButton#pushButton_i:pressed{background-color:rgb(239, 41, 41)}");
 pushButton_u=new QPushButton;
 //快捷键
 pushButton_u->setShortcut(QKeySequence(QLatin1String("u")));
 pushButton_u->setText("u");
 pushButton_u->setStyleSheet("QPushButton#pushButton_i:pressed{background-color:rgb(239, 41, 41)}");
 pushButton_o=new QPushButton;
 //快捷键
 pushButton_o->setShortcut(QKeySequence(QLatin1String("o")));
 pushButton_o->setText("o");
 pushButton_o->setStyleSheet("QPushButton#pushButton_i:pressed{background-color:rgb(239, 41, 41)}");
 QHBoxLayout *first=new QHBoxLayout;
 first->addWidget(pushButton_u);
 first->addWidget(pushButton_i);
 first->addWidget(pushButton_o);
 mainlayout->addLayout(first);

 pushButton_j=new QPushButton;
 pushButton_j->setText("j");
 //快捷键
 pushButton_j->setShortcut(QKeySequence(QLatin1String("j")));
 pushButton_j->setStyleSheet("QPushButton#pushButton_i:pressed{background-color:rgb(239, 41, 41)}");
 is_all_check=new QCheckBox;
 is_all_check->setText("使用全向轮模式");
 //快捷键
 is_all_check->setShortcut(QKeySequence(QLatin1String("k")));
 pushButton_l=new QPushButton;
 pushButton_l->setText("l");
 //快捷键
 pushButton_l->setShortcut(QKeySequence(QLatin1String("l")));
 pushButton_l->setStyleSheet("QPushButton#pushButton_i:pressed{background-color:rgb(239, 41, 41)}");
 QHBoxLayout *second=new QHBoxLayout;
 second->addWidget(pushButton_j);
 second->addWidget(is_all_check);
 second->addWidget(pushButton_l);
 mainlayout->addLayout(second);

 pushButton_m=new QPushButton;
 //快捷键
 pushButton_m->setShortcut(QKeySequence(QLatin1String("m")));
 pushButton_m->setText("m");
 pushButton_m->setStyleSheet("QPushButton#pushButton_i:pressed{background-color:rgb(239, 41, 41)}");
 pushButton_back=new QPushButton;
 //快捷键
 pushButton_back->setShortcut(QKeySequence(QLatin1String(",")));
 pushButton_back->setText(",");
 pushButton_back->setStyleSheet("QPushButton#pushButton_i:pressed{background-color:rgb(239, 41, 41)}");
 pushButton_backr=new QPushButton;
 //快捷键
 pushButton_backr->setShortcut(QKeySequence(QLatin1String(".")));
 pushButton_backr->setText(".");
pushButton_backr->setStyleSheet("QPushButton#pushButton_i:pressed{background-color:rgb(239, 41, 41)}");
QHBoxLayout *third=new QHBoxLayout;
third->addWidget(pushButton_m);
third->addWidget(pushButton_back);
third->addWidget(pushButton_backr);
mainlayout->addLayout(third);

QLabel *linera_label=new QLabel;
linera_label->setText("线速度（cm/s）");
linera_slider=new QSlider;
linera_slider->setOrientation(Qt::Horizontal);  // 水平方向
linera_slider->setRange(0,100);
linera_slider->setValue(50);
QHBoxLayout *five=new QHBoxLayout;
five->addWidget(linera_label);
five->addWidget(linera_slider);
mainlayout->addLayout(five);

QLabel *yaw_label=new QLabel;
yaw_label->setText("角速度（cm/s）");
yaw_slider=new QSlider;
yaw_slider->setOrientation(Qt::Horizontal);  // 水平方向
yaw_slider->setRange(0,100);
yaw_slider->setValue(100);
QHBoxLayout *six=new QHBoxLayout;
six->addWidget(yaw_label);
six->addWidget(yaw_slider);
mainlayout->addLayout(six);

 setLayout(mainlayout);

 //绑定信号

 //绑定速度控制按钮
 connect(pushButton_i,SIGNAL(clicked()),this,SLOT(slot_cmd_control()));
 connect(pushButton_u,SIGNAL(clicked()),this,SLOT(slot_cmd_control()));
 connect(pushButton_o,SIGNAL(clicked()),this,SLOT(slot_cmd_control()));
 connect(pushButton_j,SIGNAL(clicked()),this,SLOT(slot_cmd_control()));
 connect(pushButton_l,SIGNAL(clicked()),this,SLOT(slot_cmd_control()));
 connect(pushButton_m,SIGNAL(clicked()),this,SLOT(slot_cmd_control()));
 connect(pushButton_back,SIGNAL(clicked()),this,SLOT(slot_cmd_control()));
 connect(pushButton_backr,SIGNAL(clicked()),this,SLOT(slot_cmd_control()));

 //创建发布者
 velocity_publisher_ = nh_.advertise<geometry_msgs::Twist>( output_topic_.toStdString(), 1 );

}

void cmd_control::slot_cmd_control()
{

    QPushButton* btn=qobject_cast<QPushButton*>(sender());
    char key=btn->text().toStdString()[0];
    //速度
    float liner=linera_slider->value()*0.01;
    float turn=yaw_slider->value()*0.01;
    bool is_all=is_all_check->isChecked();
    switch (key) {
        case 'u':
            move_base(is_all?'U':'u',liner,turn);
        break;
        case 'i':
            move_base(is_all?'I':'i',liner,turn);
        break;
        case 'o':
            move_base(is_all?'O':'o',liner,turn);
        break;
        case 'j':
            move_base(is_all?'J':'j',liner,turn);
        break;
        case 'l':
            move_base(is_all?'L':'l',liner,turn);
        break;
        case 'm':
            move_base(is_all?'M':'m',liner,turn);
        break;
        case ',':
            move_base(is_all?'<':',',liner,turn);
        break;
        case '.':
            move_base(is_all?'>':'.',liner,turn);
        break;
    }

}
void cmd_control::move_base(char k,float speed_linear,float speed_trun)
{
    std::map<char, std::vector<float>> moveBindings
    {
      {'i', {1, 0, 0, 0}},
      {'o', {1, 0, 0, -1}},
      {'j', {0, 0, 0, 1}},
      {'l', {0, 0, 0, -1}},
      {'u', {1, 0, 0, 1}},
      {',', {-1, 0, 0, 0}},
      {'.', {-1, 0, 0, 1}},
      {'m', {-1, 0, 0, -1}},
      {'O', {1, -1, 0, 0}},
      {'I', {1, 0, 0, 0}},
      {'J', {0, 1, 0, 0}},
      {'L', {0, -1, 0, 0}},
      {'U', {1, 1, 0, 0}},
      {'<', {-1, 0, 0, 0}},
      {'>', {-1, -1, 0, 0}},
      {'M', {-1, 1, 0, 0}},
      {'t', {0, 0, 1, 0}},
      {'b', {0, 0, -1, 0}},
      {'k', {0, 0, 0, 0}},
      {'K', {0, 0, 0, 0}}
    };
    char key=k;
    //计算是往哪个方向
    float x = moveBindings[key][0];
    float y = moveBindings[key][1];
    float z = moveBindings[key][2];
    float th = moveBindings[key][3];
    //计算线速度和角速度
    float speed = speed_linear;
    float turn = speed_trun;
    // Update the Twist message
    geometry_msgs::Twist twist;
   twist.linear.x = x * speed;
   twist.linear.y = y * speed;
   twist.linear.z = z * speed;

   twist.angular.x = 0;
   twist.angular.y = 0;
   twist.angular.z = th * turn;

   // Publish it and resolve any remaining callbacks
   velocity_publisher_.publish(twist);
   ros::spinOnce();
}


// 重载父类的功能
void cmd_control::save( rviz::Config config ) const
{
  rviz::Panel::save( config );
  config.mapSetValue( "Topic", output_topic_ );
}

 重载父类的功能，加载配置数据
//void cmd_control::load( const rviz::Config& config )
//{
//  rviz::Panel::load( config );
//  QString topic;
//  if( config.mapGetString( "Topic", &topic ))
//  {
//    output_topic_editor_->setText( topic );
//    updateTopic();
//  }
//}


}

// 声明此类是一个rviz的插件
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(cmd_control_space::cmd_control,rviz::Panel )
// END_TUTORIAL

四，实现效果

编译成功后，我们来运行rviz，需要注意的是：一定要source devel文件夹下的setup脚本，来生效路径，否则会找不到插件：
打开rviz:

rosrun rviz rviz

选择菜单栏:
panels->add new panel:

在这里插入图片描述
最终效果：
效果

完整项目地址：
github

悦读

道可道，非常道；名可名，非常名。 无名，天地之始，有名，万物之母。 故常无欲，以观其妙，常有欲，以观其徼。 此两者，同出而异名，同谓之玄，玄之又玄，众妙之门。

最新收录
mybatis+redis使用
构造+有序集合，CF 1023D - Array Restoration
vscode 找不到python解释器_VSCode下配置python调试运行环境的方法
教你Python字符串的基本操作：拆分和连接(1)
工程计算——求解线性方程组AX=b
利用pytorch 训练常见分类网络
引用传递示例，以及String类型如何传递的原理
数据加密算法
git 、shell脚本
Invar-RAG：基于不变性对齐的LLM检索方法提升生成质量
;