本系列教程文章专栏:
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
//所需要包含的头文件
#include
#include
#include //plugin基类的头文件
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
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
#include
#include
#include
#include
#include
#include
#include
#include
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_EXPORT_CLASS(cmd_control_space::cmd_control,rviz::Panel )
// END_TUTORIAL
编译成功后,我们来运行rviz,需要注意的是:一定要source devel文件夹下的setup脚本,来生效路径,否则会找不到插件:
打开rviz:
rosrun rviz rviz
选择菜单栏:
panels->add new panel:
完整项目地址:
github