teleop_rviz_plugin(1)
????????今天我们来看一下Far planner的rviz插件。也就是运行roslauch far planner far planner.launch的时候的那个rviz。
主要的部分就是右侧的两大块内容了。其中对应的代码有四个。我们先来看teleop_panel.h
teleop_panel.h
#ifndef TELEOP_PANEL_H
#define TELEOP_PANEL_H
#ifndef Q_MOC_RUN
# include <ros/ros.h>
# include <rviz/panel.h>
#endif
#include <stdio.h>
// 和Qt相关的头文件
#include <QPainter>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QTimer>
#include <QPushButton>
#include <QCheckBox>
#include <QFileDialog>
#include <std_msgs/Empty.h>
#include <std_msgs/Bool.h>
#include <std_msgs/String.h>
#include <sensor_msgs/Joy.h>
class QLineEdit;
namespace teleop_rviz_plugin
{
class DriveWidget;
//这个TeleopPanel 继承自rviz::Panel，里面有一系列的函数，具体可以ctrl点进去看它的@breif介绍
class TeleopPanel: public rviz::Panel
{
Q_OBJECT
public:
//TeleopPanel的构造函数
TeleopPanel( QWidget* parent = 0 ); //QWidget 基础窗口控件
//虚函数申明 load 和 save
virtual void load( const rviz::Config& config );
virtual void save( rviz::Config config ) const;
//qt槽
public Q_SLOTS:
//像是设置车子的？这个应该是手柄相关的 可以不看
void setVel( float linear_velocity_, float angular_velocity_, bool mouse_pressed_ );
protected Q_SLOTS:
void pressButton1(); // reset_visibility_graph
void pressButton2(); //resume navigation to goal
void pressButton3(); //Read
void pressButton4(); //Save
void clickBox1(int val); //Planning attempable
void clickBox2(int val); //update visibility graph
void sendVel(); //设置手柄的
protected:
// 类指针
DriveWidget* drive_widget_;
ros::Publisher velocity_publisher_; //和手柄发送速度有关的pub
ros::Publisher attemptable_publisher_; //和clickBox1有关的pub
ros::Publisher update_publisher_; //和clickBox2有关的pub
ros::Publisher reset_publisher_; //和pressButton1有关的pub
ros::Publisher read_publisher_; //和pressButton3有关的pub
ros::Publisher save_publisher_; //和pressButton4有关的pub
ros::NodeHandle nh_;
QPushButton *push_button_1_;
QPushButton *push_button_2_;
QPushButton *push_button_3_;
QPushButton *push_button_4_;
QCheckBox *check_box_1_;
QCheckBox *check_box_2_;
float linear_velocity_;
float angular_velocity_;
// 可能是捕获鼠标设定nav_goal的时候用到的
bool mouse_pressed_;
bool mouse_pressed_sent_;
};
}
#endif // TELEOP_PANEL_H
? ? ? ? 接下来我们来看teleop_panel.cpp，我们主要关心的是PressButton 1~4 以及两个clickbox的内容。先看看它的构造函数都写了什么：
TeleopPanel::TeleopPanel( QWidget* parent )
: rviz::Panel( parent ), linear_velocity_( 0 ), angular_velocity_( 0 ), mouse_pressed_( false ), mouse_pressed_sent_( false )
//构造函数赋初值
{
QVBoxLayout* layout = new QVBoxLayout; //Qt 垂直布局 (QVBoxLayout)
check_box_1_ = new QCheckBox( "Planning Attemptable", this );
check_box_1_->setCheckState( Qt::Checked ); //checked 默认就是勾上
layout->addWidget( check_box_1_ );
check_box_2_ = new QCheckBox( "Update Visibility Graph", this );
check_box_2_->setCheckState( Qt::Checked );
layout->addWidget( check_box_2_ );
push_button_1_ = new QPushButton( "Reset Visibility Graph", this );
layout->addWidget( push_button_1_ );
push_button_2_ = new QPushButton( "Resume Navigation to Goal", this );
layout->addWidget( push_button_2_ );
push_button_3_ = new QPushButton( "Read", this );
layout->addWidget( push_button_3_ );
push_button_4_ = new QPushButton( "Save", this );
layout->addWidget( push_button_4_ );
drive_widget_ = new DriveWidget; //鼠标推动小车移动的控件
layout->addWidget( drive_widget_ );
setLayout( layout ); //把上面所有的layout都set完
QTimer* output_timer = new QTimer( this );
//槽绑定 push_button_1_ 是显示在画面中的控件
//SIGNAL( pressed() ) 代表按压下去执行操作
//SLOT( pressButton1() )表示的是触发的函数
connect( push_button_1_, SIGNAL( pressed() ), this, SLOT( pressButton1() ));
connect( push_button_2_, SIGNAL( pressed() ), this, SLOT( pressButton2() ));
connect( push_button_3_, SIGNAL( pressed() ), this, SLOT( pressButton3() ));
connect( push_button_4_, SIGNAL( pressed() ), this, SLOT( pressButton4() ));
connect( check_box_1_, SIGNAL( stateChanged(int) ), this, SLOT( clickBox1(int) ));
connect( check_box_2_, SIGNAL( stateChanged(int) ), this, SLOT( clickBox2(int) ));
connect( drive_widget_, SIGNAL( outputVelocity( float, float, bool )), this, SLOT( setVel( float, float, bool )));
connect( output_timer, SIGNAL( timeout() ), this, SLOT( sendVel() ));
output_timer->start( 100 );
velocity_publisher_ = nh_.advertise<sensor_msgs::Joy>( "/joy", 5 ); //发布的是手柄的数据，用不到可以不管
attemptable_publisher_ = nh_.advertise<std_msgs::Bool>( "/planning_attemptable", 5 );
update_publisher_ = nh_.advertise<std_msgs::Bool>( "/update_visibility_graph", 5 );
reset_publisher_ = nh_.advertise<std_msgs::Empty>( "/reset_visibility_graph", 5 );
read_publisher_ = nh_.advertise<std_msgs::String>( "/read_file_dir", 5 ); //在graph_decoder中用到的用来读取vgh文件的节点
save_publisher_ = nh_.advertise<std_msgs::String>( "/save_file_dir", 5 ); //在graph_decoder中用到的用来读写入vgh文件的节点
drive_widget_->setEnabled( true );
}? ? ? ? 设置完控件以及控件的绑定以后，我们就要对各个按键按压下去时执行的操作（函数）进行完善，我们接着看看各个按钮对应的操作：?
pressButton1?
void TeleopPanel::pressButton1()
{
if ( ros::ok() && velocity_publisher_ )
{
std_msgs::Empty msg;
reset_publisher_.publish(msg);
}
}????????让我们看看他判断的条件，ros正在运行 且?velocity_publisher_ 运行正常。制造一个空消息，然后reset_publisher_发布这个空消息，这样别人订阅这个节点得到的就是空消息。
pressButton2
void TeleopPanel::pressButton2()
{
if ( ros::ok() && velocity_publisher_ )
{
sensor_msgs::Joy joy;
joy.axes.push_back(0);
joy.axes.push_back(0);
joy.axes.push_back(-1.0);
joy.axes.push_back(0);
joy.axes.push_back(1.0);
joy.axes.push_back(1.0);
joy.axes.push_back(0);
joy.axes.push_back(0);
joy.buttons.push_back(0);
joy.buttons.push_back(0);
joy.buttons.push_back(0);
joy.buttons.push_back(0);
joy.buttons.push_back(0);
joy.buttons.push_back(0);
joy.buttons.push_back(0);
joy.buttons.push_back(1);
joy.buttons.push_back(0);
joy.buttons.push_back(0);
joy.buttons.push_back(0);
joy.header.stamp = ros::Time::now();
joy.header.frame_id = "teleop_panel";
velocity_publisher_.publish( joy );
}
}? ? ? ? 这个是和手柄有关的 我们不看了。
pressButton3
void TeleopPanel::pressButton3()
{
if ( ros::ok() && velocity_publisher_ )
{
QString qFilename = QFileDialog::getOpenFileName(this, tr("Read File"), "/", tr("VGH - Visibility Graph Files (*.vgh)"));
std::string filename = qFilename.toStdString();
std_msgs::String msg;
msg.data = filename;
read_publisher_.publish(msg);
}
}? ? ? ? 设计了一个qFilename的对话框QFileDialog，让你去选择后缀名为.vgh的文件。读取到以后是一种字符串数组，用filename把qFilename中得到的转换成c++的std::string 。
? ? ? ? 再联系到msg，其中msg.data存放的就是路径的名称。那么我们把路径名称存进去，再通过read_publisher_发布这个消息，别人订阅的时候，就可以得到一串vgh的路径。
????????这里联系到decoder_graph里面对路径读取以后，解析每一行，把各个属性赋值给nav_point，如果大家没有印象了，可以去看之前的Far Planner代码系列（1）和（2）。
pressButton4
void TeleopPanel::pressButton4()
{
if ( ros::ok() && velocity_publisher_ )
{
QString qFilename = QFileDialog::getSaveFileName(this, tr("Save File"), "/", tr("VGH - Visibility Graph Files (*.vgh)"));
std::string filename = qFilename.toStdString();
if (filename != "") {
int length = filename.length();
if (length < 4) {
filename += ".vgh";
} else if (filename[length - 4] != '.' || filename[length - 3] != 'v' || filename[length - 2] != 'g' || filename[length - 1] != 'h') {
filename += ".vgh";
}
}
std_msgs::String msg;
msg.data = filename;
save_publisher_.publish(msg);
}
}? ? ? ? Save部分和Read部分差不多，都是提供一个名字给msg，联系到之前Decoder_graph里的SaveGraphCallBack函数其中的前几行：
const std::string file_path = msg->data; //把msg中的data提取出来 也就是teleop_panel.cpp中的file_name
if (file_path == "") return; //如果file_path为空 直接返回
std::ofstream graph_file; //定义输出流字节了? ? ? ? 这里的msg就是传进来的save_publisher的msg了。?
???????? msg怎么看是哪种呢？ 比如 publisher的节点是/save_file_dir ，就会有一个对应的subscriber节点 也是/save_file_dir 那么这个subscriber的消息类型就是publisher发布的msg 。
clickedBox1
void TeleopPanel::clickBox1(int val)
{
if ( ros::ok() && attemptable_publisher_ )
{
std_msgs::Bool msg;
msg.data = bool(val);
attemptable_publisher_.publish(msg);
}
}? ? ? ? 定义了一个Bool类型的msg? 那么clickbox1的结果 cheked就是1 unchecked就是0 那么把这个值传入msg.data 通过attemptable_publisher_去发布就好了。
?clickedBox2
void TeleopPanel::clickBox2(int val)
{
if ( ros::ok() && update_publisher_ )
{
std_msgs::Bool msg;
msg.data = bool(val);
update_publisher_.publish(msg);
}
}? ? ? ? 同box1 不多说了。
void TeleopPanel::save( rviz::Config config ) const
{
rviz::Panel::save( config );
}
void TeleopPanel::load( const rviz::Config& config )
{
rviz::Panel::load( config );
}? ? ? ? 剩下几个是储存rviz本身内容的配置文件。不用管了。