9.21 V1

librviz_tutorial/CMakeLists.txt

@@ -17,6 +17,7 @@ FILE(GLOB SDK_SRC
 find_package(catkin REQUIRED COMPONENTS 
 rviz roscpp std_msgs 
 rosconsole sensor_msgs # node.cpp
+rosbag  
 )
 
 

librviz_tutorial/src/myviz.cpp

@@ -3,6 +3,7 @@
 #include<QSplitter>
 #include <QLabel>
 #include<QPlainTextEdit>
+#include<QPushButton>
 
 #include <QVBoxLayout>
 #include <QHBoxLayout>
@@ -14,13 +15,36 @@
 #include <QTreeWidgetItem>
 #include <QHeaderView> 
 
+#include<QDebug>
+#include<QKeyEvent>
+#include <QMutex>
+#include<QTimer>
+#include<QObject>
+#include <QMetaEnum>
+#include <QRegExp>
+#include <cmath>
+#include<QMessageBox>
+#include<QCoreApplication>
+#include <QProgressBar>
+
+#include <rosbag/bag.h>
+#include <rosbag/view.h>
+#include <rosbag/query.h>
+#include<string>
+#include<QFileDialog>
+
+#include "rviz/properties/property_tree_widget.h"
+#include "rviz/selection/selection_manager.h"
+#include "rviz/panel.h"
+
 #include "rviz/visualization_manager.h"
+#include"rviz/tool_manager.h"
 #include "rviz/render_panel.h"
 #include "rviz/display.h"
 
 #include "myviz.h"
 #include "../tools/color.h"
-
+#include "qthread.h"
 // BEGIN_TUTORIAL
 // Constructor for MyViz.  This does most of the work of the class.
 MyViz::MyViz( QWidget* parent)
@@ -34,86 +58,229 @@ MyViz::MyViz( QWidget* parent)
 
   // 全局变量存放处
   // 颜色名称，可添加，待设置rgb类型设置颜色
-  color_name = {"yellow","red","green","black","blue","white","pink","purple","skyblue","lightgreen","orange",
+  color_name = {"white","red","green","black","blue","yellow","pink","purple","skyblue","lightgreen","orange",
   };
+  // 颜色路径名
+  color_icon_filename = QString("/home/wbw/Rviz/librviz_ws/src/librviz_tutorial/sources/images/%1.png");
+  speed_name = {"2.0","1.5","1.25","1.0","0.75","0.5",};
+  duration = 0;
+  isclicked = true;
+  ispauseclick = true;
+  sliderreleased = false;
+  playagain = false;
+  isthreadplaying = false;
+  abs = false;
+  stored = false;
 
-  //pub_thread.join();
-  // Construct and lay out labels and slider controls.
-  // Line Thickness
-  QLabel* thickness_label = new QLabel( "Line Thickness" );
-  QSlider* thickness_slider = new QSlider( Qt::Horizontal );
-  thickness_slider->setMinimum( 1 );
-  thickness_slider->setMaximum( 100 );
-  // Cell Size
-  QLabel* cell_size_label = new QLabel( "Cell Size" );
-  QSlider* cell_size_slider = new QSlider( Qt::Horizontal );
-  cell_size_slider->setMinimum( 1 );
-  cell_size_slider->setMaximum( 100 );
-  // Topic_Cloud
-  // QLabel* Topic=new QLabel("TOPIC:");
-  // QLineEdit* Topic_text=new QLineEdit();
 
-  // Topic_Laser 输入雷达
-  QLabel* Topic_laser = new QLabel("TOPIC_Laser:");
-  QLineEdit* Topic_laser_text = new QLineEdit();
-
-  // Laser
-  QLabel* Size=new QLabel("Laser Size");
-  QSpinBox* size=new QSpinBox();
   // 网格结构
   QGridLayout* controls_layout = new QGridLayout();
-  controls_layout->addWidget( thickness_label, 0, 0 );
+  int ctl_index = 0;
-  controls_layout->addWidget( thickness_slider, 1, 0 );
-  controls_layout->addWidget( cell_size_label, 2, 0 );
-  controls_layout->addWidget( cell_size_slider, 3, 0 );
-  // controls_layout->addWidget(Topic,0,4);
-  // controls_layout->addWidget(Topic_text,0,5);
 
-  controls_layout->addWidget(Topic_laser, 4, 0);
+    //select按钮
-  controls_layout->addWidget(Topic_laser_text, 5, 0);
+  QPushButton* selection_button = new QPushButton("select");
+  QPushButton* move_camera_button = new QPushButton("move_camera");
 
-  controls_layout->addWidget(Size,6,0);
+  controls_layout->addWidget( move_camera_button, ctl_index, 0);
-  controls_layout->addWidget(size,7,0);
+  controls_layout->addWidget( selection_button, ctl_index++ ,1);
 
-  // 颜色选择QComBox
+  // offline 布局
-  QLabel* label_color = new QLabel("Color:");
+  // QPushButton setting!////////////////////////////////////////////////////////////////////////////////////////////////
-  QComboBox * comboBox = new QComboBox();
+  QPushButton* play = new QPushButton("play", this);
-  for(int i =0 ; i < color_name.size(); ++i){
+  QPushButton* pause = new QPushButton("pause", this);
-      comboBox->addItem(color_name[i]);
+  QPushButton* but_save = new QPushButton("save", this);// buttonn: save one frame
+  QPushButton* but_saveMul = new QPushButton("saveMul", this);// button: save multiple frames
+  QPushButton* but_file = new QPushButton("file", this);// button: select file
+
+  // QLabel setting!/////////////////////////////////////////////////////////////////////////////////////////////////////
+  QLabel* label_speed = new QLabel("speed:");//speed combobox
+  QLabel* label_split = new QLabel("");//speed combobox
+
+  // QSlider setting!////////////////////////////////////////////////////////////////////////////////////////////////////
+  progress = new QSlider(Qt::Horizontal);
+  // QComboBox setting!//////////////////////////////////////////////////////////////////////////////////////////////////
+  QComboBox * speed_combobox = new QComboBox();
+  //enable button////////////////////////////////////////////////////////////////////////////////////////////////////////
+  play->setEnabled(true);
+  pause->setEnabled(true);
+  but_save->setEnabled(true);
+  but_saveMul->setEnabled(true);
+  but_file->setEnabled(true);
+
+  // setting button size!////////////////////////////////////////////////////////////////////////////////////////////////
+  play->setFixedSize(100, 30);
+  pause->setFixedSize(100, 30);
+  but_save->setFixedSize(100, 30);
+  but_saveMul->setFixedSize(100, 30);
+  but_file->setFixedSize(100, 30);
+
+  for(int i = 0; i < speed_name.size(); i++)
+    speed_combobox->addItem(speed_name[i] + "x");
+  speed_combobox->setCurrentIndex(3);
+
+  controls_layout->addWidget( label_split, ctl_index++, 0 );
+  controls_layout->addWidget( but_file, ctl_index, 0 );
+  controls_layout->addWidget( but_save, ctl_index++, 1 );
+  controls_layout->addWidget( but_saveMul, ctl_index++, 0 );
+  
+  controls_layout->addWidget(label_speed,ctl_index,0);
+  controls_layout->addWidget(speed_combobox,ctl_index++,1);
+  
+  controls_layout->addWidget( play, ctl_index, 0 );
+  controls_layout->addWidget( pause, ctl_index++, 1 );
+  controls_layout->addWidget(progress,ctl_index++,0, 1, 2); // addwidget(widget,row,col,m,n);//m是占几行,n是占几列,
+
+  controls_layout->setSpacing(5);
+
+  // 是否选择显示坐标轴
+  QLabel* label_axes = new QLabel("Display Axes:");
+  QComboBox * axesBox = new QComboBox();
+  axesBox->addItem("no");
+  axesBox->addItem("yes");
+
+  // 选择圆圈范围
+  QLabel* label_circle = new QLabel("Circle Scale(m): ");
+  QComboBox* circleBox = new QComboBox();
+  circleBox->addItem("cancel");
+  for(int i =1; i<=3 ;++i){
+    circleBox->addItem(QString(std::to_string(i).c_str()));
   }
-  controls_layout->addWidget(label_color,8,0);
+  circleBox->addItem("5");
-  controls_layout->addWidget(comboBox,8,1);
 
-  // Tree
+  controls_layout->addWidget(label_axes, ctl_index,0);
-  Tree_Display(controls_layout, 9);
+  controls_layout->addWidget(axesBox, ctl_index++,1);
+  controls_layout->addWidget(label_circle, ctl_index,0);
+  controls_layout->addWidget(circleBox, ctl_index++,1);
+
+  // 第二个layout
+  QGridLayout* display_layout = new QGridLayout();
+  int display_index = 0;
+  
+  // Tree 全局变量存放地方
+  Tree_Display(display_layout, display_index++);
 
-  // 设置网格某一行伸缩比例
-  controls_layout->setRowStretch(9, 6);
   // controls_layout->setRowStretch(3, 0);
+  // Make signal/slot connections. 进行信号/插槽连接
+
+  // 增加切换select
+  connect(selection_button, SIGNAL(clicked()), this, SLOT( slot_select()));
+  connect(move_camera_button, SIGNAL(clicked()), this, SLOT( slot_move_camera()));
+
+
+  // 增加坐标变换
+  connect(axesBox, SIGNAL(currentIndexChanged(int)), this, SLOT(AxesDisplayChanged(int)));
+  // 增加圆圈变换
+  connect(circleBox, SIGNAL(currentIndexChanged(int)), this, SLOT(CircleDisplayChanged(int)));
+  //off
+  connect(progress,&QSlider::valueChanged,this,[=](){// 变动会触发
+    if(!ismoved){
+      ischanged = true;// “戳”这一个动作会使ischange变成true
+      // qDebug()<<"CLICK"<<ischanged;
+    }
+  });
+  connect(progress,&QSlider::sliderMoved,this,[=](){// 变动会触发
+    sliderreleased = true;
+    ismoved = true;
+    // ispauseclick = false;
+  });
+  connect(progress,&QSlider::sliderPressed,this,[=](){// 拖动时打开暂停
+    // sliderreleased = true;
+    // qDebug() << "SLIDER PRESS";
+    ispauseclick = false;
+  });
+  connect(progress,&QSlider::sliderReleased,this,[=](){// 释放时关闭暂停
+    // sliderreleased = true;
+    ispauseclick = true;
+    ismoved = false;
+  });
+  connect(but_save, &QPushButton::clicked, this,  [&]() {
+    if(isthreadplaying)
+      is_save = true;
+    else
+      QMessageBox::information(this, "tips", "No file is played");
+    });
+  connect(but_saveMul, &QPushButton::clicked, this,  [&]() {
+    if(isthreadplaying){
+      if(is_saveMul){
+        is_saveMul = false;
+        is_saveMulFinish = true;
+      }
+      else
+        is_saveMul = true;
+    }else{
+      QMessageBox::information(this, "tips", "No file is played");
+    }
+  });
+  connect(but_file, &QPushButton::clicked, this, &MyViz::selectFile);
+  connect(speed_combobox, SIGNAL(currentIndexChanged(int)), this, SLOT(SpeedChanged(int)));//speed change
+  connect(play, &QPushButton::clicked,this,[=]() {
+      // 只有当线程启动时才能按暂停键
+      if(filePath.isEmpty())
+      {
+        QMessageBox msgBox;
+        msgBox.setWindowTitle("Tips");
+        msgBox.setText("Please Choose a File!");
+        msgBox.resize(2000, 1000);
+        msgBox.exec();
+      }
+      else{
+      if(!playagain && !threadout)// 选了文件点击play就会执行这个
+      {
+        threadout = false;
+        isthreadplaying = true;
+        ispauseclick = true;
+        stored = true;
+        playagain = true;
+       
+        std::thread rosBagPlay(&MyViz::openFile, this);// 新开线程执行操作
+        rosBagPlay.detach();// 执行完成后自动回收资源
+        
+        
+      }
+      if(threadout && !playagain)
+      {
+        threadout = false;
+        playagain = true;
+        ispauseclick = true;
+        stored = true;
+        std::thread rosBagPlay(&MyViz::openFile, this);// 新开线程执行操作
+        rosBagPlay.detach();// 执行完成后自动回收资源
+        
+      }
+      if(!isthreadplaying)
+      {
+        isthreadplaying = true;
+        ispauseclick = true;
+        
+        progress->setSliderPosition(0);
+        std::thread rosBagPlay(&MyViz::openFile, this);// 新开线程执行操作
+        rosBagPlay.detach();
+        
+      }
+      }
+ 
+  });
+  connect(pause, &QPushButton::clicked, this, [&]() {
+    if(isthreadplaying){
+    if(ispauseclick)
+    {
+      ispauseclick = false;
+    }
+    else
+    {
+      ispauseclick = true;
+    }
+    }
+    });
 
   // Construct and lay out render panel.
   render_panel_ = new rviz::RenderPanel();  // RVIZ在QT上显示的类
   QHBoxLayout* main_layout = new QHBoxLayout;  // 主要layout QV垂直，QH水平
-  main_layout->addLayout( controls_layout );
-  main_layout->addWidget( render_panel_,2); // 存放rviz的位置, stretch 拉伸系数
 
   // QSplitter *splitter = new QSplitter();
   // main_layout->addWidget(splitter);
   // splitter->addWidget(render_panel_);
 
-  // Set the top-level layout for this MyViz widget.
-  setLayout( main_layout );
-
-  // Make signal/slot connections. 进行信号/插槽连接
-  connect( thickness_slider, SIGNAL( valueChanged( int )), this, SLOT( setThickness( int )));
-  connect( cell_size_slider, SIGNAL( valueChanged( int )), this, SLOT( setCellSize( int )));
-  // 增加雷达话题
-  connect(Topic_laser_text, SIGNAL(textChanged(const QString &)), this, SLOT(setLaserTopic(const QString &)));
-  connect(size,SIGNAL(valueChanged(int)),this,SLOT(setLaserSize(int)));
-
-  // 增加颜色变换
-  connect(comboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(LaserColorChanged(int)));
-
   // Next we initialize the main RViz classes.
   //
   // The VisualizationManager is the container for Display objects,
@@ -124,9 +291,23 @@ MyViz::MyViz( QWidget* parent)
   // 初始化camera 这行代码实现放大 缩小 平移等操作
   render_panel_->initialize( manager_->getSceneManager(), manager_ );
   // manager_->setFixedFrame("world");
+  // 初始化tool_manager_
+  tool_manager_ = manager_->getToolManager();
+
   manager_->initialize();
   manager_->startUpdate();
 
+  // 创建select panel
+  QLabel *select_label = new QLabel("Select Display:"); 
+  tree_widget_ = new rviz::PropertyTreeWidget();
+  tree_widget_->setModel(manager_->getSelectionManager()->getPropertyModel());
+
+  display_layout->addWidget(select_label, display_index++, 0);
+  display_layout->addWidget(tree_widget_, display_index++, 0);
+  
+  // 设置网格某一行伸缩比例,放最后一行
+  // controls_layout->setRowStretch(--ctl_index, 6);
+
   // Create a Grid display.
   grid_ = manager_->createDisplay( "rviz/Grid", "adjustable grid", true );
   ROS_ASSERT( grid_ != NULL );
@@ -134,13 +315,13 @@ MyViz::MyViz( QWidget* parent)
   // cloud = manager_->createDisplay( "rviz/PointCloud2", "point cloud", true );
   // ROS_ASSERT( cloud != NULL );
 
-  laser = manager_->createDisplay( "rviz/LaserScan", "Qlaser", true );
+  laser_ = manager_->createDisplay( "rviz/LaserScan", "Qlaser", true );
-  ROS_ASSERT( laser );
+  ROS_ASSERT( laser_ );
   // 新增雷达
-  laser->subProp("Topic")->setValue("/scan");
+  laser_->subProp("Topic")->setValue("/scan");
-  laser->subProp("Size (Pixels)")->setValue("2");
+  laser_->subProp("Size (Pixels)")->setValue("2");
-  laser->subProp("Color Transformer")->setValue("FlatColor");
+  laser_->subProp("Color Transformer")->setValue("FlatColor");
-  laser->subProp("Color")->setValue(comboBox->currentText());
+  // laser_->subProp("Color")->setValue(comboBox->currentText());
   // cloud->subProp("Topic")->setValue("/pose_graph/octree");
   // cloud->subProp("Style")->setValue("Points");
   // cloud->subProp("Size (Pixels)")->setValue("2");
@@ -151,9 +332,23 @@ MyViz::MyViz( QWidget* parent)
   grid_->subProp( "Line Style" )->setValue( "Billboards" );
   grid_->subProp( "Color" )->setValue( QColor( Qt::white ) );
 
-  // Initialize the slider values.
+  // 新增坐标系
-  thickness_slider->setValue( 5 );
+  axes_display= manager_->createDisplay("rviz/Axes", "Axes", false);
-  cell_size_slider->setValue( 6 );
+  ROS_ASSERT( axes_display != NULL );
+  axes_display->subProp("Length")->setValue(5); // 设置透明度
+  axes_display->subProp("Radius")->setValue(0.1); // 设置坐标系的缩放大小
+
+  // 新增Marker_ 显示圆圈
+  marker_ = manager_->createDisplay("rviz/Marker", "Marker", false);
+  ROS_ASSERT( marker_ != NULL );
+  marker_->subProp("Topic")->setValue("visualization_marker");
+
+  // Set the top-level layout for this MyViz widget.
+  main_layout->addLayout( controls_layout );
+  main_layout->addLayout( display_layout );
+  main_layout->addWidget( render_panel_,2); // 存放rviz的位置, stretch 拉伸系数
+  setLayout( main_layout );
+
 }
 
 void MyViz::Tree_Display(QGridLayout* controls_layout, int index){
@@ -163,11 +358,11 @@ void MyViz::Tree_Display(QGridLayout* controls_layout, int index){
   menu->header()->setSectionResizeMode(QHeaderView::ResizeToContents);
 
   menu->setHeaderLabels(QStringList()<<"key"<<"value");
+  menu->setHeaderHidden(true); 
   // menu->setColumnCount(2);
 
   // 设置不同层次菜单的缩进
   // menu->setIndentation(10);
-
   // global-一层树
   QTreeWidgetItem *global= new QTreeWidgetItem(menu, QStringList("全局变量"));
   menu->addTopLevelItem(global);
@@ -177,22 +372,18 @@ void MyViz::Tree_Display(QGridLayout* controls_layout, int index){
   QTreeWidgetItem *fixed_frame = new QTreeWidgetItem(global, QStringList("Fixed Frame"));
 
   QLineEdit* fixed_frame_text = new QLineEdit();
-  fixed_frame_text->setText("world");
+  fixed_frame_text->setText("laser");
   fixed_frame_text->setStyleSheet("background:transparent;border-width:0;border-style:inset");
   // 设置事件
   connect(fixed_frame_text,SIGNAL(textChanged(const QString&)),this,SLOT(setFixedFrame(const QString&)));
-
   // 将其他控件(非TreeWidgetItem)放入treeItem中
   menu->setItemWidget(fixed_frame, 1, fixed_frame_text); // 将container 放到 Item ,数字为列数
   
-
   QTreeWidgetItem *bg_color = new QTreeWidgetItem(global, QStringList("BackGround Color"));
   QLineEdit* bg_color_text = new QLineEdit();
   bg_color_text->setText("48;48;48");
   bg_color_text->setStyleSheet("border-width:0;border-style:outset");
-  // 设置事件
   connect(bg_color_text,SIGNAL(textChanged(const QString&)),this,SLOT(setBackgroundColor(const QString&)));
-
   menu->setItemWidget(bg_color, 1, bg_color_text); // 将container 放到 Item ,数字为列数
   
   // global->addChild(fixed_frame);
@@ -203,21 +394,194 @@ void MyViz::Tree_Display(QGridLayout* controls_layout, int index){
   QTreeWidgetItem *grid_color = new QTreeWidgetItem(grid, QStringList("Color"));
   QComboBox * grid_color_cbox = new QComboBox();
   for(int i =0 ; i < color_name.size(); ++i){
-      grid_color_cbox->addItem(color_name[i]);
+      QString color_path = color_icon_filename.arg(color_name[i]);
+      QIcon color_icon(color_path);
+      grid_color_cbox->addItem(color_icon, color_name[i]);
   }
   connect(grid_color_cbox, SIGNAL(currentIndexChanged(int)), this, SLOT(GridColorChanged(int)));
   menu->setItemWidget(grid_color, 1, grid_color_cbox);
 
-  QTreeWidgetItem *item3 = new QTreeWidgetItem(menu, QStringList("Laser"));
+  // grid cell size
+  QTreeWidgetItem *grid_cell = new QTreeWidgetItem(grid, QStringList("Cell Size"));
+  QSlider* cell_size_slider = new QSlider( Qt::Horizontal );
+  cell_size_slider->setMinimum( 1 );
+  cell_size_slider->setMaximum( 100 );
+  connect( cell_size_slider, SIGNAL( valueChanged( int )), this, SLOT( setCellSize( int )));
+  cell_size_slider->setValue( 1 );
+  menu->setItemWidget(grid_cell, 1, cell_size_slider);
+
+  // grid Line Thickness
+  QTreeWidgetItem *grid_thickness = new QTreeWidgetItem(grid, QStringList("Line Thickness"));
+  QSlider* thickness_slider = new QSlider( Qt::Horizontal );
+  thickness_slider->setMinimum( 1 );
+  thickness_slider->setMaximum( 100 );
+  connect( thickness_slider, SIGNAL( valueChanged( int )), this, SLOT( setThickness( int )));
+  thickness_slider->setValue( 5 );
+  menu->setItemWidget(grid_thickness, 1, thickness_slider);
+
+  // laser - 一层树
+  QTreeWidgetItem * laser = new QTreeWidgetItem(menu, QStringList("Laser"));
+  // 二层
+  QTreeWidgetItem *laser_topic = new QTreeWidgetItem(laser, QStringList("Topic "));
+  // Topic_Laser 输入雷达
+  QLineEdit* laser_topic_text = new QLineEdit();
+  // 增加雷达话题
+  connect(laser_topic_text, SIGNAL(textChanged(const QString &)), this, SLOT(setLaserTopic(const QString &)));
+  menu->setItemWidget(laser_topic, 1, laser_topic_text);
+
+  // Laser size
+  QTreeWidgetItem *laser_size = new QTreeWidgetItem(laser, QStringList("Size "));
+  QSpinBox* laser_size_text=new QSpinBox();
+  connect(laser_size_text,SIGNAL(valueChanged(int)),this,SLOT(setLaserSize(int)));
+  menu->setItemWidget(laser_size, 1, laser_size_text);
+
+  // Laser color
+  QTreeWidgetItem *laser_color = new QTreeWidgetItem(laser, QStringList("Color "));
+  QComboBox * comboBox = new QComboBox();
+  for(int i =0 ; i < color_name.size(); ++i){
+      QString color_path = color_icon_filename.arg(color_name[i]);
+      QIcon color_icon(color_path);
+      comboBox->addItem(color_icon, color_name[i]);
+  }
+  comboBox->setCurrentIndex(1);
+    // 增加颜色变换
+  connect(comboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(LaserColorChanged(int)));
+  menu->setItemWidget(laser_color, 1, comboBox);
+
 
   QTreeWidgetItem *item13 = new QTreeWidgetItem(global, QStringList("Frame rate"));
   
-  QTreeWidgetItem *item21 = new QTreeWidgetItem(grid, QStringList("Cell Size"));
   QTreeWidgetItem *item22 = new QTreeWidgetItem(grid, QStringList("Line Style"));
-  QTreeWidgetItem *item23 = new QTreeWidgetItem(grid, QStringList("Color"));
   controls_layout->addWidget(menu, index, 0 );
 }
 
+void MyViz::PlayCircle(int index){
+  int argc =0;
+  char** argv;
+  ros::init(argc, argv, "basic_shapes");
+  ros::NodeHandle n;
+  ros::Rate r(1);
+  ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);
+  uint32_t shape = visualization_msgs::Marker::LINE_STRIP;
+
+  if (ros::ok())
+    {
+      visualization_msgs::Marker marker;
+      // Set the frame ID and timestamp.  See the TF tutorials for information on these.
+      marker.header.frame_id = "laser";
+      marker.header.stamp = ros::Time::now();
+ 
+      // Set the namespace and id for this marker.  This serves to create a unique ID
+      // Any marker sent with the same namespace and id will overwrite the old one
+      marker.ns = "basic_shapes";
+      marker.id = 0;
+  
+      // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
+      marker.type = shape;
+ 
+      // Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
+      marker.action = visualization_msgs::Marker::ADD;
+ 
+     // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
+      marker.pose.position.x = 0;
+      marker.pose.position.y = 0;
+      marker.pose.position.z = 0;
+      marker.pose.orientation.x = 0.0;
+      marker.pose.orientation.y = 0.0;
+      marker.pose.orientation.z = 0.0;
+      marker.pose.orientation.w = 1.0;
+ 
+      // Set the scale of the marker -- 1x1x1 here means 1m on a side
+      // 设置线段的宽度
+      marker.scale.x = 0.02;
+      marker.scale.y = 1.0;
+      marker.scale.z = 1.0;
+ 
+      // Set the color -- be sure to set alpha to something non-zero!
+      marker.color.r = 0.0f;
+      marker.color.g = 1.0f;
+      marker.color.b = 0.0f;
+      marker.color.a = 1.0;
+      // points
+     // 绘制同心圆
+      // double r_c= std::stod(argv[1]); 
+      double radius = index;
+      int num_points = 360;
+      // Create the vertices for the points and lines 画圆
+      for (uint32_t i = 0; i < num_points; ++i)
+      {
+        // float y = sin(f + i / 100.0f * 2 * M_PI);
+       // float z = cos(f + i / 100.0f * 2 * M_PI);
+        double angle = 2 * M_PI * i / num_points;
+        geometry_msgs::Point point;
+        point.x = radius * cos(angle);
+        point.y = radius * sin(angle);
+        point.z = 0.0;
+        marker.points.push_back(point);
+      }
+      marker.lifetime = ros::Duration();
+ 
+      // Publish the marker
+      while (marker_pub.getNumSubscribers() < 1)
+      {
+        if (!ros::ok())
+        {
+          std::cout<< "publish return 0" << std::endl;
+        }
+        ROS_WARN_ONCE("Please create a subscriber to the marker");
+        // sleep(1);
+      }
+      marker_pub.publish(marker);
+ 
+      r.sleep();
+    }
+
+}
+
+void MyViz::CircleDisplayChanged(int index){
+  if(index == 0){
+    marker_->setValue(false); 
+  }else
+  {
+    manager_->setFixedFrame("laser");
+
+    marker_->setValue(true);
+    std::thread ThreadCircle(&MyViz::PlayCircle, this, index);// 新开线程执行操作
+    ThreadCircle.detach();// 执行完成后自动回收资源
+  }
+}
+
+void MyViz::AxesDisplayChanged(int index)
+{
+    switch (index)
+    {
+        case 0: // "yes" 选项被选择
+            if (axes_display != NULL)
+              axes_display->setValue(false);
+              break;
+
+        case 1: // "no" 选项被选择
+            if (axes_display != NULL)
+              axes_display->setValue(true);
+              break;
+    }
+}
+
+void MyViz::slot_move_camera(){
+  rviz::Tool* current_tool_= tool_manager_->getDefaultTool();
+  tool_manager_->setCurrentTool(current_tool_);
+  manager_->startUpdate();
+}
+
+
+void MyViz::slot_select(){
+  // 获得工具类
+  // tool_manager_ = manager_->getToolManager();
+  rviz::Tool* current_tool_ = tool_manager_->addTool("rviz/Select");
+  tool_manager_->setCurrentTool(current_tool_);
+  manager_->startUpdate();
+}
+
 void MyViz::setBackgroundColor(const QString& color_string){
   if(manager_!=NULL){
     const QString t = color_string;
@@ -238,7 +602,7 @@ void MyViz::setBackgroundColor(const QString& color_string){
 
 
 void MyViz::LaserColorChanged(int index){
-  if(laser!=NULL) laser->subProp("Color")->setValue(color_name[index]);
+  if(laser_!=NULL) laser_->subProp("Color")->setValue(color_name[index]);
   
 }
 
@@ -282,11 +646,11 @@ void MyViz::setCloudTopic(const QString &newTopic){
 }
 
 void MyViz::setLaserTopic(const QString &newTopic){
-  if(laser!=NULL){
+  if(laser_!=NULL){
-    laser->subProp( "Topic" )->setValue(newTopic);
+    laser_->subProp( "Topic" )->setValue(newTopic);
     ROS_INFO_STREAM("laser topic changed to => "<<newTopic.toStdString());
   }
-  laser->setEnabled(true);
+  laser_->setEnabled(true);
   // laser->subProp( "Color" )->setValue( QColor( Qt::yellow ) );
   manager_->setFixedFrame("laser");
   manager_->startUpdate();
@@ -304,8 +668,8 @@ void MyViz::setCloudSize(int cloudsize){
 }
 
 void MyViz::setLaserSize(int lasersize){
-  if(laser!=NULL){
+  if(laser_!=NULL){
-    laser->subProp("Size (Pixels)")->setValue(lasersize);
+    laser_->subProp("Size (Pixels)")->setValue(lasersize);
   }
 }
 
@@ -318,3 +682,324 @@ void MyViz::pubThread(){
     ROS_INFO_STREAM_ONCE("here is in publish process!");
   }
 }
+
+
+void MyViz::openFile()
+{
+  ros::Rate r(10.0);
+  rosbag::Bag bag,newBag;
+  std::string pcdFilePathStr = filePath.toStdString();
+  int frameEnd;
+  int frameStart; 
+  bool saveFlag = false; 
+  if(stored)
+  {
+  char** argv;// 字符串内容(输入)
+  int argc=0;// 字符串个数(输入)
+  countbag = 0;
+  ros::init(argc, argv, "laser_scan_publisher");// "laser"???????????重新启动一下roscore可以解决
+  // qDebug()<<"11111";
+  
+  ros::NodeHandle n;
+  scan_pub = n.advertise<sensor_msgs::LaserScan>("scan", 5);// "topic"
+  // sleep time
+
+  bag.open(pcdFilePathStr, rosbag::bagmode::Read);
+ 
+  rosbag::View view1(bag, rosbag::TopicQuery("/scan"));
+  view = &view1;
+  std::cout<<view->size()<<std::endl;
+
+  for (rosbag::View::iterator it = view->begin(); it != view->end(); ++it)// 用于预先存储bag包的信息
+  {
+    auto m = *it;
+    sensor_msgs::LaserScan::ConstPtr input = m.instantiate<sensor_msgs::LaserScan>();
+    indx.push_back(input);
+  }
+  stored = false;
+  indxsize = indx.size();
+  bag.close();
+  qDebug()<<indxsize;
+  std::cout << "stored finished" << std::endl;
+  }
+  qDebug()<<"stored 后";
+  while (isthreadplaying)// 用于播放存储的bag包的信息
+  {
+    if(threadout)
+    {
+      break;
+    }
+    qDebug()<<countbag;
+    double countbagvalue = countbag;
+    if(countbag>=indxsize)
+  {
+    scan_pub.publish(*indx[indxsize-1]);
+    end = true;// 停在当前位置
+    countbag = 0;// 从头开始
+    qDebug()<<"publish end";
+  }
+    // progressmove = false;
+    
+    if(is_rateChanged){
+      qDebug() << QString::number(speed_rate);
+      r = ros::Rate(speed_rate);
+      r.reset();
+      is_rateChanged = false;
+    }
+    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+    if(is_save){
+      if(newBag.isOpen())
+        newBag.close();
+      std::string temFileStr = pcdFilePathStr;
+      temFileStr.resize(pcdFilePathStr.length() - 4);
+      std::string saveFilePathStr =  temFileStr + "frame" \
+                                      + std::to_string(countbag) + ".bag";
+      std::cout << saveFilePathStr;
+      std::ifstream file(saveFilePathStr.c_str());
+      if (!file.good()) {
+        // std::ofstream outputFile(saveFilePathStr); //create a file
+        // outputFile.close();
+
+        QFile newFile(QString::fromStdString(saveFilePathStr));
+
+        newBag.open(saveFilePathStr, rosbag::bagmode::Write);
+        // for(int i=0;i<10;i++){
+        newBag.write("/scan", ros::Time::now(), indx[countbag]);
+        // }
+        newBag.close();
+      }else{
+        // file exist
+      }
+      
+      is_save = false;
+      r.sleep();
+      countbag++; 
+      continue;
+    }
+    if(is_saveMul || is_saveMulFinish){
+      if(ispauseclick && is_saveMul)
+      {
+        is_saveMul = false;
+        QMessageBox msgBox;
+        msgBox.setWindowTitle("Tips");
+        msgBox.setText("Pause First Please!");
+        msgBox.resize(2000, 1000);
+        msgBox.exec();
+        continue;
+      }
+    
+      if(is_saveMulFinish){
+        frameEnd = countbag;
+        std::string temFileStr = pcdFilePathStr;
+        temFileStr.resize(pcdFilePathStr.length() - 4);
+        std::string saveFilePathStr =  temFileStr + "frame" \
+                      + std::to_string(frameStart) + "to" + std::to_string(frameEnd) + ".bag";
+        std::cout << saveFilePathStr;
+        std::ifstream file(saveFilePathStr.c_str());
+        if (!file.good()) {
+   
+
+        newBag.open(saveFilePathStr, rosbag::bagmode::Write);
+   
+        
+        for(int i = frameStart; i < frameEnd; i++){
+    
+          newBag.write("/scan", ros::Time::now(), indx[i]);
+          
+        
+        }
+        is_saveMulFinish = false;
+        is_saveMul = false;
+        saveFlag = false;
+        newBag.close();
+        }else{
+          
+        }
+      }
+      else{
+        qDebug()<<countbag;
+        if(!saveFlag){
+          frameStart = countbag;
+          saveFlag = true;
+        }else{
+          ;
+        }
+      }
+      scan_pub.publish(*indx[countbag]);
+      progress->setSliderPosition(ceil(countbag/indxsize*100));
+      qDebug()<<countbag;
+      qDebug()<< "*************************"   <<frameStart;
+      r.sleep();
+      countbag ++;
+      //Reached the end of the file, but did not stop the multiple frame saving operation
+      if(countbag == indxsize){
+        // newBag.close();
+        frameEnd = indxsize;
+        is_saveMulFinish = true;
+
+        countbag = 0;
+        
+        // ispauseclick = false;
+      }
+        
+      continue;}
+    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+    if(ismoved)// 手拖动时改变进度条
+    {
+      end = false;
+      int value = (progress->value()/100)*indxsize;
+      double valuedouble = progress->value();
+      countbag = round((valuedouble/100)*indxsize);
+      if(countbag >= indxsize)// 直接重新开始for循环
+      {
+        countbag = indxsize-1;
+        progress->setSliderPosition(100);
+        ispauseclick = false;
+      }
+      ismoved = false;
+    }
+    else if(ischanged)// 手戳动时改变进度条
+    {
+      end = false;
+      int value = (progress->value()/100)*indxsize;
+      double valuedouble = progress->value();
+      countbag = round((valuedouble/100)*indxsize);
+      if(countbag >= indxsize)// 直接重新开始for循环
+      {
+        countbag = indxsize-1;
+        progress->setSliderPosition(100);
+        ispauseclick = false;
+      }
+      scan_pub.publish(*indx[countbag]);
+      countbag++;
+      ischanged = false;
+      r.sleep();
+      continue;
+      // ismoved = false;
+    }
+    if(!ispauseclick || end)
+      {
+        qDebug()<<"pause";
+        if(countbag<indxsize){
+        scan_pub.publish(*indx[countbag]);}
+        r.sleep();
+        qDebug() << sliderreleased <<end <<is_saveMulFinish;
+        // qDebug()<<"pause";
+        continue;
+      }
+      
+    else if(!end)
+      {
+        if(countbag<indxsize)
+        {
+          if(countbag == 0){qDebug()<<"0 again!";}
+          scan_pub.publish(*indx[countbag]);}
+        
+        qDebug()<<indxsize;
+        if(progress->sliderPosition()<=100 && countbagvalue/indxsize*100<=100)
+        {
+          qDebug()<<"pause";
+          progress->setSliderPosition(ceil(countbagvalue/indxsize*100));
+          qDebug()<<speed_rate;
+        }
+        countbag++;
+        ischanged = false;
+        r.sleep();
+      }
+    }
+  qDebug()<<"------end-------";
+}
+
+
+double MyViz::extractDuration(const QByteArray& output) {
+    QString outputString = QString::fromLocal8Bit(output);
+    QString delimiter = "duration:";
+    outputString = outputString.section(delimiter, 1, 1);
+    delimiter ="\n";
+    outputString = outputString.section(delimiter, 0,0);
+    outputString.remove(" ");
+    outputString.remove("s") ;
+    double number = outputString.toDouble();
+
+    return number;
+}
+
+
+void MyViz::selectFile(){
+  threadout = true;
+  if(threadout)// 清空播放容器
+  {
+    stored = true;
+    indx.clear();
+  }
+  progress->setSliderPosition(0);
+  isthreadplaying = false;
+  playagain = false;
+  filePath = QFileDialog::getOpenFileName(nullptr, "select file", "/home", "bag(*.bag);;所有文件 (*)");
+  qDebug()<< filePath;
+  if(filePath.isEmpty()){
+    QMessageBox msgBox;
+    msgBox.setWindowTitle("Tips");
+    msgBox.setText("No file is selected");
+    msgBox.resize(2000, 1000);
+    // 设置样式表，包括透明度设置
+    msgBox.setStyleSheet("QMessageBox {"
+                         "background-color: rgba(255, 255, 255, 0.9);" // 设置背景颜色和透明度
+                         "border: 2px solid rgba(0, 0, 0, 0.9);"     // 设置边框颜色和透明度
+                         "}"
+                         "QLabel {"
+                         "color: rgba(0, 0, 0, 0.9);"                 // 设置文本颜色和透明度
+                         "font-size: 16px;"
+                         "}");
+    QTimer selectTimer;
+    selectTimer.setSingleShot(true); // 设置定时器只触发一次
+    selectTimer.start(2000);         // 3秒后触发定时器，单位是毫秒
+
+    //连接定时器的超时信号到关闭消息框的槽函数
+    QObject::connect(&selectTimer, &QTimer::timeout, [&]() {
+        msgBox.done(QMessageBox::Ok); // 关闭消息框
+    });
+    msgBox.exec();
+  }else{
+    
+    QMessageBox msgBox;
+    msgBox.setWindowTitle("Tips");
+    msgBox.setText("File selected");
+    msgBox.resize(2000, 1000);
+    // 设置样式表，包括透明度设置
+    msgBox.setStyleSheet("QMessageBox {"
+                         "background-color: rgba(255, 255, 255, 0.9);" // 设置背景颜色和透明度
+                         "border: 2px solid rgba(0, 0, 0, 0.9);"     // 设置边框颜色和透明度
+                         "}"
+                         "QLabel {"
+                         "color: rgba(0, 0, 0, 0.9);"                 // 设置文本颜色和透明度
+                         "font-size: 16px;"
+                         "}");
+    QTimer selectTimer;
+    selectTimer.setSingleShot(true); // 设置定时器只触发一次
+    selectTimer.start(2000);         // 3秒后触发定时器，单位是毫秒
+
+    //连接定时器的超时信号到关闭消息框的槽函数
+    QObject::connect(&selectTimer, &QTimer::timeout, [&]() {
+        msgBox.done(QMessageBox::Ok); // 关闭消息框
+    });
+    msgBox.exec();
+    isthreadplaying = true;
+  }
+}
+
+
+void MyViz::SpeedChanged(int index){
+  // if(isthreadplaying)
+  bool ok;
+  double number = speed_name[index].toDouble(&ok);
+  if(ok){
+    speed_rate = 10.0 * number;
+    qDebug() << "*************func test" << QString::number(speed_rate);
+    is_rateChanged = true;
+  }
+    
+  else{
+    qDebug() << "failed";
+  }
+}

librviz_tutorial/src/myviz.h

@@ -29,17 +29,31 @@
 #ifndef MYVIZ_H
 #define MYVIZ_H
 
+
 #include <QWidget>
 #include<QString>
 #include <QGridLayout>
 #include<ros/ros.h>
 #include"std_msgs/String.h"
 #include<thread>
+#include <visualization_msgs/Marker.h>
+
+#include<QTimer>
+#include <QMutex>
+#include <QProcess>
+#include <QThread>
+#include<QSlider>
+#include <rosbag/bag.h>
+#include <sensor_msgs/LaserScan.h>
+#include <rosbag/view.h>
+
 namespace rviz
 {
 class Display;
 class RenderPanel;
 class VisualizationManager;
+class ToolManager;
+class PropertyTreeWidget;
 }
 
 // BEGIN_TUTORIAL
@@ -52,6 +66,8 @@ public:
   virtual ~MyViz();
   void subCallback(const std_msgs::String& msg);
   void pubThread();
+  void PlayCircle(int index);  // 圆圈线程执行函数
+  rosbag::View *view;
 private Q_SLOTS:   // QT信号和槽
   void setThickness( int thickness_percent );
   void setCellSize( int cell_size_percent );
@@ -64,18 +80,71 @@ private Q_SLOTS:   // QT信号和槽
   void GridColorChanged(int index); // 格栅颜色
   void setFixedFrame(const QString &FixedFrame);
   void setBackgroundColor(const QString& bg_color); // 背景颜色
+  void AxesDisplayChanged(int index); // axe
+  void CircleDisplayChanged(int index); // circle
   void Tree_Display(QGridLayout* layout, int index);
+  
+  void slot_select(); // 切换至select模式
+  void slot_move_camera();
+
+  // off
+  double extractDuration(const QByteArray& output);
+  void selectFile();
+  void SpeedChanged(int index);
+  void openFile();
 private:
   rviz::VisualizationManager* manager_; // 
   rviz::RenderPanel* render_panel_;
+  rviz::ToolManager* tool_manager_;
   rviz::Display* grid_;
   rviz::Display* cloud;
-  rviz::Display* laser;
+  rviz::Display* laser_;
+  rviz::Display* axes_display;
+  rviz::Display* marker_;
+  rviz::PropertyTreeWidget* tree_widget_ ;
   ros::NodeHandle nh;
   ros::Subscriber sub;
   ros::Publisher pub;
   std::thread pub_thread;
   std::vector<QString> color_name; // 颜色的名称集合
+  QString color_icon_filename;
+  // off
+  bool isclicked;// 用于判断play第几次按下play键
+  bool ispauseclick;// 用于判断暂停键第几次按下
+  int count;// 用于记录timeout的次数
+  bool sliderreleased;// 用于判断是否拖动了进度条
+  bool playagain;// 用于检测是否多次按下play按钮
+  bool isthreadplaying;// 用于标识线程是否在运行
+  bool abs;// 用于单独开线程？
+  bool stored;// 用于存储一次容器,只在第一次play的时候存储容器
+  bool is_pause = false;
+  bool is_reset = false;
+  bool is_end = false;
+  bool is_save = false;
+  bool is_saveMul = false;
+  bool is_bagClosed = true;
+  bool is_rateChanged = false;
+  bool is_openFile = false;
+  bool is_setFrameIndex = false;
+  bool is_saveMulFinish = false;
+  bool end = false;
+  bool threadout = false;
+  bool ischanged = false;
+  bool ismoved = false;
+  double speed_rate;
+  QSlider* progress;
+  QString filePath;
+  double duration;
+  int remain;
+  int prog_num = 0;
+  std::vector<QString> speed_name;
+  ros::Publisher scan_pub;
+  std::vector<sensor_msgs::LaserScan::ConstPtr> indx;// 存储节点信息
+  int countbag;// 存储播放的具体位置
+  double indxsize;
+  int test = 0;
+
+
 };
 // END_TUTORIAL
 #endif // MYVIZ_H