Far planner代码系列(3)

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的文件。读取到以后是一种字符串数组,用filenameqFilename中得到的转换成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本身内容的配置文件。不用管了。

  • 0
    点赞
  • 0
    收藏
  • 打赏
    打赏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
©️2022 CSDN 皮肤主题:1024 设计师:我叫白小胖 返回首页
评论

打赏作者

LZZ and MYY

你的鼓励将是我创作的最大动力

¥2 ¥4 ¥6 ¥10 ¥20
输入1-500的整数
余额支付 (余额:-- )
扫码支付
扫码支付:¥2
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值